Multiple Turtlebots in Gazebo - how to solve conflicts in multiple cameras?

Question

Hello all!

I'm trying to introduce two turtlebots in gazebo for some of our experiments, and I followed instructions given in the answer to

answers.ros.org/question/41433/multiple-robots-simulation-and-navigation/

..and successfully spawned two turtlebots with the lasers working correctly (and independently) for both. Now, I want to get the cameras for both the turtlebots working, but have had no luck in that regard yet. I suspect the problem is that the separate tf_prefixes for the two robots are not getting appended to the camera topics of the two robots while publishing, and consequently the robots try to publish to the same topic and fail.

I have not been able to find a solution to this yet, and any help in this regard would be much appreciated!

My Current configuration is:

Ubuntu 11.10 + ROS Electric (Yes, i am stuck with Electric and cannot upgrade at the moment), Gazebo (simulator_gazebo) - version 1.4.15

Also, I get the following ERRORs when i try to roslaunch my launch file for gazebo node:

"Tried to advertise a service that is already advertised in this node [/camera/image_raw/compressed/set_parameters]"

"Tried to advertise a service that is already advertised in this node [/camera/image_raw/theora/set_parameters]"

Here are my launch files:

** create_multi_robot_world.launch **

<?xml version="1.0" ?>
<launch>
  <param name="/use_sim_time" value="true" />

  <!-- start world -->
  <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_exp)/worlds/Boyd_5th_floor_brown_doors.world" respawn="false" output="screen" />

  <!-- include our robots -->
  <include file="$(find turtlebot_exp)/launch/robots.launch"/>
</launch>

---

** robots.launch **

        <?xml version="1.0" ?>
    <launch>
      <!-- No namespace here as we will share this description. 
           Access with slash at the beginning -->
      <param name="robot_description"
        command="$(find xacro)/xacro.py $(find turtlebot_description)/urdf/turtlebot.urdf.xacro" />
    
      <!-- BEGIN ROBOT 1-->
      <group ns="robot1">
        <param name="tf_prefix" value="robot1_tf" />
        <include file="$(find turtlebot_exp)/launch/one_robot.launch" >
          <arg name="init_pose" value="-x 2.5 -y 0 -z 0" />
          <arg name="robot_name"  value="Robot1" />
        </include>
      </group>
    
      <!-- BEGIN ROBOT 2-->
    
      <group ns="robot2">
        <param name="tf_prefix" value="robot2_tf" />
        <include file="$(find turtlebot_exp)/launch/one_robot.launch" >
          <arg name="init_pose" value="-x -1.5 -y 0 -z 0" />
          <arg name="robot_name"  value="Robot2" />
        </include>
      </group>
    
    </launch>

---

** one_robot.launch **

<?xml version="1.0" ?>

<launch>
    <arg name="robot_name"/>
    <arg name="init_pose"/>

    <node name="spawn_minibot_model" pkg="gazebo" type="spawn_model" args="$(arg init_pose) -urdf -param /robot_description -model $(arg robot_name)" respawn="false" output="screen"/> <!-- launch-prefix="xterm -e ddd" /> -->


  <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
    <rosparam command="load" file="$(find turtlebot_bringup)/config/diagnostics.yaml" />
  </node>

    <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="30.0" />
    </node>


    <!-- The odometry estimator, throttling, fake laser etc. go here -->
    <!-- All the stuff as from usual robot launch file -->


  <!-- The odometry estimator -->

  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="publish_tf" value="true"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="false"/>
    <param name="vo_used" value="false"/>
    <param name="output_frame" value="odom"/> <!-- This line is added by @KPM to enable running of amcl in simulator -->
  </node>


  <!-- throttling -->
  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
    <param name="max_rate" value="20.0"/>
    <remap from="cloud_in" to="camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
  </node>

  <!-- Fake Laser -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.15"/>
    <param name="max_height" value="0.15"/>
    <remap from="cloud" to="cloud_throttled"/>
  </node>

  <!-- Fake Laser (narrow one, for localization -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.025"/>
    <param name="max_height" value="0.025"/>
    <remap from="cloud" to="cloud_throttled"/>
    <remap from="scan" to="narrow_scan"/>
  </node>

</launch>

---

Originally posted by kedarm on ROS Answers with karma: 86 on 2013-06-22

Post score: 1

---

Original comments

Comment by Martin Günther on 2013-06-23:
The "tried to advertise a service" warnings can be ignored, they always pop up (I think). Don't confuse tf_prefix with remapping, you need to get both right. If remapping works, rostopic list should show sth like /robot1/camera/... and not just /camera/....

Comment by kedarm on 2013-06-30:
I thought so too initially, but i still don't see any camera feed from either of the two turtlebots. Also, these are not merely warnings, they pop up as "ERRORS". I tried to see if /robot_1/camera/.. gets published, but its not. And all i see is /camera/...

Answer 1

Do you use the $(arg robot_name) also for your camera nodes? Then the camera topics should look like robot_name/image_raw/set_parameters

---

Originally posted by davinci with karma: 2573 on 2013-06-23

This answer was NOT ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by kedarm on 2013-06-30:
Yes, I am using the $(arg robot_name) ....please check my launch files in the new edit. Am i doing something wrong here?

Comment by davinci on 2013-06-30:
you should use the parameter in tags like ,

Comment by kedarm on 2013-07-07:
Ok...i tried this, and its still failing. I believe now that the problem is not in the launch file but somewhere within simulator_gazebo when the /camera/image_raw topic is attempted to be published. It might be related to this bug: https://code.ros.org/trac/ros-pkg/ticket/5511

Answer 2

OK, I have finally been able to find a solution to this problem. The problem here was two-fold.

1. The /camera/image_raw topic does not get resolved to the correct TF prefix when getting published
2. The correct frame ID does not get published with the image topic when there are multiple robots with the same camera.

Solution to problem 1:

In the turtlebot_description package, inside the "urdf" folder, there is a file named "gazebo.urdf.xacro". Modify the "turtlebot_sim_kinect" macro in that file and remove the leading "/" from the imageTopicName element. (I also removed it from other topic names since having absolute topic names here is a bad practice to start with). Now, the final "turtlebot_sim_kinect" xacro should look like:

<xacro:macro name="turtlebot_sim_kinect">
  <gazebo reference="camera_link">
    <sensor:camera name="camera">
      <imageFormat>R8G8B8</imageFormat>
      <imageSize>640 480</imageSize>
      <hfov>60</hfov>
      <nearClip>0.05</nearClip>
      <farClip>3</farClip>
      <updateRate>20</updateRate>
      <baseline>0.1</baseline>
      <controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <imageTopicName>camera/image_raw</imageTopicName>
        <pointCloudTopicName>camera/depth/points</pointCloudTopicName>
        <cameraInfoTopicName>camera/camera_info</cameraInfoTopicName>
        <frameName>camera_depth_optical_frame</frameName>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
      </controller:gazebo_ros_openni_kinect>
    </sensor:camera>
  </gazebo>
</xacro:macro>

...save the file and exit. Now, the /camera/image_raw topic should resolve correctly to the correct robot prefix. Namely... "/robot1/camera/image_raw" and "/robot2/camera/image_raw" in my case. Note: I believe recompilation is unnecessary since we only modified the xacro. Although, I did do a recompile without realizing this at first. Even if you DO do a recompile, it shouldn't do any harm.

Solution to problem 2:

The camera topics don't carry the correct frame ID for the images they publish. Which means that although the camera topics are separate, the frame IDs do not get resolved correctly to the appropriate TF prefix. Hence, someone who wants to use the two independent camera images cannot do so.

This is related to a similar bug in the laser sensor that was fixed here:

code.ros.org/trac/ros-pkg/ticket/5511

Using the same principle that was applied in the laser sensor bug fix, I applied the solution to the camera sensor as follows...

Go to the "gazebo_ros_openni_kinect.cpp" file in /src of the "gazebo_plugins" package. (This is the source for the camera controller specified in "gazebo.urdf.xacro" file earlier)

Add the following to the code:

1.Add this at the top with other includes-

#include "tf/tf.h"

2.Add the following lines inside the "GazeboRosOpenniKinect::LoadChild" function:

std::string prefix;
this->rosnode_->getParam(std::string("tf_prefix"), prefix);
this->frameName = tf::resolve(prefix, this->frameName);

NOTE: add the above lines before any of the topics is advertised! Ideally, just before the line-

this->image_pub_ = this->itnode_->advertise(

(This makes sure the TF prefix is resolved before any topic is advertised)

Finally... save, exit, and recompile the gazebo_plugins package.

Voila!!! The multiple cameras should be up and running!

---

Originally posted by kedarm with karma: 86 on 2013-10-03

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by jorge on 2013-10-03:
Which version of turtlebot_decription are you using? the leading "/" problem is fixed on hydro, but your fix must be applied to previous versions.

Comment by bit-pirate on 2013-10-03:
Please also fill tickets on GitHub for bugs you find. Also code.ros.org is not used any more and most packages have been migrated, but not all tickets.

Comment by kedarm on 2013-10-03:
@jorge: Yeah...i am stuck with Electric, hence the leading "/" problem. It did occur to me that the problem might be fixed with newer versions. Anyways, I'll try bit-pirate's suggestion and file a ticket (and a fix if i can) on GitHub.

@bit-pirate: I'll definitely file a ticket on GitHub. I gave the link to that code.ros.org ticket because that was where I found it. Thanks a bunch for the suggestion though! :-)