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

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

Revision: 14237
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14237&view=rev
Author:   hsujohnhsu
Date:     2009-04-22 19:26:06 +0000 (Wed, 22 Apr 2009)

Log Message:
-----------
fix on timeout catch for TF transforms.  appears to be working in sim now.

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-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-22 19:26:06 UTC (rev 14237)
@@ -146,7 +146,7 @@
     }
     catch(tf::LookupException& ex) {
       ROS_ERROR("No Transform available Error: %s\n", ex.what());
-      return; // kind of pointless unless there's more code added below this try catch block
+      return;
     }
     catch(tf::ConnectivityException& ex) {
       ROS_ERROR("Connectivity 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-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-22 19:26:06 UTC (rev 14237)
@@ -124,9 +124,18 @@
     }
     catch(tf::ExtrapolationException& ex) {
       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-      if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
-        return false;
+      return false;
     }
+    // check global pose timeout
+    if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+      ROS_ERROR("Transform timeout. global psoe stamp: %f current time: %f",global_pose.stamp_.toSec(),current_time.toSec());
+      return false;
+    }
+    // check goal pose timeout
+    if (current_time.toSec() - goal_pose.stamp_.toSec() > transform_tolerance_) {
+      ROS_ERROR("Transform timeout. goal psoe stamp: %f current time: %f",goal_pose.stamp_.toSec(),current_time.toSec());
+      return false;
+    }
 
     double wx = global_pose.getOrigin().x();
     double wy = global_pose.getOrigin().y();

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-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-04-22 19:26:06 UTC (rev 14237)
@@ -354,9 +354,16 @@
     }
     catch(tf::ExtrapolationException& ex) {
       ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-      if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
-        return;
+      return;
     }
+    // check global_pose timeout
+    if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+      ROS_ERROR("Transform timeout.");
+        ROS_ERROR("   current time    : %f",current_time.toSec());
+        ROS_ERROR("   globalpose stamp: %f",global_pose.stamp_.toSec());
+        ROS_ERROR("   tolerance       : %f",transform_tolerance_);
+      return;
+    }
 
     double wx = global_pose.getOrigin().x();
     double wy = global_pose.getOrigin().y();
@@ -407,9 +414,16 @@
       }
       catch(tf::ExtrapolationException& ex) {
         ROS_ERROR("Extrapolation Error: %s\n", ex.what());
-        if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
-          return;
+        return;
       }
+      // check global_pose timeout
+      if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+        ROS_ERROR("Transform timeout.");
+        ROS_ERROR("   current time    : %f",current_time.toSec());
+        ROS_ERROR("   globalpose stamp: %f",global_pose.stamp_.toSec());
+        ROS_ERROR("   tolerance       : %f",transform_tolerance_);
+        return;
+      }
 
       double wx = global_pose.getOrigin().x();
       double wy = global_pose.getOrigin().y();


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