Mapping with ROS and Cartographer

In this class, we'll be mapping using Cartographer, Google's mapping software. Cartographer is built as a real-time SLAM package, but we'll be doing this in two steps, rather than driving our robot around and mapping at the same time. The reason for this is that driving the robot around and observing the world is time-consuming (though fun at first). Suppose you make a run, and the map is almost good. Wouldn't it be nice to be able to change some parameters and run the software again on exactly the same motions and observations? Of course it would. So:

Recording a trajectory

Our first step is to drive around, see some walls, and record what we see and what actions we take. ROS has the ability to store all this in what is called a "bag file," so we'll make one of those that holds everything Cartographer needs to build a map.

  1. In a terminal, run rosbag record /camera/depth/camera_info /camera/depth/image_raw /laser_scan /mobile_base/sensors/imu_data_raw /odom /tf It will give you an error - that's OK, just let it sit there.
  2. In a second terminal, run turtleLegsEyes
  3. Finally, in a third terminal, run roslaunch turtlebot_teleop keyboard_teleop.launch This terminal is the one that you'll use to drive the robot around.
  4. Drive the robot! Observe some walls, close some loops. Try to keep your paths fairly straight. It's OK if people quickly walk through the camera's field of view, as long as they don't stand there.
  5. Close all those out with Ctrl-C, in reverse order that you started them up.

OK! You now have a .bag file. Time to build a map!

Building a map

OK, time to replay that bag file, this time with Cartographer running. Have a seat, plug in your laptop.

  1. In a terminal, run roslaunch continuous_driver my_turtlebot.launch
  2. In a second terminal, run rosbag play --clock <YOURBAGFILENAME>
  3. In a third, run python ~/turtleAPI/viewMap.py to watch the map get constructed.
  4. If when it's done you like what you see, run rosservice call /finish_trajectory "stem: 'SOMETHING'" The SOMETHING is a filename of your choosing. You can then go to ~/.ros/ and see the SOMETHING.yaml and SOMETHING.pgm in there. If you look at the pgm, you'll see your map again.
  5. If you don't like your map, consider changing some parameters and running it again, rather than starting all over with a new trajectory. Open up ~/catkin_ws/src/cartographer_turtlebot/cartographer_turtlebot/configuration_files/turtlebot_depth_camera_2d.lua In there, you'll see some values you can play around with. Think your turns were noisy? Give the motion model some more randomness by turning up the max_angle_radians. Want smaller or bigger submaps? Turn num_laser_fans down or up, respectively. Want the bar higher or lower to recognize loop closures? Change min_score higher or lower, respectively. Play with it! To keep track, the default values are below.

Sharing a map with me

If you get a good map, please do the following:

  1. Run rosbag compress YOURBAGFILE - this might take some time, depending on the size of the trajectory.
  2. Let me know. I'll need the .yaml, the .pgm, the .bag file, the .pb file (which stores the reconstructed trajectory) and a copy of your turtlebot_depth_camera_2d.lua config file, all named the same thing (but with different extensions).

I'll need a bunch of good maps, so don't hold back.

Default config file

include "map_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  sensor_bridge = {
    horizontal_laser_min_range = 0.1,
    horizontal_laser_max_range = 4.,
    horizontal_laser_missing_echo_ray_length = 2.,
    constant_odometry_translational_variance = 1e-4,
    constant_odometry_rotational_variance = 1e-4,
  },
  map_frame = "map",
  tracking_frame = "gyro_link",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  use_odometry_data = true,
  use_horizontal_laser = true,
  use_horizontal_multi_echo_laser = false,
  num_lasers_3d = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.25
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = .075
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.covariance_scale = 1e-2
TRAJECTORY_BUILDER_2D.submaps.num_laser_fans = 300
SPARSE_POSE_GRAPH.optimize_every_n_scans = 100
SPARSE_POSE_GRAPH.constraint_builder.min_score = 0.8

return options