Use of SLAM_TOOLBOX

Usage of SLAM_TOOLBOX

    • Introduction
    • long term mapping
    • position
    • tool
      • Plug-in based optimizer
      • Map merging – Usage example of serialized raw data and pose map
        • dynamic
      • RVIZ plugin
      • Manually modify the map
    • API
      • Subscribed topics
      • Posted topics
      • Published services
    • Configuration file
      • Solver parameters
      • Toolbox parameters
      • matcher parameters
    • Install

Introduction

Slam Toolbox is a set of tools for 2D Slam. Key features include:

  • Create maps and save map pgm files
  • Refine the map, rebuild it, or continue building on a saved map
  • Long-term mapping: Load a saved map to continue mapping while removing irrelevant information from the new laser point cloud
  • Optimize positioning mode on existing maps. It is also possible to run positioning mode without mapping using LiDAR Odometer mode
  • Synchronous and asynchronous mapping
  • Dynamic map merge
  • Plug-in-based optimization solver with a new optimization plug-in based on Google Ceres
  • Interactive RVIZ plug-in
  • Provides RVIZ graphics manipulation tools for manipulating nodes and connections during mapping.
  • Map serialization and lossless data storage

For running on real production robots, it is recommended to use: slam-toolbox, which is optimized to make it about 10 times faster. Deb/source needs to be installed for other development tools (rviz plugins, etc.) that do not need to be installed on the robot.
The package has been benchmarked to build maps of ~30,000 sq. ft. at 5x+ real-time speed and ~60,000 sq. ft. at 3x real-time speed. The maximum area used is approximately 200,000 square feet. Map in synchronous mode (i.e. process all laser point clouds regardless of latency), and build larger spaces in asynchronous mode.
The mapping process is summarized as follows:

  1. ROS node: SLAM toolbox runs in synchronous mode to generate ROS nodes. This node subscribes to the laser scanning point cloud and odometry topics and publishes TF (map to odometry transformation) and maps.
  2. Obtain odometry and lidar data: In the callback function subscribed to the laser topic, a pose (using odometry) and a laser scan bound to the node will be generated. These PosedScan objects form a queue.
  3. Data processing: PosedScan object queue is used to build pose graph; laser scan matching is used to complete the odometry. This pose graph is used to calculate the robot pose and find closed loops. If a closed loop is found, the pose graph is optimized and the pose estimate is updated. Pose estimation is used to calculate and publish the robot’s odometry to map coordinate system transformation.
  4. Mapping: Laser scans associated with each pose in the pose graph are used to build and publish the map.

Long-term mapping

The concept of long-term mapping refers to fully or partially mapping an area and, over time, refining and updating that map as you continue to interact with the space. It also allows applications operating in the cloud and the use of multiple robots for mapping in a shared space (cloud distributed mapping). While Slam Toolbox can also be used for point-and-shoot mapping and save the map as a .pgm file, just as the map is traditionally stored, it also allows lossless saving of the pose map and metadata for reloading later using the same or different The robot continues to build maps.
Long-term mapping consists of several key steps:

  • Serialize and deserialize to store and reload map information
  • KD tree searches for matches to position the robot to its location upon reinitialization
  • Implementing SLAM based on pose graph optimization using 2D laser scanning

Allows users to create and update existing maps, then serialize data for use in other maps. Not only can the data be saved, but also pose graphs and associated metadata to be used.
Slam Toolbox supports all major modes:

  • Start from a predefined point (assumed to be close to the start area)
  • Start from any specific node – select a node ID to start from nearby
  • Start from any specific area – indicates the current pose in the map frame to start from, e.g. AMCL

In the RVIZ interface, you can reposition in the map or continue mapping graphically or programmatically using ROS services.

Positioning

Positioning consists of three parts:

  • Load an existing serialized map into a node
  • Maintain recent scan buffer data in pose graph
  • After the buffer expires, the scanned data will be deleted and the map will not be affected.

To enable localization mode, set mode: localization in the configuration file so that the Ceres plugin is configured correctly to be able to quickly add and remove nodes and constraints in the pose graph, but this is not strictly required , but performance optimization. Positioning mode will automatically load the pose map, take a first scan and match it to a local area to further refine the estimated position, and then start positioning.
To minimize the amount of changes required to migrate to this mode via AMCL, we also expose subscribers to the /initialpose topic used by AMCL to relocate to a location, which is also connected to RVIZ 2D Pose Estimation tool in . You can use our method to enter positioning mode and then continue using AMCL’s API for easy integration.

It can be considered as a replacement for AMCL which no longer requires any .pgm maps. If you want to modify the underlying map as you move, the long-term mapping mode above will do a better job. This positioning method may not be suitable for all applications, it does require a lot of tuning for your specific robot, and requires a high-quality odometry. AMCL is recommended for most beginners or users looking for a good out-of-the-box experience.

Tools

Plugin-based optimizer

Created a pluginlib interface for the ScanSolver abstract class to change the optimizer at runtime to test different optimizers.

Plug-ins have been generated for several different solvers that people may be interested in. Supports Ceres, G2O, SPA and GTSAM.

GTSAM/G2O/SPA is currently not supported. They don’t perform better than Ceres.

Map merging – usage example of serialized raw data and pose map

News

Use RVIZ and the plugin to load any number of pose maps, which will appear in RVIZ under map_N, along with a set of interactive markers to allow you to move them. Once you have them all positioned relative to each other the way you like, you can merge the submaps into a global map that can be downloaded using the map server implementation of your choice.

This shows more of what you can do once you have the raw data, but it doesn’t make much sense unless you’re used to manually stitching maps together.

RVIZ Plug-in

A rviz plugin is provided to aid manual loop closure and online/offline mapping. By default, interactive mode is off (allowing moving nodes) as this can have a large impact on rviz. When you want to move a node, check the interact box, move what you want, and save the changes. If you make a mistake, clear it. When finished, exit interactive mode again.

There is also a tool to help control online and offline data. You can stop processing new scans or save new scan data to a queue at any time. When you want to allow packet processing speed to catch up while the robot is stationary (This option only makes sense in synchronous mode. In asynchronous mode, the robot never falls behind.) or when you want to manually In loop closing/manual “help” mode, processing of new scan data is stopped. You can also clear the queue if it contains more content than you need.

Additionally, there are buttons for serializing and deserializing services, loading old pose maps for updating and refining, or continuing with the map and then saving back to a file. The “Start By Dock” checkbox will attempt to scan the first node for a match (assuming you start from a custom location), giving you a mileage estimate. Another option is to start using the location entered in the GUI or calling the underlying service. Additionally, if you happen to have just paused the robot or didn’t move much between runs, you can use the current mileage position estimate. Finally (and most usefully), you can use the RVIZ tool for 2D Pose Estimation, telling it where to go in Positioning Mode, just like AMCL.

Additionally, the RVIZ plugin will allow you to add serialized map files as submaps in RVIZ. They will display an interactive marker that you can translate and rotate to match, then use the “Generate Map” button to generate a composite map. At this point, the composite map is broadcast on the /map topic, and the map can be saved using map_saver.

It is recommended to always continue mapping near the dock. If this is not possible, start with pose or map merging techniques.

The interface is shown below.

Manually modify the map

Enable interactive mode and the graph node will change from a marker to an interactive marker that can be manipulated. When a node is moved, the changes can be saved, which will send the updated position to the pose graph and cause an optimization run to occur to change the pose graph with the new node position. This is helpful if the robot gets pushed, slips, hits a wall, or the odometer drifts and you want to correct it manually.

When the map is large enough, the number of interactive markers in RVIZ can be large, and RVIZ can start to lag. This feature is recommended as a test debugging tool, not for production use.

API

Below are the services/themes used. See the rviz plugin for the implementation it uses.

Subscribed topics

Topic Type Description
/scan sensor_msgs/LaserScan Input scan data from lidar
tf N/A Conversion of configured odom_frame to base_frame

Published topics

Topic Type Description
map nav_msgs/OccupancyGrid Represents the occupancy grid of the pose map with map_update_interval frequency
pose geometry_msgs/PoseWithCovarianceStamped configured The pose of base_frame in map_frame and the covariance calculated based on scan matching

Published services

Topic Type Description
/slam_toolbox/clear_changes slam_toolbox/Clear Clear changes for all pending manual pose graph operations
/slam_toolbox/deserialize_map slam_toolbox/DeserializePoseGraph Load and save from disk Serialized pose map file
/slam_toolbox/dynamic_map nav_msgs/OccupancyGrid Request the current state of the pose graph as the occupied grid
/slam_toolbox/manual_loop_closure slam_toolbox/LoopClosure Request manual changes to the pose graph
/slam_toolbox/pause_new_measurements slam_toolbox/Pause Pause processing of new incoming laser scans
/slam_toolbox /save_map slam_toolbox/SaveMap Save a map image file that can be used for display or AMCL positioning. It’s a simple wrapper over map_server/map_saver, but useful.
/slam_toolbox/serialize_map slam_toolbox/SerializePoseGraph Save Map pose diagrams and data can be used to continue mapping, slam_toolbox positioning, offline operations, etc.
/slam_toolbox/toggle_interactive_mode slam_toolbox/ToggleInteractive Toggle into and out of interactive mode

Configuration file

The default configuration is given in the config directory.

Solver parameters

  • solver_plugin – Nonlinear solver type for the karto sweep solver. Options: solver_plugins::CeresSolver, - solver_plugins::SpaSolver, solver_plugins::G2oSolver. Default: solver_plugins::CeresSolver.
  • ceres_linear_solver – The linear solver used by Ceres. Options: SPARSE_NORMAL_CHOLESKY, SPARSE_SCHUR, ITERATIVE_SCHUR, CGNR. Default is SPARSE_NORMAL_CHOLESKY.
  • ceres_preconditioner – The preconditioner used with this solver. Options: JACOBI, IDENTITY (none), SCHUR_JACOBI. The default is JACOBI.
  • ceres_trust_strategy – Trust zone strategy. Row search strategies are not exposed because they perform poorly for this purpose. Options: LEVENBERG_MARQUARDT, DOGLEG. Default value: LEVENBERG_MARQUARDT.
  • ceres_dogleg_type – If the trust policy is DOGLEG, the dogleg policy is used. Options: TRADITIONAL_DOGLEG, SUBSPACE_DOGLEG. Default value: TRADITIONAL_DOGLEG
  • ceres_loss_function – Loss function type for rejecting outliers. None equal loss squared. Options: None, HuberLoss, CauchyLoss. Default value: None.
  • mode – “Mapping” or “Location” mode, used for performance optimization in Ceres problem creation

Toolbox parameters

  • odom_frame – Odometer coordinate system

  • map_frame – map coordinate system

  • base_frame – base coordinate system

  • scan_topic – Scan the topic name, note that it is /scan not scan

  • scan_queue_size – Scan messages against queue length. Should always be set to 1 in async mode

  • use_map_saver – Instantiate the map service program and subscribe to the map topic yourself

  • map_file_name – Name of the pose map file loaded on startup (if available)

  • map_start_pose – Pose when starting mapping/positioning (if available)

  • map_start_at_dock – Starts pose map loading at the dock (first node) if available. If both pose and dock are set, use pose first.

  • debug_logging – Change logging for debugging

  • throttle_scans – Limit number of scans in sync mode

  • transform_publish_period – odom transform publishing period. 0 will not publish the transformation.

  • map_update_interval – Time interval between updating 2D occupancy map

  • enable_interactive_mode – Whether to enable interactive mode. Interactive mode will keep a cache of laser scans mapped to their IDs for visualization in interactive mode. As a result, the memory of the process will increase. This feature can be manually disabled in positioning and long-term mapping modes as they increase memory utilization over time. Valid for both mapping and continuous mapping modes.

  • position_covariance_scale – The amount to scale position covariance when publishing poses from scan matching. This can be used to adjust the effect of pose in downstream positioning filters. Covariance represents measurement uncertainty, so expanding the covariance will reduce the impact of pose on downstream filters. Default: 1.0

  • yaw_covariance_scale – Amount by which to scale yaw covariance when publishing poses from scan matches. See the description of position_covariance_scale. Default: 1.0

  • resolution – the resolution of the generated 2D occupancy map

  • max_laser_range – Maximum laser range used for 2D occupancy map rasterization

  • minimum_time_interval – Minimum duration between scans processed in synchronous mode

  • transform_timeout – Find transform TF timeout limit

  • tf_buffer_duration – How long TF messages are stored for querying. If running offline at double speed in sync mode, set it higher.

  • stack_size_to_use – Resets the stack size to the number of bytes to enable serialization/deserialization of the file. The free default is 40000000, but less is better.

  • minimum_travel_distance – Minimum distance traveled before processing a new scan

Matcher parameters

  • use_scan_matching – Whether to use scan matching to optimize mileage pose

  • use_scan_barycenter – whether to use center of gravity or scan pose

  • minimum_travel_heading – Minimum heading change to reasonably update

  • scan_buffer_size – Number of scans buffered into the chain, also used as the number of scans in the positioning mode circular buffer

  • scan_buffer_maximum_scan_distance – Delete the previous scan from the buffer, the maximum distance from the previous pose

  • link_match_minimum_response_fine – Fine resolution passed threshold link matching algorithm response

  • link_scan_maximum_distance – Maximum distance between valid link scans

  • Loop_search_maximum_distance – Maximum threshold of scan distance considered when closing a loop

  • do_loop_close – whether to do loop closing (answer is “true” if not sure)

  • Loop_match_minimum_chain_size – Finds the minimum chain length of a scan for loop closure

  • Loop_match_maximum_variance_coarse – threshold variance passed to refinement in coarse search

  • Loop_match_minimum_response_coarse – The threshold response of the loop closure algorithm in the coarse search to be passed to refinement

  • Loop_match_minimum_response_fine – Threshold response of loop closing algorithm in fine search passed to refinement

  • correlation_search_space_dimension – Search grid size for scan correlation

  • correlation_search_space_resolution – Search grid resolution for scan correlation

  • correlation_search_space_smear_deviation – Multimodal smear amount for smooth response

  • loop_search_space_dimension – Dimension of search grid for loop closure algorithm

  • loop_search_space_resolution – Search grid resolution for loop closure

  • loop_search_space_smear_deviation – Multimodal smear amount for smooth response

  • distance_variance_penalty – Penalty applied to matching scans as it differs from mileage pose

  • angle_variance_penalty – Penalty applied to matching scans as it differs from mileage pose

  • fine_search_angle_offset – Angle range used to test fine scan matching

  • rough_search_angle_offset – Angular range for testing rough scan matches

  • coarse_angle_resolution – Angular resolution within the offset range tested in scan matching

  • minimum_angle_penalty – Minimum penalty angle to ensure that dimensions do not expand

  • minimum_distance_penalty – The minimum penalty the scan can take to ensure the size does not explode

  • use_response_expansion – whether to automatically increase the search grid size if no feasible matches are found

Installation

Install using rosdep

rosdep install -q -y -r --from-paths src --ignore-src

or install via apt

apt install ros-eloquent-slam-toolbox

Run via

ros2 launch slam_toolbox online_sync_launch.py