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

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

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

Log Message:
-----------
planner transform timeout tolerance take 2.  see ticket #1190.

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

Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 22:39:34 UTC (rev 14184)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 22:45:36 UTC (rev 14185)
@@ -183,8 +183,8 @@
     tf::Stamped<tf::Pose> robot_pose;
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
-
+    ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+    robot_pose.stamp_ = ros::Time();
     try{
       tf_.transformPose(global_frame_, robot_pose, global_pose_);
     }
@@ -195,7 +195,8 @@
       ROS_ERROR("Connectivity Error: %s\n", ex.what());
     }
     catch(tf::ExtrapolationException& ex) {
-      ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+      if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
     }
   }
 

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 22:39:34 UTC (rev 14184)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 22:45:36 UTC (rev 14185)
@@ -85,7 +85,8 @@
     global_pose.setIdentity();
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+    ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+    robot_pose.stamp_ = ros::Time();
     try{
       //transform both the goal and pose of the robot to the global frame
       tf_.transformPose(global_frame_, robot_pose, global_pose);
@@ -100,8 +101,11 @@
       return false;
     }
     catch(tf::ExtrapolationException& ex) {
-      ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-      return false;
+      if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+      {
+        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+        return false;
+      }
     }
 
     double wx = global_pose.getOrigin().x();

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:39:34 UTC (rev 14184)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-04-21 22:45:36 UTC (rev 14185)
@@ -339,21 +339,20 @@
     global_pose.setIdentity();
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+    ros::Time current_time = ros::Time::now();
+    robot_pose.stamp_ = ros::Time();
     try{
       tf_.transformPose(global_frame_, robot_pose, global_pose);
     }
     catch(tf::LookupException& ex) {
       ROS_ERROR("No Transform available Error: %s\n", ex.what());
-      return;
     }
     catch(tf::ConnectivityException& ex) {
       ROS_ERROR("Connectivity Error: %s\n", ex.what());
-      return;
     }
     catch(tf::ExtrapolationException& ex) {
-      ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-      return;
+      if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
     }
 
     double wx = global_pose.getOrigin().x();
@@ -390,21 +389,20 @@
       global_pose.setIdentity();
       robot_pose.setIdentity();
       robot_pose.frame_id_ = robot_base_frame_;
-      robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+      ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+      robot_pose.stamp_ = ros::Time();
       try{
         tf_.transformPose(global_frame_, robot_pose, global_pose);
       }
       catch(tf::LookupException& ex) {
         ROS_ERROR("No Transform available Error: %s\n", ex.what());
-        return;
       }
       catch(tf::ConnectivityException& ex) {
         ROS_ERROR("Connectivity Error: %s\n", ex.what());
-        return;
       }
       catch(tf::ExtrapolationException& ex) {
-        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-        return;
+        if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+          ROS_ERROR("Extrapolation Error: %s\n", ex.what());
       }
 
       double wx = global_pose.getOrigin().x();


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