tf_prefix in fake_localization and move_base

edit

I've been examining a strange issue with multiple robots in stage and move_base. Move_base and costmap2D use tf::resolve with tf_prefix to resolve global frames. However fake_localization does not - this leads to the message:

Waiting on transform from robot_0/base_link to robot_0/map to become available before running costmap, tf error:

A few people have had trouble with this. I have examined fake_localization and I believe the following lines (as done in costmap_2d_ros.cpp) need to be added:

// get our tf prefix
ros::NodeHandle prefix_nh;
std::string tf_prefix = tf::getPrefixParam(prefix_nh);
global_frame_id_ = tf::resolve(tf_prefix, global_frame_id_);
base_frame_id_ = tf::resolve(tf_prefix, base_frame_id_);
odom_frame_id_ = tf::resolve(tf_prefix, odom_frame_id_);

Furthermore I had another problem in move_base. tf_prefix is resolved too many times in the Costmap2D and NavFN. Here is the exact situation in navfn_ros.cpp::makePlan()

ros::NodeHandle n;
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
std::string global_frame = costmap_ros_->getGlobalFrameID();

//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)){
  ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
  return false;
}

if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)){
  ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
  return false;
}

What happens is that global_frame has already been resolved (removed preceding slash) inside the Costmap2D class. Now we resolve again, and since the slash is gone it appends the prefix. Thus we get /robot_0/robot_0/map I do not have a solution for this as I expect that this might be done on purpose?

Are there any thoughts on the problems presented here and a possible solution to the over resolving? Thanks!

Here's an example launch file for one robot of a multi robot stage sim. Note that under fake_localization I can adjust the global_frame_id via parameter. However, this is a hacky fix.

<launch>
  <!-- BEGIN ROBOT 0 -->
  <group ns="robot_0">
    <param name="tf_prefix" value="robot_0"></param>

    <arg default="$(find some_path)/launch/move_base_config/cost_map.yaml" name="map_file">
    <node args="$(arg map_file)" name="map_server" pkg="map_server" type="map_server">
        <param name="frame_id" value="map"></param>
    </node>

    <node name="fake_localization" output="screen" pkg="fake_localization" respawn="false" type="fake_localization">
        Uncomment here for success - without making above changes
      <!--param name="global_frame_id" value="/robot_0/map"/-->
       <param name="global_frame_id" value="map"></param> Need this to be in a relative frame
    </node>

    <node name="move_base_node" output="screen" pkg="move_base" respawn="false" type="move_base">
      <param name="controller_frequency" value="10.0"></param>
      <param name="recovery_behavior_enabled" value="true"></param>
      <param name="oscillation_timeout" value="5.0"></param>
      <param name="oscillation_distance" value="1.0"></param>
      <rosparam command="load" file="$(find some_path)/launch/move_base_config/costmap_common_params.yaml" ns="global_costmap">
      <rosparam command ...