SLAM and Nav2 for Custom Robots in ROS2

Continuing from my last two articles where I made the custom robot models and added different plugins for differential drive, camera and lidar, we will now shift our focus to implementing NAV2 stack and make a map using SLAM toolbox in this article.

Prerequisites

Before starting, ensure you have:

• ROS2 Humble installed
• Your custom robot URDF and Gazebo simulation ready
• Basic understanding of ROS2 launch files and packages

Step 1: Install Required Packages

First, install the necessary navigation and SLAM packages:

sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
sudo apt install ros-humble-slam-toolbox

Step 2: Prepare Your Workspace

Ensure you have a ROS2 workspace with your robot description package. We’ll assume the package is named diffbot_description. All packages are in this repo.

diffbot_description/
├── CMakeLists.txt
├── package.xml
├── config/
│   ├── mapper_params_online_async.yaml
│   ├── maps(all)
├── world/
│   ├── test.world(created from gazebo)
├── launch/
│   ├── display.launch.py
│   └── gazebo.launch.py
│   └── complete.launch.py
├── urdf/
│   └── diffbot.urdf
├── rviz/
│   └── diffbot2.rviz(created from rviz)

Step 3: Create Gazebo World Launch File

Create a launch file to load your custom Gazebo world. Here’s a general outline:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Nodedef generate_launch_description():
    

def generate_launch_description():
    pkg_share = get_package_share_directory('diffbot_description')
    
        # Path to the custom world file
    world_file_name = 'test.world'
    world_path = os.path.join(pkg_share, 'world', world_file_name)
    
    # Gazebo launch
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
        launch_arguments={'world': world_path}.items(),
    )


return LaunchDescription([
        gazebo,

  ])

Step 4: SLAM Toolbox Configuration

Create a configuration file mapper_params_online_async.yaml in your package's config folder:

slam_toolbox:
  ros__parameters:

    # Plugin params
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping #localization

    # if you'd like to immediately start continuing a map at a given pose
    # or at the dock, but they are mutually exclusive, if pose is given
    # will use pose
    # map_file_name: <path>/ros2_ws/src/diffbot_description/config/map
    # map_start_pose: [0.0, 0.0, 0.0]
    # map_start_at_dock: true

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 3.0
    resolution: 0.05
    max_laser_range: 12.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.5
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.5
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true 
    loop_match_minimum_chain_size: 10           
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

Step 5: SLAM Toolbox Launch File

Create a launch file for SLAM:

import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    pkg_share = get_package_share_directory('diffbot_description')

    slam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('slam_toolbox'), 'launch', 'online_async_launch.py')]),
        launch_arguments={
            'slam_params_file': os.path.join(pkg_share, 'config', 'mapper_params_online_async.yaml'),
            'use_sim_time': 'true'}.items(),
    )

return LaunchDescription([
        slam_launch
    ])

Step 6: Mapping Your Environment

1. Launch your Gazebo simulation with the robot
2. Launch the SLAM toolbox
3. Use teleop or your robot’s navigation to move around and create a map
4. In RViz, use the “Save Map” tool to save the map files

Step 7: Localization and Navigation

After mapping, you’ll have four files in your ros2_ws folder:

• map.data
• map.pgm
• map.posegraph
• map.yaml

Move them to config folder.

Update your mapper_params_online_async.yaml:

# Uncomment these lines
map_file_name: <path>/ros2_ws/src/diffbot_description/config/map
map_start_at_dock: true

Step 8: Launch Navigation2

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true

Troubleshooting Tips

1. Ensure all topic names match between URDF, Gazebo, and launch files
2. Check use_sim_time is set to true in all nodes
3. Verify TF frames are correctly defined
4. Adjust SLAM parameters for your specific robot and environment

Advanced Configurations

• Fine-tune SLAM parameters for better mapping
• Create multiple map configurations for different environments
• Implement dynamic obstacle avoidance in Nav2

Conclusion

You’ve now set up SLAM and Navigation2 for your custom robot in ROS2. This powerful combination allows autonomous navigation and environment mapping.

Happy robotics programming! 🤖🗺️🚀

ROS2-Projects/diffbot_description at main · ibrahimmansur4/ROS2-Projects