ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org
Ask Your Question
0

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

asked 2013-06-22 17:33:11 -0500

kedarm gravatar image

updated 2013-06-30 20:13:42 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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/....

Martin Günther gravatar imageMartin Günther ( 2013-06-23 22:58:54 -0500 )edit

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/...

kedarm gravatar imagekedarm ( 2013-06-30 20:16:15 -0500 )edit

2 Answers

Sort by » oldest newest most voted
1

answered 2013-10-03 13:46:48 -0500

kedarm gravatar image

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!

edit flag offensive delete link more

Comments

Which version of turtlebot_decription are you using? the leading "/" problem is fixed on hydro, but your fix must be applied to previous versions.

jorge gravatar imagejorge ( 2013-10-03 14:15:03 -0500 )edit

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.

bit-pirate gravatar imagebit-pirate ( 2013-10-03 20:03:25 -0500 )edit

@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! :-)

kedarm gravatar imagekedarm ( 2013-10-03 21:38:02 -0500 )edit
1

answered 2013-06-23 02:19:37 -0500

davinci gravatar image

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

edit flag offensive delete link more

Comments

Yes, I am using the $(arg robot_name) ....please check my launch files in the new edit. Am i doing something wrong here?

kedarm gravatar imagekedarm ( 2013-06-30 20:20:44 -0500 )edit

you should use the parameter in tags like <remap from="cloud_in" to="camera/depth/points"/> , <remap from="cloud_in" to="$(arg robot_name)/depth/points"/>

davinci gravatar imagedavinci ( 2013-06-30 23:38:30 -0500 )edit

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

kedarm gravatar imagekedarm ( 2013-07-07 11:57:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2013-06-22 17:33:11 -0500

Seen: 2,424 times

Last updated: Oct 03 '13