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

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

Revision: 14385
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14385&view=rev
Author:   eitanme
Date:     2009-04-24 01:46:02 +0000 (Fri, 24 Apr 2009)

Log Message:
-----------
Make sure transforms are available before starting any navstack component. Also, use sim time in simulator launch files

Modified Paths:
--------------
    pkg/trunk/highlevel/test_nav/move_base/move_base.launch
    pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
    pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch
    pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
    pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
    pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp

Modified: pkg/trunk/highlevel/test_nav/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base.launch	2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base/move_base.launch	2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
 <launch>
   <master auto="start"/>
   <group name="wg">
+  <param name="/use_sim_time" value="true"/>
     <node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
       <remap from="/move_base_node/activate" to="/goal" />
       <remap from="~base_local_planner/global_plan" to="/gui_path" />

Modified: pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch	2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch	2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
 <launch>
   <master auto="start"/>
   <group name="wg">
+  <param name="/use_sim_time" value="true"/>
     <node pkg="nav" type="move_base_local" respawn="false" name="local_controller_node" output="screen">
       <remap from="/local_controller_node/activate" to="/goal" />
       <remap from="~base_local_planner/global_plan" to="/gui_path" />

Modified: pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch	2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch	2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
 <launch>
   <master auto="start"/>
   <group name="wg">
+  <param name="/use_sim_time" value="true"/>
     <node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
       <remap from="/move_base_node/activate" to="/goal" />
       <remap from="~base_local_planner/global_plan" to="/gui_path" />

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-24 01:46:02 UTC (rev 14385)
@@ -46,6 +46,11 @@
     ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("map"));
     ros_node_.param("~/navfn/costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
     ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.2);
+    
+    //we need to make sure that the transform between the robot base frame and the global frame is available
+    while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+      ROS_WARN("Waiting on transform from %s to %s to become available before running the planner", robot_base_frame_.c_str(), global_frame_.c_str());
+    }
 
     //we'll get the parameters for the robot radius from the costmap we're associated with
     ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius_, 0.325);

Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-04-24 01:46:02 UTC (rev 14385)
@@ -67,6 +67,11 @@
     ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("map"));
     ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
 
+    //we need to make sure that the transform between the robot base frame and the global frame is available
+    while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+      ROS_WARN("Waiting on transform from %s to %s to become available before running the controller", robot_base_frame_.c_str(), global_frame_.c_str());
+    }
+
     ros_node.param("~base_local_planner/yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
     ros_node.param("~base_local_planner/xy_goal_tolerance", xy_goal_tolerance_, 0.10);
 

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-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp	2009-04-24 01:46:02 UTC (rev 14385)
@@ -56,6 +56,11 @@
     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"));
 
+    //we need to make sure that the transform between the robot base frame and the global frame is available
+    while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+      ROS_ERROR("Waiting on transform from %s to %s to become available before running costmap", robot_base_frame_.c_str(), global_frame_.c_str());
+    }
+
     ros_node_.param("~" + prefix + "/costmap/transform_tolerance", transform_tolerance_, 0.2);
 
     //now we need to split the topics based on whitespace which we can use a stringstream for


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