[Personalrobots-commit] SF.net SVN: personalrobots:[23878] pkg/trunk/stacks/navigation/costmap_2d/ src

[<ei...@us...>]

Revision: 23878
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23878&view=rev
Author:   eitanme
Date:     2009-09-04 21:44:40 +0000 (Fri, 04 Sep 2009)

Log Message:
-----------
Updating for new version of ros::Rate

Modified Paths:
--------------
    pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
    pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp

Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp	2009-09-04 21:35:19 UTC (rev 23877)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp	2009-09-04 21:44:40 UTC (rev 23878)
@@ -68,8 +68,11 @@
         new_data_ = false;
         publishCostmap();
       }
-      if(!r.sleep())
-        ROS_WARN("Map publishing loop missed its desired rate of %.4f the actual time the loop took was %.4f sec", frequency, r.cycleTime().toSec());
+
+      r.sleep();
+      //make sure to sleep for the remainder of our cycle time
+      if(r.cycleTime() > ros::Duration(1 / frequency))
+        ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, r.cycleTime().toSec());
     }
   }
 

Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp	2009-09-04 21:35:19 UTC (rev 23877)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp	2009-09-04 21:44:40 UTC (rev 23878)
@@ -444,8 +444,11 @@
       end_t = end.tv_sec + double(end.tv_usec) / 1e6;
       t_diff = end_t - start_t;
       ROS_DEBUG("Map update time: %.9f", t_diff);
-      if(!r.sleep())
-        ROS_WARN("Map update loop missed its desired rate of %.4f the actual time the loop took was %.4f sec", frequency, r.cycleTime().toSec());
+
+      r.sleep();
+      //make sure to sleep for the remainder of our cycle time
+      if(r.cycleTime() > ros::Duration(1 / frequency))
+        ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, r.cycleTime().toSec());
     }
   }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.