Ask Your Question
26

Multiple robots simulation and navigation

asked 2012-08-14 22:20:00 -0500

Jakub gravatar image

How to launch multiple robots in gazebo simulator and how to setup navigation stacks for such a simulation?

There were already some questions and answers for the subject, like: here, and here, and here. This is my question and answer to summarize the discussion in one place.

edit retag flag offensive close merge delete

Comments

Hi @Jacub, thanks for this extraordinary contributions on forum. There is a similar problem (extended version) which can probably be solved only by you (given your expertise).( http://robotics.stackexchange.com/que... )

PrasadNR gravatar imagePrasadNR ( 2016-11-02 14:06:16 -0500 )edit

Hi @Jacub,thank you for give us a so wonderful tutorial about Multiple robots simulation and navigation, but I still have some trouble about this. I could do single-robot navigation in gazebo, but when i add the the namespace in the launch file,there may be a error in the amcl.

  • List item
huang_y gravatar imagehuang_y ( 2017-05-16 22:33:38 -0500 )edit

@PrasadNR and @huang_y you should make your own questions because, although related, they appear to be different than this question.

jayess gravatar imagejayess ( 2017-07-26 14:32:51 -0500 )edit

4 Answers

Sort by ยป oldest newest most voted
36

answered 2012-08-14 22:33:45 -0500

Jakub gravatar image

updated 2012-08-15 00:53:40 -0500

The answer is based on the turtlebot robot, but of course it can be used with any robot(s).

For multiple robots the key point is to manage the namespaces and tf_prefixes. Although usually we will set them to the same value for each robot in this example I will you different for purpose.

In setting up the navigation stack(s) each robot must operate in it's own namespace, but they do have to share the map (and usually robot_description). So the effect we want to achieve is:

                                  /map
       /robot1/robot_state_publisher    /robot2/robot_state_publisher
       /robot1/robot_pose_ekf           /robot2/robot_pose_ekf
       /robot1/amcl                     /robot2/amcl
       /robot1/move_base                /robot2/move_base

Simulation

First of all we have to launch the simulation, in our case gazebo. To make things more readable I split launch files in few files.

Launch file for one robot. We name it one_robot.launch:

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

    <node pkg="robot_state_publisher" type="state_publisher" 
          name="robot_state_publisher" output="screen"/>

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

Two things to mention for above launch file:

  • create two arguments to pass for robot initialization: name and position
  • notice that -param /robot_description is with the slash i.e. fully-qualified, as it will be shared.

Having the robot declaration we can now play with the namespaces. Your main launch file for multiple robots should look like (robots.launch):

<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 your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x 1 -y 1 -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 your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x -1 -y 1 -z 0" />
      <arg name="robot_name"  value="Robot2" />
    </include>
  </group>
</launch>

Last thing of course is to include it in your simulation launch file:

<launch>
  <param name="/use_sim_time" value="true" />

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

  <!-- start gui -->
  <node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen"/>

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

Navigation

For setting up the navigation stack we follow the tutorial RobotSetup, but we have to add the namespace tricks for it.

We use the same approach of splitting files. First the move_base.launch for one robot, as from tutorial:

<launch>    
  <!--- Run AMCL -->
  <include file="$(find amcl)/examples/amcl_diff.launch" />

  <!-- Define your move_base node -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find your_pkg)/costmap_common_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find your_pkg)/costmap_common_params.yaml" command ...
(more)
edit flag offensive delete link more

Comments

Wonderful! exactly what I was looking for my simulation, but if I want to move the different joints of my robots, which topic should I use? because I run "rostopic echo /robot1/joint_states" I got "WARNING: no messages received and simulated time is active.Is /clock being published?" Thanks a lot

Brioche gravatar imageBrioche ( 2012-08-20 22:50:49 -0500 )edit

This tutorial is base on TurtleBot, for which joint_state_publisher is included in gazebo_ros_create pluging (see the Hacking section). For your robot of course you have add joint_state_publisher in one_robot.launch (definition of your robot).

Jakub gravatar imageJakub ( 2012-08-22 18:32:55 -0500 )edit

Thank you, @Jakub, for sharing this! :)

harsha gravatar imageharsha ( 2013-01-10 05:25:25 -0500 )edit

can this b useful in global mapping and global localization for multipl robots?

Xittx gravatar imageXittx ( 2013-02-22 05:32:06 -0500 )edit

Very Cool, but I'm trying with ROS Hydro and I cannot achive to have separete nodes for camera plugins. What should I do? Thks

Joao Luis gravatar imageJoao Luis ( 2014-08-12 13:26:46 -0500 )edit

Now if only there was something like this for use in a real environment instead of Gazebo.

AmateurHour gravatar imageAmateurHour ( 2018-07-17 15:45:33 -0500 )edit

Thanks for your explanation. I'm going to refer to it for my project of using multiple robot arms with MoveIt! (my question on that). Question: what do I do after hacking the cpp file? Rebuild?

raequin gravatar imageraequin ( 2018-07-27 09:21:54 -0500 )edit
0

answered 2015-09-16 23:28:41 -0500

Usman Arif gravatar image

My navigation part asks for robot1/odom to map transform. I provide it through

<node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 robot1/odom map 100"/>

but when i try and do it for my 2nd robot i.e. robot2/odom to map one of the two transforms stay and the other one crashes. So I only get either robot1/odom to map or robot2/odom to map transform at a time, and depending upon that only one navigation works the other one doesn't.

A similar problem is discussed in the answers but those solutions aren't working for me. Need help, stuck for days :)

edit flag offensive delete link more

Comments

you need to do the opposite transform. map to robot1/odom and map to robot2/odom. you can have only one parent but many children

MasterTsoutsos gravatar imageMasterTsoutsos ( 2019-02-09 08:13:21 -0500 )edit
0

answered 2012-08-15 02:03:56 -0500

maurizio gravatar image

updated 2012-08-15 07:29:40 -0500

WRT your notes, I got stuck since I didn't get neither /Robot1/joint_states nor /Robot2/joint_states, I had to add the option -unpause like the following:

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

I am almost done, but I have a problem with the map, following your notes I have. I guess there is still a problem with namespace, in particular It asks for /base/link but it shouldn't,

[ WARN] [1345042406.057976802, 32.122000000]: You have set map parameters, but also requested to use the static map. Your parameters will be overwritten by those given by the map server
[ INFO] [1345042406.058039834, 32.122000000]: Received a 640 X 544 map at 0.050000 m/pix

[ WARN] [1345042411.112545628, 37.177000000]: Waiting on transform from /base_link to /map to become available before running costmap, tf error: 
[ WARN] [1345042416.151239313, 42.215000000]: Waiting on transform from /base_link to /map to become available before running costmap, tf error: 
[ WARN] [1345042421.160855380, 47.224000000]: Waiting on transform from /base_link to /map to become available before running costmap, tf error: 
[ WARN] [1345042426.241246711, 52.305000000]: Waiting on transform from /base_link to /map to become available before running costmap, tf error:

THE AMCL FILE IS LIKE THIS (I have tried both setting use_map_topic either to true or false but doesn't work. If true, I have also tried to add the line <remap from="map" to="/map"/> but nothing):

<launch>

  <arg name="use_map_topic" default="true"/>
  <arg name="scan_topic" default="narrow_scan" />

  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic" value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type" value="diff"/>
    <param name="odom_alpha5" value="0.1"/>
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="60"/>
    <param name="laser_max_range" value="12.0"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="2000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.2"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.25"/>
    <param name="update_min_a" value="0.2"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="resample_interval" value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance" value="1.0"/>
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
    <remap from="scan" to="$(arg scan_topic)"/>    
  </node>

</launch>

THIS IS POSE_ROBOT_EKF

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

Comments

If you use robot_pose_efk in your turtlebot then you should connect to odom_combined. Try to add: <param name="base_frame_id" value="base_footprint"/> <param name="odom_frame_id" value="odom_combined"/> <param name="global_frame_id" value="/map" />

Jakub gravatar imageJakub ( 2012-08-15 05:28:03 -0500 )edit

do I have to set it in the robot_pose_ekf part or wherever in the file? I tried in the robot_pose_ekf, in the amcl file but nothing works... I am posting the portion of robot_pose_ekf in my answer

maurizio gravatar imagemaurizio ( 2012-08-15 05:30:17 -0500 )edit

No. You should add it to your amcl node. Instead of previous odom_frame_id=odom

Jakub gravatar imageJakub ( 2012-08-15 05:31:25 -0500 )edit
1

Done: it says "Waiting on transform from /base_link to /map to become available before running costmap, tf err"

maurizio gravatar imagemaurizio ( 2012-08-15 05:39:39 -0500 )edit

Once again in amcl.launch try with adding those two remappings: <remap from="static_map" to="/static_map" /> <remap from="map" to="/map" />

Jakub gravatar imageJakub ( 2012-08-15 05:43:52 -0500 )edit

It is always asking for: "Waiting on transform from /base_link to /map to become available". I don't understand why it says /base_link... why?

maurizio gravatar imagemaurizio ( 2012-08-15 05:45:34 -0500 )edit

Yes, that's the point. Most probably in your global or local costmap parameters you have /base_link instead of simply base_link without the slash. So global should have: /map and base_link, and local should have odom_combined and base_link.

Jakub gravatar imageJakub ( 2012-08-15 05:50:33 -0500 )edit

do I have to ckeck the code of the nodes? In particular do you know the files?

maurizio gravatar imagemaurizio ( 2012-08-15 05:54:30 -0500 )edit
0

answered 2015-04-18 15:19:41 -0500

YunfeiLu gravatar image

Hi, I met a problem when I test your launch file,

  • IOError: [Errno 2] No such file or directory: '/opt/ros/indigo/share/turtlebot_description/urdf/turtlebot.urdf.xacro' while processing /home/yunfeilu330/catkin_ws/src/beginner_tutorials/launch/robots.launch: Invalid tag: Cannot load command parameter [robot_description]: command [/opt/ros/indigo/share/xacro/xacro.py /opt/ros/indigo/share/turtlebot_description/urdf/turtlebot.urdf.xacro] returned with code [1].

It seems that I don't have turtlebot.urdf.xacro, then what should I use to instead?

edit flag offensive delete link more

Comments

I got the same error, how did u fix it ? thank you

Ramez gravatar imageRamez ( 2015-06-24 10:20:14 -0500 )edit

You can use a command like this to load the urdf file <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'"/>

base, stacks and 3d_sensor are arguments that you can pass to it.

sina.cb gravatar imagesina.cb ( 2015-07-08 13:16:08 -0500 )edit

My arguments look like this <arg name="base" value="$(optenv TURTLEBOT_BASE kobuki)"/> <arg name="stacks" value="$(optenv TURTLEBOT_STACKS hexagons)"/> <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>

sina.cb gravatar imagesina.cb ( 2015-07-08 13:16:39 -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

22 followers

Stats

Asked: 2012-08-14 22:20:00 -0500

Seen: 26,485 times

Last updated: Sep 16 '15