Running cartographer on several simulated gazebo robots

[cblesing]

Hey guys, first of all, thanks for that awesome work! I am working for Fraunhofer, a research institute. In general we deal with intralogistics and mobile robot platforms. In one of our recent research projects we deal with the topics multi-robot slam and semantic mapping. We want to use your cartographer for the 2D-slam part. In a first step I have set up a gazebo simulation with 2 simple simulated robots. Each has a initial start position robot_0 (left one): (x=2, y=0, phi=0°) and robot_1 (right one): (x=5, y=0, phi=180°). In gazebo everything looks quite fine. See the picture for that.

But when I start to look to rviz i get a little confused about what is going on behind the scenes.

As you can see, the odom frames are not at the positions that gazebo says. I don't understand that.

I have created 2 separate lua files, one for robot_0 and one for robot_1. Except of the specific namespace they are basically the same.

`include "map_builder.lua" include "trajectory_builder.lua"

options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "robot_0/base_link", published_frame = "robot_0/odom", odom_frame = "robot_0/odom", provide_odom_frame = false, publish_frame_projected_to_2d = true, use_odometry = true, use_nav_sat = false, use_landmarks = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., }

MAP_BUILDER.use_trajectory_builder_2d = true MAP_BUILDER.num_background_threads = 4

-- LOCAL SLAM

TRAJECTORY_BUILDER_2D.min_range = 0.1 TRAJECTORY_BUILDER_2D.max_range = 30. TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. TRAJECTORY_BUILDER_2D.use_imu_data = false TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 30 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 2 -- GLOBAL SLAM

POSE_GRAPH.constraint_builder.min_score = 0.55 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.8 POSE_GRAPH.optimize_every_n_nodes = 10 POSE_GRAPH.matcher_translation_weight = 1e5 POSE_GRAPH.matcher_rotation_weight = 0.0001

return options ` Here is my tf tree, which looks even fine.

But I don't understand the initial positions of the odom frames and in realtion to this the position of the base_link frames which are tracked by cartographer.

Can you help me out?

Thanks in advance

[oprezz]

Hello @cblesing!

I am working on a similar project but stuck at this point. Could you solve this problem? If so, can you share your experience?

Thank you!

[MichaelGrupp]

Closing as inactive

[GrantZheng86]

Hi @oprezz and @cblesing,

How did you guys manage to start SLAM for multiple robots? I am currently stuck at add the second robot to the task. I've tried using "start_trajectory" but that only starts SLAM using the default topics.

Thanks!

[oprezz]

Hi Grant! At the time I used the library, there was an option to to add configfile to the start_trajectory, like this:

rosservice call /start_trajectory "configuration_directory: '/home/${USER}/cartographer_ws/install_isolated/share/cartographer_ros/configuration_files/' configuration_basename: 'robot2.lua' use_initial_pose: true initial_pose: position: {x: 8.0, y: 18.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} relative_to_trajectory_id: 0"

hope this helps.

[GrantZheng86]

Hi @oprezz,

Thanks for your suggestion! I've tried similar things where I call the service with a designated lua configuration file. I was able to successfully start a new trajectory only when there's no active trajectory (i.e. All trajectories have been finished), and this only works for a single robot. So this is similar to #1321. My desired setup is to have cartographer start multiple trajectories that subscribe to multiple point cloud and tf topics published from multiple robots.

But when I tried to call the "start_trajectory" service, I couldn't find a way to let Cartographer create another trajectory that subscribes to different topics published from the second robot. Cartographer always subscribes to the remapped topics in the launch file (I have something like this <remap from="points2" to="rb1/reduced_points" />) Two of my robots are publishing point cloud and tf information to different topics (e.g. robot1 publishes to /rb1/pointcloud and robot published to /rb2/pointcloud), and I wanted cartographer to create two trajectories that subscribe to those two different topics.

In the RVIZ screenshot from this issue, it looks like there were two independent cartographer nodes launched because two independent /map topics were present (i.e. robot0/Map and robot1/Map). This isn't the desired behavior since those two robots cannot update the same map.

Do you have a similar setup for your Cartographer or you took a different route?

Thanks a lot for your help!

Grant

[oprezz]

I understand your task, I also did something very similar. To use the cartographer with multiple robots, you have to use it with the grpc extension.

Then you start the two robot which generates the sensors data, in different terimanals, with different ROS masters, e.g.: Terminal 1:

export GAZEBO_MASTER_URI=http://localhost:11312
ROS_MASTER_URI="http://localhost:11311" roslaunch robot_sim_pkg gazebo_robot1.launch

Terminal 2:

export GAZEBO_MASTER_URI=http://localhost:11112
ROS_MASTER_URI="http://localhost:11111" roslaunch robot_sim_pkg gazebo_robot2.launch

Terminal 3: ROS_MASTER_URI="http://localhost:11311" roslaunch cartographer_ros robot1_master.launch Terminal 4: ROS_MASTER_URI="http://localhost:11111" roslaunch cartographer_ros robot2_client.launch After this you can start the two cartographer nodes. One of them will be the cartographer master, the other one publishes its data to this master node.

The master node's launch file is something like this:

<launch>
<group>
  <env name="ROS_MASTER_URI" value="http://localhost:11311" />
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_grpc_server" pkg="cartographer_ros"
      type="cartographer_grpc_server.sh" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename robot1.lua">
  </node>

  <node name="cartographer_node_robot1" pkg="cartographer_ros"
      type="cartographer_grpc_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename robot1_cloud.lua
          -server_address localhost:55555
          -client_id 1"
   output="screen">

         <remap from="scan_1" to="front_scan" />
         <remap from="scan_2" to="rear_scan" />
     <remap from="scan" to="car1/top_scanner" /> 
     <remap from="odom" to="car1/odom" />
  </node>

</group>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/config.rviz" />
</launch>

The other client cartographer launch file is something like this:

<launch>
<group>
  <env name="ROS_MASTER_URI" value="http://localhost:11311" />
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_grpc_server" pkg="cartographer_ros"
      type="cartographer_grpc_server.sh" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename robot2_cloud.lua">
  </node>

  <node name="cartographer_node_robot2" pkg="cartographer_ros"
      type="cartographer_grpc_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename robot2.lua
          -server_address localhost:55555
          -client_id 2"
   output="screen">

         <remap from="scan_1" to="front_scan" />
         <remap from="scan_2" to="rear_scan" />
     <remap from="scan" to="car2/top_scanner" /> 
     <remap from="odom" to="car2/odom" />
  </node>

</group>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

The robot1_cloud.lua contains information about the server entrypoint:

include "map_builder_server.lua"

MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
MAP_BUILDER_SERVER.server_address = "0.0.0.0:55555"

return MAP_BUILDER_SERVER

The robot2_cloud.lua defines the connection for the client:

include "map_builder_server.lua"

MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
MAP_BUILDER_SERVER.uplink_server_address = "localhost:55555"

return MAP_BUILDER_SERVER

I hope this helps! Note that I used this pkg more then 1 year ago, and things could have changed.

[GrantZheng86]

Thanks a lot for the suggestions! I'll give it a try.

[sandilyasg]

@GrantZheng86 @cblesing @oprezz @MichaelGrupp how did you set up 2 robots with gazebo in ros to run cartographer? Can you please help, I am stuck with figuring out the namespace issues. What do your launch files look like? Please help, I opened an issue #1780