Cartographer Map Building

Function Pack Introduction

Cartographer is an open-source SLAM (Simultaneous Localization and Mapping) library, which is for real-time map building and positioning for robots or mobile devices. It is developed by Google and is widely applied in ROS (Robot Operating System).
Cartographer utilizes multiple sensors, such as LiDAR, Inertial Measurement Units (IMU), and cameras, to gather perceptual data from the environment. Through SLAM algorithms, it fuses these data sources to achieve Simultaneous Localization and Mapping. Cartographer can dynamically generate high-quality maps in real-time in unknown environments, providing accurate localization information for robots.

Demo

Install cartographer

Using apt to install cartographer:

sudo apt install ros-humble-cartographer
sudo apt install ros-humble-cartographer-ros

Check the installation:

ros2 pkg list | grep cartographer

When outputting the following content, it is successfully installed:

cartographer_ros
cartographer_ros_msgs

Create Function Pack

Enter workspace:

cd */ugv02/src

Create the function pack:

ros2 pkg create ugv02_cartographer

Create the launch and config file folder:

cd ugv02_cartographer
mkdir config launch

Compile cartographer Map Building

Create the cartographer_2d.lua file (configuration file) in the ugv02_cartographer/config directory:

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

options = {
  map_builder = MAP_BUILDER,  
  trajectory_builder = TRAJECTORY_BUILDER,  
  map_frame = "map",  -- map frame name 
  tracking_frame = "laser_link",  -- tracking frame name
  published_frame = "base_link",  -- published frame name 
  odom_frame = "odom",  -- name of the odometer frame
  provide_odom_frame = true,  -- whether to provide the odometer frame
  publish_frame_projected_to_2d = false,  -- whether to publish 2d gesture  
  use_pose_extrapolator = false,
  use_odometry = false,  -- whether use odometry
  use_nav_sat = false,  -- whether use the navigation satellite 
  use_landmarks = false,  -- whether use the landmark
  num_laser_scans = 1,  -- LiDAR number  
  num_multi_echo_laser_scans = 0,  -- number of multi-echo LiDAR  
  num_subdivisions_per_laser_scan = 1,  -- number of subdivisions for each laser scan
  num_point_clouds = 0,  -- number of cloud points
  lookup_transform_timeout_sec = 0.2,  -- timeout for finding transformations (seconds)  
  submap_publish_period_sec = 0.3,  -- submap release cycle (seconds)
  pose_publish_period_sec = 5e-3,  -- attitude release period (seconds)
  trajectory_publish_period_sec = 30e-3,  -- trajectory release period (seconds)
  rangefinder_sampling_ratio = 1.,  -- rangefinder sampling ratio
  odometry_sampling_ratio = 1.,  -- odometer sampling rate
  fixed_frame_pose_sampling_ratio = 1.,  -- fixed frame attitude sampling ratio  
  imu_sampling_ratio = 1.,  -- IMU sampling ratio
  landmarks_sampling_ratio = 1.,  -- landmarks sampling ratio
}
 
MAP_BUILDER.use_trajectory_builder_2d = true  -- whether use 2D SLAM
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35  -- Number of range data for submaps in the 2D track builder  
TRAJECTORY_BUILDER_2D.min_range = 0.1  -- ignore anything smaller than the robot radius, limiting it to the minimum scan range of the lidar
TRAJECTORY_BUILDER_2D.max_range = 3.5  -- the maximum scanning range of the lidar
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.  -- Restricted to maximum LiDAR scanning range  
TRAJECTORY_BUILDER_2D.use_imu_data = false  -- whether use IMU data
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true  -- Whether to scan for matches using real-time loopback detection

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)  -- Modify 1.0 to 0.1, increased sensitivity to movement
POSE_GRAPH.constraint_builder.min_score = 0.65  -- Modify 0.55 to 0.65, the minium score of Fast csm, can be optimized above this score 
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7  -- Modify 0.6 as 0.7, Minimum global positioning score below which global positioning is considered currently inaccurate

return options

Create cartographer.launch.py file under the path of src/ugv02_cartographer/launch/, and then you will start writing the launch file that starts the cartographer build node.

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # Positioning function pack
    pkg_share = FindPackageShare(package='ugv02_cartographer').find('ugv02_cartographer')
    
    # Configure node launch information 
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    # Map resolution
    resolution = LaunchConfiguration('resolution', default='0.05')
    # Map publish period  
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    # Configuration file folder path
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
    # Configuration file
    configuration_basename = LaunchConfiguration('configuration_basename', default='cartographer_2d.lua')

    #Nodes
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename])

    cartographer_occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])

    #Launch file
    ld = LaunchDescription()
    ld.add_action(cartographer_node)
    ld.add_action(cartographer_occupancy_grid_node)

    return ld

Open src/ugv02_cartographer/CmakeLists.txt, add the installation command, and the system can find these files after compiling.

install(
  DIRECTORY config launch
  DESTINATION share/${PROJECT_NAME}
)

Compile

Return the workspace and compile:

cd ~/ugv02
colcon build

Running Test

Open a new terminal, navigate to the workspace, source it, and start the radar as per the instructions in the tutorial "UGV02 Radar Tutorial" (ensure the serial port is authorized).

cd ~/ugv02
source ./install/setup.bash
ros2 launch ldlidar_stl_ros2 ld19.launch.py

Open a new terminal, navigate to the workspace, source it, and launch the map building.

cd ~/ugv02
source ./install/setup.bash
ros2 launch ugv02_cartographer cartographer.launch.py

In a computer (virtual machine) with Ubuntu and ros2, make sure that the computer is connected to the same network or wifi as the Raspberry Pi, and open a new terminal.

rviz2

Click on the add button in the bottom left corner of rviz to add a control to rviz to display the map:


The map effect is shown below:

Next, you can follow the Ugv02 Using LiDAR Tutorial to add LiDAR point cloud data display and vehicle coordinate frame in RViz. Then, refer to Chassis Control Tutorial to control the movement of the robot and observe the mapping process.
When you feel the map is OK, use the following command to save the map.
First, install the map management package.

sudo apt install ros-humble-nav2-map-server

Run the map-saving command with the last parameter being the path and name of the map.

ros2 run nav2_map_server map_saver_cli -f ./map