[Personalrobots-commit] SF.net SVN: personalrobots:[14180] pkg/trunk

[<hsu...@us...>]

Revision: 14180
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14180&view=rev
Author:   hsujohnhsu
Date:     2009-04-21 22:09:06 +0000 (Tue, 21 Apr 2009)

Log Message:
-----------
update per ticket 1190, transform timeout for nav stack.

Modified Paths:
--------------
    pkg/trunk/highlevel/nav/include/nav/move_base_action.h
    pkg/trunk/highlevel/nav/src/move_base_action.cpp
    pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
    pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
    pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
    pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp

Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h	2009-04-21 22:09:06 UTC (rev 14180)
@@ -141,6 +141,7 @@
       tf::Stamped<tf::Pose> global_pose_;
       double inscribed_radius_, circumscribed_radius_, inflation_radius_;
       double controller_frequency_;
+      double transform_tolerance_; // timeout before transform errors
 
   };
 };

Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 22:09:06 UTC (rev 14180)
@@ -51,6 +51,7 @@
     ros_node_.param("~global_frame", global_frame_, std::string("map"));
     ros_node_.param("~robot_base_frame", robot_base_frame_, std::string("base_link"));
     ros_node_.param("~controller_frequency", controller_frequency_, 20.0);
+    ros_node_.param("~transform_tolerance", transform_tolerance_, 0.1);
 
     //for display purposes
     ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
@@ -182,7 +183,7 @@
     tf::Stamped<tf::Pose> robot_pose;
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time();
+    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
 
     try{
       tf_.transformPose(global_frame_, robot_pose, global_pose_);

Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-04-21 22:09:06 UTC (rev 14180)
@@ -87,6 +87,7 @@
       const costmap_2d::Costmap2D& cost_map_;
       NavFn planner_;
       std::string global_frame_, robot_base_frame_;
+      double transform_tolerance_; // timeout before transform errors
   };
 };
 

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 22:09:06 UTC (rev 14180)
@@ -42,6 +42,7 @@
     //read parameters for the planner
     ros_node_.param("~/navfn/global_frame", global_frame_, std::string("map"));
     ros_node_.param("~/navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
+    ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.1);
   }
 
   double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
@@ -84,7 +85,7 @@
     global_pose.setIdentity();
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time::now();
+    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
     try{
       //transform both the goal and pose of the robot to the global frame
       tf_.transformPose(global_frame_, robot_pose, global_pose);

Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h	2009-04-21 22:09:06 UTC (rev 14180)
@@ -209,6 +209,7 @@
       std::vector<ObservationBuffer*> clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
       bool rolling_window_; ///< @brief Whether or not the costmap should roll with the robot
       bool current_; ///< @brief Whether or not all the observation buffers are updating at the desired rate
+      double transform_tolerance_; // timeout before transform errors
 
   };
 };

Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-04-21 22:09:06 UTC (rev 14180)
@@ -56,6 +56,8 @@
     ros_node_.param("~" + prefix + "/costmap/global_frame", global_frame_, string("map"));
     ros_node_.param("~" + prefix + "/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
 
+    ros_node_.param("~" + prefix + "/costmap/transform_tolerance", transform_tolerance_, 0.1);
+
     //now we need to split the topics based on whitespace which we can use a stringstream for
     stringstream ss(topics_string);
 
@@ -337,7 +339,7 @@
     global_pose.setIdentity();
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time();
+    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
     try{
       tf_.transformPose(global_frame_, robot_pose, global_pose);
     }
@@ -388,7 +390,7 @@
       global_pose.setIdentity();
       robot_pose.setIdentity();
       robot_pose.frame_id_ = robot_base_frame_;
-      robot_pose.stamp_ = ros::Time();
+      robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
       try{
         tf_.transformPose(global_frame_, robot_pose, global_pose);
       }


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