kobuki_gazebo_plugins: request for tf prefix of base_footprint

[slivingston]

Is it possible to instantiate multiple Kobuki models so as to have a single tf tree organized by model names? Organization could be based on prefixes to frame IDs, as with tf_prefix in robot_state_publisher. Similarly, how can the odom topic be pushed into a private namespace?

I have not found a way to achieve these. I suspect that my difficulty is due to a bug in how gazebo_ros_kobuki.cpp handles naming of the topic odom and the frame base_footprint.

I am happy to try to fix it and open a pull request, but I want to first be sure this is indeed an issue. Below is a diff for gazebo_ros_kobuki.cpp (as of 9f509a502c8be4cc88dd93436cf7017c5ee124ff) that is a first step toward correction, though it would be better to provide a parameter regarding whether and what to use as a prefix for frame IDs.

--- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
+++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
@@ -195,7 +195,7 @@ void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
   odom_pose_[0] = 0.0;
   odom_pose_[1] = 0.0;
   odom_pose_[2] = 0.0;
-  odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
+  odom_pub_ = nh_priv_.advertise<nav_msgs::Odometry>("odom", 1);
   odom_reset_sub_ = nh_priv_.subscribe("commands/reset_odometry", 10, &GazeboRosKobuki::resetOdomCB, this);

   /*
@@ -395,7 +395,7 @@ void GazeboRosKobuki::OnUpdate()
    */
   odom_.header.stamp = joint_state_.header.stamp;
   odom_.header.frame_id = "odom";
-  odom_.child_frame_id = "base_footprint";
+  odom_.child_frame_id = node_name_+"/base_footprint";

   // Distance travelled by main wheels
   double d1, d2;

[stonier]

This is always tricky as it probably requires sweeping changes across the whole kobuki-turtlebot workspace. I've no clear idea of what it would entail across the whole workspace...so as I said, tricky and would require significant testing.

Indigo would be a better place to experiment making sweeping multi-kobuki tests as we haven't yet officially 'released' there yet and are less likely to disturb people with bugs.

[jihoonl]

tf_prefix should work after #34.

So I can configure like this https://github.com/robotics-in-concert/concert_services/blob/indigo/concert_service_gazebo/robots/kobuki/kobuki.launch#L13-L29

[slivingston]

I am trying the roslaunch example that you link to, which is at commit robotics-in-concert/concert_services@ae5a524edcb8f99f58f9dad26bc190766731cd64 at time of writing (so for posterity, your original URL would be https://github.com/robotics-in-concert/concert_services/blob/ae5a524edcb8f99f58f9dad26bc190766731cd64/concert_service_gazebo/robots/kobuki/kobuki.launch#L13-L29).

Upon trying the example, I found that the topic names are correctly placed in a namespace according to the tf_prefix parameter. However, the frame_id of messages on the odom topic of each robot are not reliably correct unless the <param> elements of the launch file are moved to within <node pkg="gazebo_ros" ...>. Unless this is done, the parameter tf_prefix is at the root of the parameter namespace, so each inclusion of the <launch> code (as in the example) rewrites the value of tf_prefix.

My explanation of this behavior is as follows. gazebo_ros_utils, which is now used in the Kobuki Gazebo plugin, assigns to tf_prefix_ using tf::getPrefixParam. This tf function uses the roscpp NodeHandle functions to get the parameter value. The documentation for NodeHandle::searchParam indicates that the parameter tree is crawled up until a match is found. Thus the problem can be solved by making tf_prefix be a parameter within the namespace of the node handle for each plugin instantiation,

<node pkg="gazebo_ros" ...>
   <param name="tf_prefix" value="$(arg name)"/>
</node>

[slivingston]

I just tried the gazebo_concert example in the rocon_tutorials repository and found that the error I describe above does not occur. My previous comment is motivated by an example that does not involve rocon. I am now investigating why the behavior is different.