Humble Navigation Simulation

Navigation Overview

Navigation Overview

Concept

Robot navigation refers to the ability of a robot to autonomously move from one location to another without human intervention. In ROS2, the most commonly used framework for implementing navigation is Nav2.

Nav2, also known as Navigation2, is built upon ROS's navigation stack. It adopts the same cutting-edge technology used in autonomous vehicles, optimized and adapted specifically for the navigation needs of mobile robots and ground-based robots. This project empowers mobile robots to traverse complex environments, enabling them to perform a wide range of user-defined application tasks, and is compatible with virtually any type of robot dynamics. Nav2 not only supports robot movement from point A to point B but also allows for setting intermediate poses and executing various tasks such as object tracking and full-coverage navigation. As a production-grade, high-quality navigation framework trusted by over 100 companies worldwide, Nav2 has earned widespread acclaim for its exceptional reliability and stability.

Purpose

Nav2 plays a significant role in the field of robot navigation, mainly reflected in the following aspects.

• Nav2 offers a diverse range of navigation capabilities, supporting multiple sensor inputs such as LiDAR and cameras to obtain real-time environmental information, enabling intelligent path planning and precise motion control. It supports various navigation modes to meet different application needs and features strong extensibility, allowing seamless integration with ROS 2 components.
• Nav2 delivers outstanding performance, utilizing efficient algorithms and data processing techniques to ensure the robot responds quickly and stably during navigation.
• In terms of safety, Nav2 uses multiple obstacle avoidance algorithms and parameter tuning functions, defines safety zones, and provides diagnostic monitoring to ensure safe robot navigation.

In short, Nav2 provides powerful support for robot navigation and applications. Whether it's automated production in the industrial sector or intelligent robots in the service industry, Nav2 plays a key role in enhancing a robot's autonomy and level of intelligence.

Navigation Installation

Using Ubuntu's package manager, you can install Nav2 in binary form with the following command:

sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup

# Humble
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup

# Jazzy
sudo apt install ros-jazzy-navigation2
sudo apt install ros-jazzy-nav2-bringup

Replace <ros2-distro> in the instructions with the currently used ROS2 version.

Navigation Conditions

To deeply learn Nav2, you need to understand the basics of ROS2 robots and have a simulation or physical robot as a practice environment.

ROS2 Basics

• ROS2 Communication: Understand basic concepts in ROS2 such as Nodes, Topics, Services, and Actions, and how to achieve communication between nodes through these mechanisms.
• Lifecycle Management: Familiarize yourself with lifecycle nodes in ROS2, understanding the state transition process of nodes including startup, configuration, activation, deactivation, and cleanup. This is crucial for managing complex ROS2 systems.
• rviz2: Proficient in using rviz2 for visual debugging, including how to set different display types (such as point clouds, maps, paths, etc.), and how to interact with ROS2 nodes through rviz2 (such as setting navigation goals, observing robot status, etc.).
• URDF: Understand URDF (Unified Robot Description Format) files, which are an XML format used in ROS to describe robot models. Become familiar with how to write and modify URDF files to accurately represent robot structures in simulation or real-world environments.
• TF Coordinate Transformations: Master the use of the TF (Transform) library and understand the importance of coordinate transformations in robot navigation. Learn how to set up and query transformation relationships between different coordinate systems to ensure the robot can accurately localize itself and its surrounding environment.

Practical environment

• Simulation Environment: Set up a ROS2-compatible simulation environment, and configure the corresponding robot model, sensors (such as LiDAR, camera, etc.), and simulation world. Ensure that the robot in the simulation environment can receive velocity commands, feedback odometry messages, publish sensor data, and TF coordinate transforms.
• Physical Robot: If conditions permit, prepare a physical robot. This robot should have similar capabilities to those in the simulation environment, meaning it can receive velocity commands, provide odometry feedback, publish sensor data such as LiDAR scans, and publish TF coordinate transforms. Additionally, ensure the physical robot has the ROS2 system installed and that the corresponding drivers and algorithm libraries are configured.
• Nav2 Package: Install and configure the Nav2 package along with its dependencies. Nav2 is a comprehensive software package in ROS2 for robot navigation, which includes multiple functional modules such as path planning, obstacle avoidance, and local and global map maintenance. Ensure that the Nav2 package is compatible with your ROS2 version and has been properly configured as needed.

By mastering the foundational knowledge above and preparing the practice environment, you will be better equipped to learn and apply Nav2 for developing and debugging robot navigation tasks.

Navigation Parameter Reference

https://docs.nav2.org/configuration/index.html

SLAM localization and mapping

Concept

SLAM (Simultaneous Localization and Mapping) is a key technology in robotics and autonomous navigation, enabling robots to map an unknown environment.

Performing SLAM under ROS 2 typically involves using SLAM-related software packages specifically developed for ROS 2. These packages leverage ROS 2's communication framework (such as topics, services, and actions) to process incoming sensor data, including LiDAR, cameras, inertial measurement units, and more.

Below are some common SLAM systems used in ROS2:

1. SLAM Toolbox: SLAM Toolbox is a widely used SLAM solution in ROS 2, maintained by Steve Macenski. It was developed to replace the well-known gmapping and Cartographer SLAM packages from the original ROS, providing a variety of features in the field of 2D laser SLAM.
2. Cartographer for ROS 2: Cartographer is a 2D and 3D SLAM library developed by Google. Although it was originally developed for ROS, an adapted version has been created for ROS 2, enabling its use within the ROS 2 ecosystem.

To start a SLAM project in ROS 2, you'll also need specific sensors (such as a LiDAR or camera), sufficient computing power to run SLAM algorithms, and familiarity with ROS 2 concepts like nodes, topics, services, actions, and parameters to enable effective data processing and communication. As the project evolves, you may also need to consider advanced features such as dynamic reconfiguration, 3D mapping, and path planning.

Purpose

First, it must be clarified that SLAM and Nav2 do not have a direct dependency relationship—they are two relatively independent technical frameworks. However, in practical applications, the two exhibit a close connection and complementarity.

Independence: SLAM can independently construct an environmental map without the involvement of Nav2. It relies on sensor data to perceive the environment and uses algorithms to estimate the robot's position and pose, thereby building a map of the surroundings. At the same time, Nav2 also possesses autonomous navigation capabilities. Even without a SLAM map as input, it can rely on environmental information from other sources to navigate.

Complementarity: Although the two are technically independent of each other, their combination brings significant advantages. Nav2 can leverage the precise maps created by SLAM for efficient path planning, ensuring the robot can smoothly navigate to its target location. Meanwhile, SLAM can utilize Nav2's navigation control functions to collect environmental information in real time as the robot moves, thereby further improving the efficiency and accuracy of map building.

Therefore, although SLAM and Nav2 are technically independent of each other, their complementarity in building a complete robot navigation system enables robots to achieve more autonomous and precise mapping or navigation in complex environments.

Overview of slam_toolbox

concept

SLAM Toolbox is an open-source software toolkit for building 2D maps, designed to provide researchers and developers with a platform for quickly constructing and implementing SLAM algorithms. It integrates a variety of commonly used SLAM algorithms, offers rich data processing and filtering functions, as well as tools for map building and environmental modeling.

Function

• Algorithm Integration includes SLAM algorithms based on Kalman filters and particle filters, as well as SLAM algorithms based on graph optimization.
• Data Processing provides data fusion, data preprocessing, and anomaly detection capabilities, supporting the processing of data from multiple sensors such as LiDAR, cameras, and IMUs.
• Map Building Constructs an accurate map using sensor data and robot motion information, then updates and optimizes it.
• Modular design supports users inserting their own algorithms or replacing existing modules to achieve highly customized SLAM solutions.

Advantages

• Open-source and Flexibility As open-source software, SLAM Toolbox provides source code and documentation, allowing users to freely modify and extend the algorithms.
• Multi-Sensor Support Supports data from various sensors such as LiDAR, cameras, and IMUs, adapting to the needs of different application scenarios.
• Efficiency Leverages modern C++ features to optimize performance, ensuring efficient operation even on resource-constrained hardware.
• Broad Application Prospects Suitable for multiple fields including academic research, drone navigation, autonomous vehicles, indoor robots, and industrial automation.

slam_toolbox installation

Using the Advanced Packaging Tool (APT) package manager, you can install slam_toolbox in binary form with the following command:

sudo apt install ros-<ros2-distro>-slam-toolbox

#humble
sudo apt install ros-humble-slam-toolbox
#jazzy
sudo apt install ros-jazzy-slam-toolbox

Replace <ros2-distro> in the instructions with the currently used ROS2 version.

slam_toolbox node description

In slam_toolbox, two commonly used nodes are: sync_slam_toolbox_node and async_slam_toolbox_node.

sync_slam_toolbox_node：

This is a synchronization node that waits for all sensor data to arrive before processing, ensuring data integrity and consistency, thereby improving the accuracy of localization and mapping. However, synchronous processing may introduce latency, making it more suitable for scenarios that prioritize data consistency and accuracy over real-time performance.

async_slam_toolbox_node：

Unlike a synchronous node, this is an asynchronous node that can process received data immediately, reducing latency and improving response speed. However, asynchronous processing may lead to incomplete data synchronization, and the localization and mapping results may be less accurate than those from a synchronous approach. Therefore, it is better suited for scenarios that demand high real-time performance but have relatively lower requirements for data consistency and accuracy.

When choosing between sync_slam_toolbox_node and async_slam_toolbox_node, you need to weigh the application requirements and environment. If real-time performance is critical and some localization or mapping error is acceptable, choose async_slam_toolbox_node. If data consistency and accuracy are more important, and real-time performance is not the primary concern, then choose sync_slam_toolbox_node. Additionally, the main difference between the two lies in the data processing method, while the topics published, topics subscribed to, services offered, and parameters used by both nodes are the same.

1. Subscribed topics

Although not subscribing to /odom, it is necessary to publish /odom in order to change the coordinates.

2. Published topics

3. Published services

4. parameter
   1. Solver parameters

• solver_plugin The nonlinear solver type used for the Karto scan 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. Default is JACOBI.
• ceres_trust_strategy Trust region strategy. Line search strategies are not exposed because they perform poorly for this use case. Options: LEVENBERG_MARQUARDT, DOGLEG. Default: LEVENBERG_MARQUARDT.
• ceres_dogleg_type If the trust strategy is DOGLEG, use the dogleg strategy. Options: TRADITIONAL_DOGLEG, SUBSPACE_DOGLEG. Default: TRADITIONAL_DOGLEG.
• ceres_loss_function Loss function type for rejecting outliers. None is equivalent to squared loss. Options: None, HuberLoss, CauchyLoss. Default: None.
• mode "Mapping" or "Localization" mode, used for performance optimization in Ceres problem creation.
  • Toolbox parameters
• odom_frame odometry coordinate frame
• map_frame Map coordinate system
• base_frame Base coordinate system
• scan_topic Scans the topic name. Note that it is /scan, not scan.
• scan_queue_size The queue length for scan messages. In asynchronous mode, this should always be set to 1.
• use_map_saver instantiates the map service program and subscribes to the map topic on its own.
• map_file_name The name of the pose graph file loaded at startup (if available).
• map_start_pose The pose when starting mapping/localization (if available)
• map_start_at_dock Loads the pose graph at the dock (first node) on startup (if available). If both pose and dock are set, pose takes precedence.
• debug_logging Change logs for debugging
• throttle_scans The number of scans limited in synchronous mode
• transform_publish_period The publishing period for odometry (odom) transforms. A value of 0 will not publish transforms.
• map_update_interval The time interval for updating the 2D occupancy map.
• enable_interactive_mode controls whether interactive mode is enabled. Interactive mode retains a cache of laser scans mapped to their IDs for visualization during interactive sessions. As a result, the process's memory usage will increase. This feature can be manually disabled in localization and long-term mapping modes, as they increase memory utilization over time. It is effective for both mapping and continuous mapping modes.
• position_covariance_scale The amount by which to scale the position covariance when publishing a pose from scan matching. This can be used to adjust the influence of the pose in downstream localization filters. The covariance represents the uncertainty of the measurement, so increasing the covariance will reduce the impact of the pose on the downstream filter. Default value: 1.0
• yaw_covariance_scale The amount by which to scale the yaw covariance when publishing poses from scan matching. See the description of position_covariance_scale. Default value: 1.0
• resolution The resolution of the generated 2D occupancy grid map
• max_laser_range Maximum laser range used for 2D occupancy map rasterization
• The minimum time interval between scans processed in synchronous mode.
• transform_timeout Lookup transform TF timeout limit
• tf_buffer_duration The duration for which TF messages are stored for querying. If running offline at a multiple speed in synchronous mode, set this higher.
• stack_size_to_use The number of bytes to reset the stack size to, in order to enable file serialization/deserialization. The free default value is 40,000,000, but less is better.
• minimum_travel_distance Minimum travel distance before processing a new scan
  • Matcher parameters
• use_scan_matching – whether to use scan matching to optimize odometry poses.
• use_scan_barycenter Whether to use the center of gravity or scan pose
• minimum_travel_heading Minimum reasonable heading change for an update
• scan_buffer_size The number of scans buffered into the chain, also used as the number of scans in the loop buffer for localization mode.
• scan_buffer_maximum_scan_distance The maximum distance from a previous pose at which to remove previous scans from the buffer.
• link_match_minimum_response_fine Threshold for fine-resolution link matching algorithm response
• link_scan_maximum_distance Maximum distance between valid link scans
• Loop_search_maximum_distance Maximum threshold for scan distance considered during loop closure
• do_loop_close Whether to perform loop closure (if unsure, the answer is "true")
• Loop_match_minimum_chain_size Minimum chain length for finding closed-loop scans
• Loop_match_maximum_variance_coarse The threshold variance passed to refinement during the coarse search.
• Loop_match_minimum_response_coarse The threshold response of the loop closure algorithm in the coarse search to be passed to the refinement.
• Loop_match_minimum_response_fine The threshold response of the loop closure algorithm during fine search is passed to the refinement.
• correlation_search_space_dimension Search grid size for scan correlation
• correlation_search_space_resolution The search grid resolution for scan correlation.
• correlation_search_space_smear_deviation The multimodal smear amount used to smooth the response.
• The size of the search grid for the loop closure algorithm.
• loop_search_space_resolution Search grid resolution for loop closure
• loop_search_space_smear_deviation — multimodal smear amount for smoothing responses
• distance_variance_penalty A penalty applied to matching scans, as it differs from the odometry pose.
• angle_variance_penalty A penalty applied to matching scans when they deviate from the odometry pose.
• fine_search_angle_offset Used to test the angular range for fine scan matching.
• rough_search_angle_offset is used to test the angular range for coarse scan matching.
• coarse_angle_resolution The angular resolution within the offset range tested during scan matching.
• minimum_angle_penalty The minimum penalty angle that ensures dimensions do not expand.
• The minimum_distance_penalty scan ensures the minimum penalty that prevents sizes from exploding.
• use_response_expansion If no feasible match is found, whether to automatically increase the search grid size.

Basic usage of slam_toolbox

1. Preparation Work

In the src directory, first run the following command to create a package in the workspace's src directory:

ros2 pkg create mycar_slam_slam_toolbox --dependencies slam_toolbox

2. Write launch files and parameter files

Under the function package, create a new launch directory and params directory. Inside the launch directory, create a new online_sync_launch.py file and enter the following content:

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_params_file = LaunchConfiguration('slam_params_file')

    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation/Gazebo clock')
    declare_slam_params_file_cmd = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(get_package_share_directory("mycar_slam_slam_toolbox"),
                                   'params', 'mapper_params_online_sync.yaml'),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node')

    start_sync_slam_toolbox_node = Node(
        parameters=[
          slam_params_file,
          {'use_sim_time': use_sim_time}
        ],
        package='slam_toolbox',
        executable='sync_slam_toolbox_node',
        name='slam_toolbox',
        output='screen')

    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time_argument)
    ld.add_action(declare_slam_params_file_cmd)
    ld.add_action(start_sync_slam_toolbox_node)

    return ld

This launch file primarily loads the sync_slam_toolbox_node node from the slam_toolbox package and reads a configuration file named mapper_params_online_sync.yaml from the current package's params directory. This configuration file does not exist yet, so you need to create a new file named mapper_params_online_sync.yaml in the params directory and enter the following content:

slam_toolbox:
  ros__parameters:
    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

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

    #map_file_name: test_steve
    #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 
    map_update_interval: 2.0
    resolution: 0.05
    max_laser_range: 20.0 
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    stack_size_to_use: 40000000 
    enable_interactive_mode: true

    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.1
    minimum_travel_heading: 0.1
    scan_buffer_size: 100
    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_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    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

The content of the configuration file needs to be dynamically adjusted based on the actual situation.

3. Edit the configuration file

Open CMakeLists.txt and enter the following content:

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

4. Compile

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select mycar_slam_slam_toolbox

5. Execute
   1. Please first call the following command to start the simulation environment:

   . install/setup.bash
   ros2 launch demo_gazebo_sim gazebo_sim_robot_world.launch.py
   

   3. Then, in the terminal, navigate to the current workspace and enter the following command:

   . install/setup.bash
   ros2 launch mycar_slam_slam_toolbox online_sync_launch.py use_sim_time:=True
   

   5. Start rviz2, set the Fixed Frame to map, add the map plugin and set the topic to /map, and the map created by slam_toolbox will be displayed. As the robot moves, the map will update accordingly.
   6. The use_sim_time:=True parameter indicates that simulation time is used.

Finally, it should be noted that this section uses the sync_slam_toolbox_node node, which builds the map synchronously, while the usage of the asynchronous mapping node async_slam_toolbox_node is similar to the synchronous one.

We use the keyboard control node to drive the robot across the entire map.

Black: obstacle

White: obstacle-free zone

Gray: Unknown area

SLAM is mapping and localization. The above covers mapping, so what is localization?

The positioning means Slam will publish a /tf, which contains the coordinate transformation between the robot and the map.

This /tf specifically publishes the coordinate transform from map to odom, so you need to handle the coordinate relationship between odom and base_link yourself.

This design allows the entire coordinate tree to be a chain structure, preventing base_link or base_footprint from having two parent classes, which would increase computational load.

Notes:

1. Suddenly, the image quality degraded.
   1. If your environment is very simple, the map may suddenly become corrupted during mapping. In this case, it is most likely an issue with do_loop_closing. Try setting do_loop_closing to false and test again. This thing uses SLAM to detect shapes with lasers and match them against previously traveled paths. If the matched shapes are similar enough, it connects the current trajectory with the previous one, then re-optimizes the entire pose graph to make the overall map more consistent. Mainly to prevent odom from drifting. Since odom drifts, when the robot returns to the starting point, SLAM might think the position is off by tens of centimeters, but this can be corrected using the LiDAR.
   2. Try to go straight through narrow passages as much as possible, and avoid rotating in place. If a crash occurs in such a narrow passage, it is most likely due to issues with scan matching related parameters.

Overview of Cartographer

Concept

Cartographer is a laser SLAM algorithm library based on graph optimization, released by Google, supporting the construction of both 2D and 3D maps. It combines data from LiDAR and an Inertial Measurement Unit (IMU), using efficient algorithms to achieve real-time, accurate localization and mapping.

Features

• Parallel Scan Matching leverages parallel computing techniques to accelerate scan matching speed and improve mapping efficiency.
• Pose Graph Optimization uses graph optimization techniques to estimate the robot's pose and the topological structure of the map, reducing accumulated error.
• Real-time Map Updates Update the map in real-time as the robot moves to ensure accuracy and timeliness.
• Loop Closure Detection identifies areas the robot has previously visited, further reducing accumulated error and improving the global consistency of the map.
• Multi-sensor fusion supports the fusion of data from multiple sensors such as LiDAR, IMU, and odometry, improving the accuracy of localization and mapping.

Advantages

• Efficient and Stable Cartographer's algorithm is carefully designed and optimized to run efficiently and stably in complex environments.
• High precision Provides high-precision localization and mapping results through graph optimization and loop closure detection techniques.
• Flexibility Supports both 2D and 3D map building to meet the needs of different application scenarios.
• Open source and free Cartographer is an open source project, allowing users to freely access and use its source code and documentation.
• Community Support Features an active community support system, allowing users to obtain help and support from developers worldwide.

Install Cartographer

Using Ubuntu's package manager, you can install Cartographer in binary form with the following command:

sudo apt install ros-<ros2-distro>-cartographer
sudo apt install ros-<ros2-distro>-cartographer-ros

# humble
sudo apt install ros-humble-cartographer
sudo apt install ros-humble-cartographer-ros
#jazzy
sudo apt install ros-jazzy-cartographer
sudo apt install ros-jazzy-cartographer-ros

Of the two installation commands above, the first one is used to install the core library of Cartographer. This package does not integrate directly with ROS2 but exists as an independent algorithm library, providing underlying computational support for mapping and localization. The second one is the Cartographer wrapper for the ROS2 environment, which provides an interface with the ROS2 system, allowing the Cartographer algorithm to run within the ROS2 environment. Additionally, please replace <ros2-distro> in the commands with the current ROS2 version being used.

Cartographer node description

In the Cartographer framework, cartographer_node and cartographer_occupancy_grid_node are two key nodes, each serving distinct roles and functions. A detailed introduction follows.

cartographer_node：

Primarily responsible for subscribing to data from various sensors (such as LiDAR, IMU, odometry, etc.) and building a map in real time based on this data. It uses a submap approach to gradually construct and update the map, ensuring accurate localization and real-time mapping.

cartographer_occupancy_grid_node：

This node is responsible for receiving the submap list (/submap_list) published by cartographer_node, stitching it into a complete occupancy grid map, and then publishing this map. This node is the final step in map generation, enabling Cartographer to output a map that is human-readable and easy to visualize.

The collaborative work of these two nodes involves the former being responsible for real-time map construction and updates, while the latter is responsible for stitching sub-maps into a complete grid map and publishing it, enabling Cartographer to efficiently implement SLAM functionality.

1. Topics subscribed to by cartographer_node

2. The topics published by cartographer_node

3. The service published by cartographer_node

4. cartographer_node parameters

cartographer_node node needs to receive a parameter configuration file that contains various parameters required for map building, trajectory tracking, and other tasks.

5. The topic subscribed to by cartographer_occupancy_grid_node.

6. The topic published by cartographer_occupancy_grid_node

7. The service requested by cartographer_occupancy_grid_node

8. cartographer_occupancy_grid_node parameters

cartographer_occupancy_grid_node The node needs to configure parameters such as the map's resolution and update cycle to ensure the generated grid map meets specific accuracy and real-time requirements.

Basic usage of Cartographer

1. Preparation Work

In the src directory, first run the following command to create a package in the workspace's src directory:

ros2 pkg create mycar_slam_cartographer --dependencies cartographer

2. Write launch files and parameter files

Under the function package, create a new launch directory and params directory. Inside the launch directory, create a new cartographer.launch.py file and enter the following content:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'false')
    resolution_arg = DeclareLaunchArgument('resolution', default_value='0.05')

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', os.path.join(get_package_share_directory("mycar_slam_cartographer"),"params"),
            '-configuration_basename', 'mycar.lua'],
        output = 'screen'
    )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
            {'resolution': LaunchConfiguration('resolution')}],
    )

    return LaunchDescription([
        use_sim_time_arg,
        resolution_arg,
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

This launch file primarily loads the cartographer_node and cartographer_occupancy_grid_node nodes from cartographer_ros, and reads a configuration file named mycar.lua from the params directory of the current package. This configuration file does not exist yet, so you need to create a new mycar.lua file in the params directory and enter the following content:

include "map_builder.lua" -- 地图构建器
include "trajectory_builder.lua" -- 轨迹构建器

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",  -- 地图坐标系
  tracking_frame = "base_link", -- 跟踪的坐标系，可以是基坐标系、雷达或imu的坐标系
  published_frame = "odom", -- cartographer发布的位姿（pose）的坐标系
  odom_frame = "carto_odom",  -- cartographer 计算后优化的里程计，并非机器人本身里程计
  provide_odom_frame = false, -- 是否发布cartographer的里程计
  publish_frame_projected_to_2d = true, -- 是否转换成2d(无俯仰、滚动的情况下为 true)
  use_odometry = true, -- 是否订阅里程计数据
  use_nav_sat = false, -- 是否订阅GPS
  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 = 1.5, -- 坐标变换超时时间
  submap_publish_period_sec = 0.5, -- 发布子图的时间间隔
  pose_publish_period_sec = 5e-3, -- 发布pose的时间间隔
  trajectory_publish_period_sec = 30e-3, -- 发布轨迹的时间间隔
  rangefinder_sampling_ratio = 1., -- 雷达采样比例
  odometry_sampling_ratio = 0.8, -- 里程计采样比例(如果里程计精度低，可以减小该设置值)
  fixed_frame_pose_sampling_ratio = 1., -- 参考坐标系采样比例
  imu_sampling_ratio = 1.,-- imu采样比例
  landmarks_sampling_ratio = 1., -- 路标采样比例
}

MAP_BUILDER.use_trajectory_builder_2d = true -- 启用2D轨迹构建器

TRAJECTORY_BUILDER_2D.min_range = 0.15 -- 最小雷达有效距离
TRAJECTORY_BUILDER_2D.max_range = 6.0 -- 最大雷达有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. -- 缺失数据的射线长度
TRAJECTORY_BUILDER_2D.use_imu_data = false -- 是否使用 imu 数据
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 是否使用在线相关扫描匹配
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) -- 运动滤波器的最大角度限制（以弧度为单位）

POSE_GRAPH.constraint_builder.min_score = 0.65 -- 建约束时的最小分数
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 -- 全局定位时的最小分数

-- POSE_GRAPH.optimize_every_n_nodes = 0

return options

The content of the configuration file needs to be dynamically adjusted based on the actual situation.

3. Edit the configuration file

Open CMakeLists.txt and enter the following content:

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

4. Compile

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select mycar_slam_cartographer

5. Execute

(1) First, call the following command to start the simulation environment:

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_robot_world.launch.py

(2) Then, in the terminal, navigate to the current workspace and enter the following command:

. install/setup.bash
ros2 launch mycar_slam_cartographer cartographer.launch.py use_sim_time:=True

(3) Start rviz2, set Fixed Frame to map, add the map plugin and set the topic to /map. The created map will then be displayed, and as the robot moves, the map will update accordingly.

Map service

During SLAM mapping, map data is stored in memory, which means that once the node is shut down, the data is also released. A more reasonable implementation would be to serialize the constructed map to disk for persistent storage, and later deserialize the map data from disk for other operations. In Nav2, the relevant packages for map serialization and deserialization have already been encapsulated. The package is: nav2_map_server.

In nav2_map_server, you can interact with the rest of the Nav2 system through topics and service interfaces. Under the nav2_map_server package, there are two important nodes: map_saver_cli and map_server. The map_saver_cli node can be used to save the map, while the map_server node displays the map at startup.

Save the map (serialization)

Map Saving Node Description

The map saving node in nav2_map_server is map_saver_server. The relevant information about this node is as follows.

1. Subscribed topics

2. parameter

• save_map_timeout The maximum wait time for the save map operation.
• free_thresh_default The probability threshold below which a grid cell is considered unoccupied.
• occupied_thresh_default The probability threshold at which a grid cell is considered occupied.
• map_subscribe_transient_local Messages are not retained after node restart, default is true.

3. map_saver_cli

Additionally, for ease of use, an executable program called map_saver_cli has been wrapped on top of map_saver_server. It allows for more convenient configuration of map-saving related data via actual parameters, and subsequent execution also calls map_saver_cli. Its actual parameter list is as follows:

• -t The map topic being subscribed to.
• -f Map storage path.
• --occ The probability threshold at which a grid cell is considered occupied.
• --free The probability threshold at which a grid cell is considered unoccupied.
• --fmt Image format.
• --mode Map mode: trinary (default), scale, or raw.

Basic Map Saving Operations

Preparation

Please start the simulation or the physical robot first, then launch the SLAM-related nodes to implement basic mapping functionality.

Save the Map

SLAM mapping is complete. In the terminal, navigate to the workspace and run the following command to save the map:

ros2 run nav2_map_server map_saver_cli -f map/my_map

The above command will subscribe to the /map topic and save the data from the /map topic as files. In the map directory under the workspace (you need to create this directory yourself, otherwise an exception will be thrown), two files will be generated, named: my_map.yaml and my_map.pgm.

Map interface

In Nav2, there are two main interfaces related to maps:

• nav_msgs/msg/MapMetaData Map metadata, including the map's width, height, resolution, and other properties.
• nav_msgs/msg/OccupancyGrid map grid data, typically displayed graphically in RViz.

nav_msgs/msg/MapMetaData

Call the command ros2 interface show nav_msgs/msg/MapMetaData to view the interface format, which displays the following content (comments have been localized):

# 它包含了关于OccupancyGrid特性的基本信息

# 地图加载时间
builtin_interfaces/Time map_load_time
        int32 sec
        uint32 nanosec

# 地图分辨率 [米/像素]
float32 resolution

# 地图宽度 [像素]
uint32 width

# 地图高度 [像素]
uint32 height

#地图的原点坐标[米，米，弧度]。这是地图中单元格(0,0)左下角在现实世界中的位置和方向。
geometry_msgs/Pose origin
        Point position
                float64 x
                float64 y
                float64 z
        Quaternion orientation
                float64 x 0
                float64 y 0
                float64 z 0
                float64 w 1

nav_msgs/msg/OccupancyGrid

Call the command ros2 interface show nav_msgs/msg/OccupancyGrid to view the interface format, which displays the following content (comments have been localized):

# 它代表一个二维网格地图。
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# 地图元数据
MapMetaData info
        builtin_interfaces/Time map_load_time
                int32 sec
                uint32 nanosec
        float32 resolution
        uint32 width
        uint32 height
        geometry_msgs/Pose origin
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1

# 地图数据按照行优先的顺序进行排列，

# 这意味着首先填充第一行的所有单元格，

# 然后填充第二行，依此类推。

# 起始单元格是(0,0)，也就是地图的左上角。

# 单元格(1, 0)紧接着(0,0)，是x方向上紧邻的下一个单元格。

# 而单元格(0, 1)则位于第一行的第二个位置，其索引等于地图的宽度（info.width），

# 然后才是(1, 1)单元格，即第二行的第二个单元格。

# 关于地图数据的值，它们根据具体的应用需求来定义。但在很多情况下，

# 会使用0表示该单元格是未占用的，即机器人可以安全通过；

# 1表示该单元格是确定占用的，即存在障碍物；

# 而-1表示该单元格的状态是未知的，即机器人尚未探测到该区域的状态。
int8[] data

Map Storage Format

In the Basic Map Saving Operations section, after the map is saved, two files are generated. These two files are used to store the serialized map data. Among them, my_map.pgm is an image resource that can be opened with an image viewer, while my_map.yaml stores the metadata information of the map, used to describe the image. The content format is as follows:

image: my_map.pgm
mode: trinary
resolution: 0.05
origin: [-0.955, -10.9, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

Parameter Explanation:

• image The path to the image resource being described, which can be either an absolute path or a relative path.
• resolution Image resolution (unit: m/pixel).
• The 2D pose of the bottom-left pixel in the origin map is (x, y, z), with yaw representing counterclockwise rotation (yaw = 0 indicates no rotation).
• occupied_thresh Pixels with an occupancy probability greater than this threshold are considered fully occupied.
• free_thresh Pixels with occupancy below this threshold are considered completely free.
• negate Should the semantics of white/black and free/occupied be inverted.
• mode Map mode: trinary (default), scale, or raw.

Read the map (deserialization)

Map Reading Node Description

In nav2_map_server, the map reading node is map_server. The relevant information for this node is as follows.

Published Topics

Parameters

• frame_id The name of the map coordinate system.
• topic_name Topic name.
• yaml_filename Map data source.

Basic Map Reading Operations

Preparation

Please first run the following command to create a package in the src directory of your workspace:

ros2 pkg create mycar_map_server --dependencies nav2_map_server

Under the function package, create a new launch folder and add the following configuration to CMakeLists.txt:

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

Read the Map

When reading a map using map_server, there are two common approaches: using terminal commands and integrating with a launch file. Both methods produce the same result and can publish map messages as topics.

colcon build
source ./install/setup.bash

Method 1: Terminal Commands

Please enter the workspace in the terminal and input the following command:

ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=map/my_map.yaml

Since map_server is a node with a lifecycle, it still needs to be configured and activated. Please open a new terminal and run the following command:

ros2 lifecycle set /map_server configure
ros2 lifecycle set /map_server activate

If execution completes without errors, call ros2 topic list to see the /map topic, indicating that the map message has been published.

Method 2: Launch Integration

Method 1 requires manually setting the map_server lifecycle, which makes the steps a bit cumbersome. Therefore, we can also integrate this node into a launch file to simplify the startup process. Create a new file named map_server.launch.py in the launch directory and enter the following content:

import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
  map_file = os.path.join('map', 'my_map.yaml')
  map_server_node = Node(
      package='nav2_map_server',
      executable='map_server',
      name='map_server',
      output='screen',
      parameters=[{'use_sim_time': True},
                  {'yaml_filename':map_file}]
  )
  manager_mapper_node = Node(
    package='nav2_lifecycle_manager',
    executable='lifecycle_manager',
    name='lifecycle_manager_mapper',
    output='screen',
    parameters=[{'use_sim_time': True},
      {'autostart': True},
      {'node_names': ['map_server']}]
  )
  return LaunchDescription([map_server_node,manager_mapper_node])

In this file, the lifecycle_manager component from the nav2_lifecycle_manager package is used. This component can automatically configure and activate the lifecycle-managed nodes it oversees. After building the package and executing this launch file, the final effect is similar to that of Method 1.

ros2 launch mycar_map_server map_server.launch.py

Display the map

Open rviz2, then add the Map plugin, set the topic to /map, and set the topic's Durability Policy.

Set the option to Transient Local, and the map data will display correctly.

AMCL Adaptive Monte Carlo Localization

Localization is the process by which a robot determines its own position on a known map, providing foundational information for robot navigation.

The localization technology in Nav2 is called AMCL, which stands for Adaptive Monte Carlo Localization. It is a localization algorithm based on a particle filter. It performs adaptive localization using the Monte Carlo method, determining the robot's position and orientation in the environment by sensing the surrounding environment and analyzing observation data. In Nav2, the corresponding functional package is nav2_amcl.

In AMCL, the core idea of the particle filter is to use a set of particles (samples) to represent the robot's possible positions on the map. Each particle has a weight that indicates the confidence level of the position it represents. The algorithm updates the positions and weights of these particles based on the robot's motion model and sensor data. Over time, the particles gradually converge near the robot's actual position, enabling an accurate estimate of the robot's location.

Location Node Description

The core node in the nav2_amcl package is amcl. The relevant information for this node is as follows.

1. Subscribed Topics

2. Published Topics

3. Published Services

4. Parameters

General Parameters

• bond_disable_heartbeat_timeout: When set to true, it disables the heartbeat-based timeout detection between the amcl node and other nodes. This is typically used when the connections between nodes are very stable and do not require frequent heartbeat checks to confirm connection status.
• base_frame_id: Defines the ID of the robot's base coordinate system, typically named base_link or a similar name.
• global_frame_id: Defines the ID of the global map coordinate system, typically map.
• odom_frame_id: Define the ID of the odometry coordinate system, typically odom.
• tf_broadcast: When set to true, the amcl node publishes the transform from the odometry frame to the global map frame.
• transform_tolerance: Set the tolerance for TF transforms to handle time inconsistencies in the TF tree.
• use_sim_time: When set to true, amcl will use ROS 2's simulated time (if available). This is useful in simulation environments.

Laser Model Parameters

• laser_model_type: Set the laser model type. likelihood_field is a commonly used model that accounts for the probability of a laser beam hitting an obstacle.
• laser_max_range and laser_min_range: Set the maximum and minimum detection range of the LiDAR respectively.
• laser_likelihood_max_dist: Set the maximum distance considered by the laser model; data beyond this distance will be ignored.
• do_beamskip and related parameters (beam_skip_distance, beam_skip_threshold, beam_skip_error_threshold): These parameters control whether to skip processing of certain laser beams to reduce computational load. However, do_beamskip is set to false, meaning no laser beams are skipped.

Particle Filter Parameters

• alpha1Toalpha5: These parameters are used to control the weight update process in the particle filter, but their specific roles may vary depending on the implementation of amcl. In the standard amcl implementation, these parameters may not be used directly.
• max_particles and min_particles: Set the maximum and minimum number of particles for the particle filter, respectively.
• resample_interval: Sets the number of filter updates needed before resampling.
• pf_err and pf_z: These parameters are used to control the performance of the particle filter, but their specific effects may depend on the implementation details of amcl.

Initial Pose Parameters

• initial_pose: Defines the robot's initial pose (x, y, yaw, z), but in practice, if set_initial_pose is set to true, this initial pose may be overwritten by the initial pose set via a service request.
• set_initial_pose: When set to true, it allows setting the robot's initial pose via a service request.
• always_reset_initial_pose: When set to false, it means the initial pose will not be automatically reset at the start of each localization session.

Other Parameters

• first_map_only: When set to false, it indicates that amcl will subscribe to and process the continuously updated map topic.
• map_topic: Define the name of the map topic, which AMCL will subscribe to in order to receive map information.
• scan_topic: Defines the name of the lidar scan data topic, which amcl will subscribe to in order to obtain data for localization.
• save_pose_rate: Set the rate (in Hz) at which the robot's pose is saved.
• recovery_alpha_fast and recovery_alpha_slow: These parameters may not be directly used in the standard amcl implementation; they might belong to a specific version of amcl or an extension.
• z_hit, z_rand, z_short, z_max, and sigma_hit: These parameters define the probability distribution in the laser model, used to calculate the probability of a laser beam hitting an obstacle or a random location.

Basic Operations for Positioning Nodes

1. Preparation

Please first run the following command to create a package in the src directory of your workspace:

ros2 pkg create mycar_localization --dependencies nav2_amcl mycar_map_server

2. Writing the launch file and parameter file

Under the function package, create new launch and params folders. In the launch directory, create a file named mycar_loca.launch.py and enter the following content:

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false'
    )

    amcl_yaml = os.path.join(
        get_package_share_directory('mycar_localization'),
        'params',
        'amcl.yaml'
    )

    amcl_node = Node(
        package='nav2_amcl',
        executable='amcl',
        name='amcl',
        output='screen',
        parameters=[
            amcl_yaml,
            {'use_sim_time': LaunchConfiguration('use_sim_time')} # 覆盖掉yaml里的use_sim_time参数,以launch启动的设置为准.
        ]
    )

    manager_localization_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_localization',
        output='screen',
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
            {'autostart': True},
            {'node_names': ['amcl']}
        ]
    )

    map_server_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('mycar_map_server'),
                'launch',
                'map_server.launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )

    return LaunchDescription([
        use_sim_time_arg,
        map_server_launch,
        amcl_node,
        manager_localization_node,
    ])

In the code above, a amcl node was created, and a configuration file named amcl.yaml was loaded from the params directory. Since amcl is also a node with a lifecycle, it was added to the lifecycle manager. Finally, localization must rely on map information, so the launch file from Basic Map Reading Operations was included to load the map.

Create a new file named amcl.yaml in the params directory, and enter the following content:

/**:
  ros__parameters:
    use_sim_time: True  # 由launch的设置覆盖
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 2.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
    set_initial_pose: false

For the specific meaning of the parameters, refer to the parameter-related content in the Localization Node Description.

3. Edit the configuration file

Open CMakeLists.txt and enter the following content:

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

4. Compilation

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select mycar_localization

5. Execution

(1) First, call the following command to start the simulation environment:

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_robot_world.launch.py

(2) Then, in the terminal, navigate to the current workspace and enter the following command:

. install/setup.bash
ros2 launch mycar_localization mycar_loca.launch.py use_sim_time:=True

(3) Start the keyboard control node as a backup:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

(4) In rviz2, set the Fixed Frame to map, add the TF plugin, and follow the basic map reading operations to add and display the map.

Next, click 2D Pose Estimate in the rviz2 menu bar to set an initial pose for the robot on the map.

Here, a rough estimate of the robot's position and orientation is sufficient; the robot will gradually calibrate itself through AMCL during motion.

First, click on 2D Pose Estimate, then left-click on the map at the robot's location. Hold down the left button without releasing it, and drag the mouse in the direction the robot is facing. A green arrow like the one below will appear. Then release the mouse button.

Add the ParticleCloud plugin, set the topic to /particle_cloud, and set Reliability Policy under the topic to Best Effort. Finally, when using the keyboard to control the robot's movement, you will notice that a point cloud appears around the robot, and as the robot moves, the point cloud will converge or diverge to varying degrees.

Even if your robot crashes into a wall and the robot positions in rviz2 and Gazebo are completely misaligned, simply let the robot move for a while, and its position will be re-estimated. AMCL is very powerful.

As shown in the image above, the position has completely shifted.

After driving for a period of time, the pose position was successfully estimated again.

Navigation Server

This section will introduce the core nodes in the navigation server, implement basic navigation functions, and integrate navigation with SLAM to achieve autonomous exploration and mapping.

Below is the official NAV2 documentation for each node's parameter descriptions (a very commonly used website for parameter tuning):

https://docs.nav2.org/configuration/index.html

If you come across English you don't understand, you can use the Chrome extension "Immersive Translation" to translate it. It provides side-by-side Chinese-English translations and is very useful.

https://chromewebstore.google.com/detail/immersive-translate-trans/bpoadfkcbjbfhfodiogcnhhhpibjhbnh?utm%5C_source=official&pli=1

https://microsoftedge.microsoft.com/addons/detail/immersive-translate-tra/amkbmndfnliijdhojkpoglbnaaahippg

If you don't know how to use this plugin, you can also check out the version translated by FishROS (though the website is unstable):

http://fishros.org/doc/nav2/configuration/index.html

Lifecycle Management Node Explanation

In Nav2, the nav2_lifecycle_manager package's lifecycle_manager node manages the lifecycle state transitions of other nodes. The lifecycle_manager node uses the ROS 2 Lifecycle Node mechanism to provide a standardized method for controlling state transitions of various nodes in Nav2. These states include Unconfigured, Inactive, Active, and Finalized. By precisely controlling these state transitions, the lifecycle_manager node ensures that each part of the Nav2 system starts, runs, and stops at the correct time, thereby improving system reliability and stability.

Parameters

• /bond_disable_heartbeat_timeout: This parameter is related to the communication bonding between nodes. Communication between nodes can be made more reliable through bonding, and the heartbeat timeout is a way to detect whether a node is still active. Setting it to false means enabling heartbeat timeout detection.
• attempt_respawn_reconnection: Takes a boolean value. When set to true, it indicates that if the managed node terminates unexpectedly, the lifecycle manager will attempt to restart it.
• autostart: Takes a boolean value. When set to true, it indicates that when the lifecycle manager starts, it will automatically attempt to launch the nodes it manages.
• bond_respawn_max_duration: Maximum duration when reconnecting or restarting a node.
• bond_timeout: Related to the communication binding timeout between nodes. If no message is received from another node within this time, that node is considered disconnected.
• diagnostic_updater:
  • period: The update cycle of the diagnostic updater. The diagnostic updater is used to collect and publish diagnostic information about the node's status, defining the frequency at which this information is updated.
• node_names: List of node names managed by this lifecycle manager.
• use_sim_time: Whether to use simulation practice.

Behavior Tree Node Explanation

Behavior Trees (BT) are a way to structure the switching between different tasks in an agent, such as a robot or a virtual entity in a computer game. It is a formalized graphical modeling language characterized by a hierarchical organization of nodes, used to describe and plan the interactions and decisions of various entities in complex systems. The /bt_navigator under the nav2_bt_navigator package is the behavior tree navigator node in Nav2, which implements a behavior tree-based navigation strategy. A behavior tree is a tree-like structure used to describe complex behaviors. By combining different behavior nodes, it can flexibly define the robot's navigation behaviors, including path planning, obstacle avoidance, recovery, and more. Below is the official website.

https://www.behaviortree.dev/

https://arxiv.org/abs/1709.00084

The GitHub link is as follows:

https://github.com/BehaviorTree/BehaviorTree.CPP

BT visualization tool Groot2 download:

https://www.behaviortree.dev/groot/

1. Subscribed topics

2. Requested Action

3. parameter

• use_sim_time: Specifies whether to use simulated time instead of real time.
• global_frame: Defines the name of the global coordinate system, typically the map coordinate system.
• robot_base_frame: Specify the coordinate system name for the robot base.
• odom_topic: The ROS topic name for odometry data.
• bt_loop_duration: Duration of the behavior tree execution loop (units may vary depending on the implementation).
• default_server_timeout: The default timeout for navigation server operations.
• enable_groot_monitoring: Enable or disable the Groot monitoring feature.
• groot_zmq_publisher_port: The port number of the ZeroMQ publisher in Groot monitoring.
• groot_zmq_server_port: The port number of the ZeroMQ server in Groot monitoring.
• plugin_lib_names: A list of library names containing the plugins needed for navigation.

Planner Node Description

In the Nav2 navigation framework, the planner_server node under the nav2_planner package is responsible for processing path planning requests and generating a path from the current position to the target position. This node depends on the map messages provided by the /global_costmap/global_costmap node during execution.

1. planner_server published topics

2. planner_server parameters

• /bond_disable_heartbeat_timeout: This parameter controls whether to disable heartbeat timeout detection. In ROS 2, the bonding mechanism between nodes is used to enhance communication reliability. Heartbeat timeout is a mechanism for detecting whether a node is still active. If this parameter is set to true, it means heartbeat timeout detection is disabled, which may be used in certain specific network environments or when communication between nodes is believed to be very stable.
• GridBased: This is a configuration block specifically used to set parameters for a grid-based planner. Grid-based planners typically use a grid representation of the map to plan paths.
  • allow_unknown: Controls whether the planner is allowed to plan paths in unknown (i.e., unexplored) areas of the map.
  • plugin: Specifies the planner plugin to use.
  • tolerance: Sets the tolerance when planning paths, typically used to account for robot size and localization uncertainty.
  • use_astar: Controls whether to use the A* algorithm for path planning*. The A* algorithm is a heuristic search algorithm that can find the shortest path from the start point to the end point.
  • use_final_approach_orientation: Controls whether the planner considers the robot's final orientation near the end of the path.
• expected_planner_frequency: This parameter represents the expected frequency at which the planner generates new paths. It helps the system monitor the planner's performance and can be used for debugging or performance optimization.
• planner_plugins: This is a list specifying the available planner plugins. In this example, it only contains one GridBased planner, but in theory it can include multiple planners of different types to accommodate various navigation needs.
• use_sim_time: This parameter controls whether to use simulated time. In a simulation environment, time is controlled by the simulation software, not by the actual physical clock. Setting this parameter to true allows nodes to run normally in the simulation environment without relying on the actual system time. This is very useful for developing and testing navigation algorithms.

3. The topic subscribed to by /global_costmap/global_costmap

4. The topic published by /global_costmap/global_costmap

5. /global_costmap/global_costmap parameters

• /bond_disable_heartbeat_timeout: Controls whether to disable heartbeat timeout detection, which is used to monitor the liveness of peer nodes during inter-node communication binding.
• always_send_full_costmap: Controls whether to always send the complete costmap information, rather than only sending the changed parts.
• clearable_layers: List the costmap layers that can be cleared, where obstacle information in these layers can be updated or cleared through some method (such as sensor data).
• filters: Defines the list of filters applied to the costmap for preprocessing or modifying map data.
• footprint: Specifies the footprint shape of the robot on the map, i.e., the spatial area the robot occupies.
• footprint_padding: Add extra padding space to the robot's footprint to account for additional space requirements during robot movement.
• global_frame: Defines the reference coordinate system used by the global costmap.
• height and width: Define the height and width of the global costmap in number of cells.
• inflation_layer: Configure the parameters of the inflation layer, which is used to add a "buffer zone" of a certain width around obstacles to prevent the robot from getting too close to them.
  • cost_scaling_factor: Scaling factor for the expansion cost.
  • enabled: Controls whether the inflation layer is enabled.
  • inflate_around_unknown: Controls whether to inflate around unknown space.
  • inflate_unknown: Controls whether unknown space is treated as an obstacle and inflated.
  • inflation_radius: The radius of expansion.
  • plugin: Specifies the inflation layer plugin to use.
• lethal_cost_threshold: Defines the cost threshold in the costmap for considering an obstacle as "lethal".
• map_topic: Specify the ROS topic to subscribe to for map information.
• observation_sources: Define the observation sources for the costmap, i.e., which sensors or data sources are used to update the map.
• obstacle_layer: Configure the parameters of the obstacle layer, which is responsible for processing obstacle information observed by sensors.
  • combination_method: Defines how to combine information from multiple observation sources.
  • enabled: Controls whether the obstacle layer is enabled.
  • footprint_clearing_enabled: Controls whether to clear obstacle information within the robot's footprint.
  • max_obstacle_height and min_obstacle_height: Define the height range of obstacles.
  • observation_sources: Specifies the source of obstacle information.
  • plugin: Specifies the obstacle layer plugin to use.
  • scan: Contains configuration related to laser scanning, such as how scan data is processed.
• origin_x and origin_y: Define the coordinates of the global costmap origin.
• plugins: List the enabled costmap plugins, which define how to build and update the costmap.
• publish_frequency: Defines the frequency of publishing the costmap.
• resolution: Defines the resolution of the costmap, which is the actual physical size represented by each cell.
• robot_base_frame: Define the reference coordinate system for the robot base.
• robot_radius: Defines the robot's radius, used to calculate the robot's footprint on the map.
• rolling_window: Controls whether to use a scrolling window (i.e., a dynamically changing map area) instead of a fixed-size map.
• static_layer: Configure the parameters of the static layer, which is responsible for processing static obstacle information in the map.
  • enabled: Controls whether the static layer is enabled.
  • map_subscribe_transient_local: Controls whether to subscribe to transient local map updates.
  • map_topic: Specify the ROS topic for the static map (if different from the global map).
  • plugin: Specifies the static layer plugin to use.
  • subscribe_to_updates: Controls whether to subscribe to map updates.
  • transform_tolerance: Define the tolerance for coordinate transformation.
• track_unknown_space: Controls whether to track unknown space in the map.
• transform_tolerance: Defines the allowable error range during coordinate transformation.
• trinary_costmap: Controls whether to use a three-state costmap (free, occupied, unknown) instead of only two states (free, occupied).
• unknown_cost_value: Value representing unknown space in the costmap.
• update_frequency: Defines the frequency at which the costmap is updated.
• use_maximum: Controls whether to use the maximum value from multiple observation sources to update the costmap.
• use_sim_time: Controls whether to use simulation time (useful in simulation environments).
• voxel_layer: Configure the parameters of the voxel layer. The voxel layer uses a voxel grid to represent obstacle information in three-dimensional space.
  • enabled: Controls whether the voxel layer is enabled.
  • footprint_clearing_enabled: Controls whether to clear voxel information within the robot's footprint.
  • mark_threshold and unknown_threshold: Define the thresholds for treating voxels as obstacles or unknown.
  • max_obstacle_height and min_obstacle_height: Define the height range of obstacles represented by voxels.
  • observation_sources: Specifies the source of voxel information.
  • origin_z: Defines the origin of the voxel grid on the Z-axis.
  • plugin: Specifies the voxel layer plugin to use.
  • publish_voxel_map: Controls whether to publish the voxel map.
  • scan: Contains configuration related to laser scanning, such as how scan data is processed.
  • z_resolution and z_voxels: Define the resolution and number of voxels in the Z-axis for the voxel grid.

Controller Node Description

In the Nav2 navigation system, the nav2_controller function package's controller_server is responsible for handling control requests during navigation tasks, ensuring the robot can move along the planned path. Its main function is to generate speed and direction control commands based on the global or local path calculated by the nav2_planner module, i.e., controlling the robot to follow the planned path. When executing, this node also depends on the map messages provided by the /local_costmap/local_costmap node.

Topics subscribed to by controller_server

Topics published by controller_server

3. controller_server parameters

• FollowPath: This section defines a plugin or configuration set named FollowPath, which may be a configuration for a path-following behavior or algorithm. It contains multiple sub-parameters and sub-configurations used to define how to follow the path.
  • BaseObstacle: Defines basic obstacle evaluation parameters, used to avoid obstacles during path following.
    • class: specifies the name of the class, which here is BaseObstacle, indicating this is a basic obstacle evaluation component.
    • scale: Defines the weight or influence of this obstacle assessment within the overall evaluation.
    • sum_scores: Indicates whether to accumulate scores from multiple obstacles; false may mean using the maximum value or other logic.
  • GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal, Oscillation: These are different evaluation or behavior components during the path following process, each with its own specific parameters and purposes, such as aligning with the goal, maintaining distance from the goal or path, reducing oscillation, and so on.
  • acc_lim_theta, acc_lim_x, acc_lim_y: These parameters define the acceleration limits of the robot in different directions.
  • critics: specifies which evaluation components (or "critics") will be used for path following decisions.
  • debug_trajectory_details: Indicates whether to publish detailed debugging information for the trajectory.
  • Other parameters related to speed, acceleration, time granularity, trajectory generation, and so on collectively define the behavior and performance of the path following algorithm.
• controller_frequency: Specifies the operating frequency of the controller (which may be FollowPath or another controller), in Hertz.
• controller_plugins: Specifies the list of controller plugins to be used; here, it only includes FollowPath.
• failure_tolerance: Defines the time or distance allowed for tolerating failure, providing a buffer when evaluating whether the controller has failed.
• general_goal_checker: Defines a general goal checker used to determine whether the robot has reached its target position and orientation.
• goal_checker_plugins: Specifies the list of goal checker plugins to be used.
• min_theta_velocity_threshold, min_x_velocity_threshold, min_y_velocity_threshold: These define the minimum velocity thresholds for the robot in different directions; values below these thresholds may be considered as stopped or stationary.
• odom_topic: Specifies the ROS topic for odometry information.
• progress_checker: Defines a progress checker used to evaluate whether the robot is moving toward the target.
• qos_overrides: Defines QoS (Quality of Service) override settings for ROS services or topics, used to adjust the reliability and performance of message delivery.
• speed_limit_topic: Specifies the ROS topic for speed limit information.
• use_sim_time: Indicates whether to use simulated time, which is very useful in the ROS simulation environment.

4. Topics subscribed to by /local_costmap/local_costmap

5. Topics published by /local_costmap/local_costmap

6. /local_costmap/local_costmap parameters

• /bond_disable_heartbeat_timeout: Whether to disable heartbeat timeout checks between nodes. When set to true, it means this feature is disabled, which may be used to reduce network traffic or adapt to specific network environments.
• always_send_full_costmap: Whether to always send the complete costmap. When set to true, the node will not rely on incremental updates, but will always send the full costmap data.
• clearable_layers: Specifies the list of layers that can be cleared. In this example, it includes obstacle_layer, voxel_layer, and range_layer, meaning obstacle data in these layers can be cleared.
• filters: Used to specify the list of filters applied to the costmap. It is empty here, indicating that no filters are applied.
• footprint: The robot's footprint polygon defines the physical area the robot occupies in two-dimensional space.
• footprint_padding: The footprint padding, used to add extra space around the robot's footprint when calculating the cost map.
• global_frame: The name of the global reference coordinate system, typically used for localization and navigation tasks.
• height: The height of the cost map (in number of cells).
• inflation_layer: Configuration of the inflation layer, used to add a certain range of inflated area around obstacles, keeping the robot at a safe distance from obstacles.
• lethal_cost_threshold: Fatal cost threshold. Cost values exceeding this threshold represent impassable areas.
• map_topic: The name of the subscribed map topic, used to obtain global map information.
• observation_sources: The configuration of the observation source specifies which sensor data will be used to update the cost map. Here it is an empty string, which may be due to a default value or a different configuration method.
• obstacle_layer: Configuration of the obstacle layer, used to process obstacle data from sensors such as LiDAR.
• origin_x, origin_y: The X and Y coordinates of the costmap origin, defining the costmap's position in the global coordinate system.
• plugins: List of enabled plugins, defining the different layers used by the costmap (such as obstacle layer, inflation layer, etc.).
• publish_frequency: The publishing frequency of the cost map (in Hz).
• resolution: The resolution of the costmap (in meters per cell).
• robot_base_frame: The name of the reference coordinate system for the robot base, used to locate the robot.
• robot_radius: The robot's radius, used to represent its physical size in the costmap.
• rolling_window: Whether to use a rolling window. When set to true, the cost map will update its position and range as the robot moves.
• track_unknown_space: Whether to track unknown space. In some cases, this may be used to handle unexplored or unknown areas.
• transform_tolerance: Transformation tolerance, which defines the thresholds for acceptable time difference and angle difference in a transformation.
• trinary_costmap: Whether to use a three-state cost map (typically free, occupied, unknown).
• unknown_cost_value: Cost value of unknown areas in the costmap.
• update_frequency: The update frequency of the costmap (in Hz), which is different from the publish frequency.
• use_maximum: Whether to use the maximum value when cost information for the same location is provided by multiple sources.
• use_sim_time: Whether to use simulated time instead of system time. This is useful in simulation environments.
• voxel_layer: Configuration of the voxel layer, used to divide three-dimensional space into voxels (volume pixels) to improve the processing efficiency of the cost map.

Recoverer Node Description

Recovery behavior is a crucial part of the robot navigation process, allowing the robot to take a series of predefined actions to attempt recovery when encountering obstacles, getting stuck, or facing other navigation issues. In Nav2, this functionality is implemented by the nav2_behaviors feature package's behavior_server.

1. Subscribed Topics

2. Published Topics

3. Provided Action Server

4. Parameters

• use_sim_time: This parameter specifies whether to use simulated time instead of real time. This is very useful in simulation environments, where time can be sped up or slowed down without waiting for real time to pass.
• global_frame: Defines the name of the global coordinate system, which is typically used for localization and path planning in navigation tasks. Here, it is set to odom, meaning that odometry data is used as the reference for the global coordinate system.
• robot_base_frame: Specifies the coordinate system name for the robot base. This is the reference point on the robot used for positioning and motion control, typically corresponding to the robot's physical center or the center of its drive wheels.
• odom_topic: This is a ROS topic name used to publish odometry data. Odometry data contains information about the robot's position and orientation changes over time, and is one of the key inputs for navigation and localization systems.
• /bond_disable_heartbeat_timeout: This parameter may be used to configure the heartbeat detection mechanism between ROS nodes. Setting it to true likely means disabling or adjusting the heartbeat timeout behavior to avoid unnecessary timeout errors in specific scenarios, such as simulation environments.
• assisted_teleop, backup, drive_on_heading, spin, wait: These are configuration items for behavior plugins that may be used in a behavior tree. Each plugin defines a specific behavior the robot can execute, such as assisted teleoperation, moving backward, driving in a specified direction, rotating in place, and waiting.
• behavior_plugins: Lists the names of available behavior plugins in the behavior tree.
• cmd_vel_teleop: Specifies the ROS topic name for velocity control commands used in teleoperation.
• costmap_topic: Defines the ROS topic name for the local costmap, which is used to represent obstacles and traversable areas in the environment.
• cycle_frequency: Defines the frequency (in Hertz) at which the navigation system updates its state and plans new paths.
• max_rotational_vel, min_rotational_vel, rotational_acc_lim: These parameters define the maximum speed, minimum speed, and acceleration limits when the robot rotates.
• projection_time: Time parameters related to updating the costmap or predicting future obstacle positions.
• footprint_topic: Defines the ROS topic name for publishing the robot's footprint (i.e., the space occupied by the robot).
• simulate_ahead_time, simulation_time_step: These parameters are related to the simulation environment and may be used to control the passage of time and step size during the simulation process.
• transform_tolerance: Defines the tolerance range for coordinate transformations, used to handle minor discrepancies between different coordinate systems.

Waypoint Following Node Description

In the Nav2 navigation stack, the nav2_waypoint_follower package's /waypoint_follower node is responsible for tracking a sequence of waypoints generated by the path planner, ensuring the robot moves safely and accurately along the planned path. The main function of this node is to compute control commands to guide the robot's motion based on the current robot position and velocity, as well as the list of waypoints provided by the path planner (such as nav2_global_planner and nav2_local_planner). These control commands may include linear and angular velocity commands, or more specific kinematic or dynamic commands, depending on the robot's type and configuration.

1. Provided Action Server

2. Requested Action Service

3. Parameters

• use_sim_time: Specifies whether to use simulated time instead of real time for node timing and synchronization. This is particularly useful in simulation environments, where the clock may not be fully synchronized with real time.
• loop_rate: Defines the main loop rate of the node, which is the number of times per second the node performs its primary tasks (such as processing data, publishing information, etc.). This parameter is crucial for controlling the node's responsiveness and resource usage.
• stop_on_failure: Indicates whether the node should stop execution when the navigation task encounters an insurmountable obstacle or reaches other failure conditions. This is important for ensuring the system can safely stop and wait for further instructions in the event of a failure.
• bond_disable_heartbeat_timeout: A reliability mechanism for inter-node communication. Bond is a mechanism in ROS 2 for stable communication between nodes, where a heartbeat signal is used to detect whether a node is still active. Setting this parameter to true disables heartbeat timeout detection, which may be useful in certain network configurations or application scenarios.
• waypoint_task_executor_plugin: Specifies the plugin to use when executing a waypoint navigation task. Waypoint navigation typically involves a series of predefined points that the robot must visit in sequence. This parameter allows the user to specify a specific plugin or algorithm for performing this type of task.
• wait_at_waypoint: This is a compound parameter used to configure specific behavior when waiting at a waypoint.
  • enabled: Enable or disable the function of waiting when reaching each waypoint.
  • plugin: Specifies the plugin type that implements the wait functionality. This allows users to select different waiting strategies or behaviors as needed.
  • waypoint_pause_duration: Defines the duration (in milliseconds) to wait at each waypoint. This can be used to ensure the robot has stabilized or completed certain operations before moving to the next waypoint.

Path Smoothing Node Description

In the Nav2 framework, the smoother_server node under the nav2_smoother package smooths planned paths by loading and running various smoother plugins, enabling the robot to move more smoothly, continuously, and safely. This feature is significant for improving the robot's navigation performance and reducing hardware wear.

1. Subscribed Topics

2. Published Topics

3. Provided Action Server

4. Parameters

• /bond_disable_heartbeat_timeout: Indicates whether to disable Bond's heartbeat timeout feature. In distributed systems, Bond is used to manage connections and heartbeats between nodes; this parameter adjusts heartbeat-related behavior.
• costmap_topic: The ROS topic name for costmap data, typically the raw data of the global costmap.
• footprint_topic: The ROS topic name for the robot's footprint (i.e., the area the robot occupies on the map), used to represent the robot's physical dimensions in the global costmap.
• robot_base_frame: Specifies the coordinate frame name of the robot base, which serves as the reference point for localization and movement in robot navigation.
• simple_smoother:
  • do_refinement: Indicates whether to enable the refinement (or further optimization) process for the path.
  • max_its: The maximum number of iterations allowed during the smoothing process, used to control the convergence time of the smoothing algorithm.
  • plugin: The type of smoothing plugin, here it is nav2_smoother::SimpleSmoother, indicating the use of a simple smoothing algorithm.
  • tolerance: The convergence tolerance of the smoothing algorithm; when the path change is less than this value, the smoothing process is considered complete.
  • w_data: The weight of data items (such as obstacle distance) during the smoothing process.
  • w_smooth: The weight of smoothing terms (such as path curvature) during the smoothing process.
• smoother_plugins: The defined list of smoothing plugins, which lists simple_smoother, indicates that this plugin will be used for path smoothing.
• transform_tolerance: Tolerance for coordinate transformation, used to handle uncertainty when converting between different coordinate systems.
• use_sim_time: Specifies whether to use simulated time instead of real time. In a simulation environment, this is typically set to true to match the simulator's virtual time; in a real environment, it should be set to false to use the actual time of the ROS system.

Speed Smoothing Node Description

The nav2_velocity_smoother node under the velocity_smoother package in the Nav2 framework is primarily responsible for smoothing the velocity commands sent by the Nav2 framework to the robot controller. Its core functionality is to achieve velocity and acceleration smoothing. This feature is crucial for ensuring the stability and safety of the robot during navigation.

1. Subscribed Topics

2. Published Topics

3. Parameters

• bond_disable_heartbeat_timeout: Indicates whether to disable the heartbeat timeout mechanism between nodes. If set to true, the heartbeat detection between nodes will not disconnect due to timeout.
• deadband_velocity: Defines which velocity components to apply deadband smoothing to (i.e., ignore small velocity changes below this threshold). Here, deadband values are set for the X-axis, Y-axis, and yaw angular velocity (theta) respectively.
• feedback: Specifies the feedback type of the speed smoother. OPEN_LOOP indicates open-loop control, meaning speed adjustments are made without considering the robot's actual speed feedback.
• max_accel: Defines the maximum acceleration limits for the robot in each direction, including the X-axis, Y-axis, and yaw angular velocity (theta).
• max_decel: Defines the maximum deceleration limits for the robot in each direction, including the X-axis, Y-axis, and yaw angular velocity (theta). Note that deceleration values are expressed as negative numbers.
• max_velocity: Defines the maximum speed limits for the robot in each direction, including the X-axis, Y-axis, and yaw angular velocity (theta).
• min_velocity: Defines the minimum speed limits for the robot in each direction, including the X-axis, Y-axis, and yaw angular velocity (theta). This is typically used to avoid sending excessively small speed commands to the low-level controller.
• odom_duration: Parameters related to odometry data, but may not be directly used by the velocity_smoother node in this context; possibly legacy or associated with other functionality.
• odom_topic: Specifies the ROS topic name for odometry data. The velocity_smoother node will subscribe to this topic to obtain the robot's motion information.
• scale_velocities: Indicates whether to proportionally adjust other velocity components based on acceleration limits. If set to false, no velocity scaling will be performed.
• smoothing_frequency: Defines the execution frequency (Hz) of the velocity smoothing operation, i.e., how many smoothing calculations are performed per second.
• use_sim_time: Specifies whether to use simulated time instead of actual system time. This is especially useful in simulation environments to ensure time consistency and predictability.
• velocity_timeout: If no new speed command is received within a specified time, the velocity_smoother node will stop publishing speed commands and may send a zero-speed command to stop the robot's motion. This is to prevent the robot from continuing to move when speed control is lost.

Navigation Function Integration (ROS2 Humble and earlier)

1. Preparation

Please first run the following command to create a package in the src directory of your workspace:

ros2 pkg create mycar_navigation2 --dependencies navigation2 nav2_common

2. Writing the launch file and parameter file

Under the function package, create the launch, params, and bts directories.

Create a new file named nav2.launch.py in the launch directory and enter the following content:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile

from nav2_common.launch import RewrittenYaml


def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false'
    )

    use_sim_time = LaunchConfiguration('use_sim_time')

    current_pkg = get_package_share_directory("mycar_navigation2")

    bt_params_file = os.path.join(current_pkg, "params", "bt.yaml")
    planner_params_file = os.path.join(current_pkg, "params", "planner.yaml")
    controller_params_file = os.path.join(current_pkg, "params", "controller.yaml")
    behavior_params_file = os.path.join(current_pkg, "params", "behavior.yaml")
    waypoint_params_file = os.path.join(current_pkg, "params", "waypoint.yaml")
    velocity_params_file = os.path.join(current_pkg, "params", "velocity.yaml")
    smoother_params_file = os.path.join(current_pkg, "params", "smoother.yaml")

    def rewrite_params(params_file):
        return ParameterFile(
            RewrittenYaml(
                source_file=params_file,
                root_key='',
                param_rewrites={
                    'use_sim_time': use_sim_time
                },
                convert_types=True
            ),
            allow_substs=True
        )

    bt_params = rewrite_params(bt_params_file)
    planner_params = rewrite_params(planner_params_file)
    controller_params = rewrite_params(controller_params_file)
    behavior_params = rewrite_params(behavior_params_file)
    waypoint_params = rewrite_params(waypoint_params_file)
    velocity_params = rewrite_params(velocity_params_file)
    smoother_params = rewrite_params(smoother_params_file)

    planner_server_node = Node(
        package='nav2_planner',
        executable='planner_server',
        name='planner_server',
        output='screen',
        parameters=[
            planner_params,
            {'use_sim_time': use_sim_time}
        ],
    )

    controller_server_node = Node(
        package='nav2_controller',
        executable='controller_server',
        name='controller_server',
        output='screen',
        parameters=[
            controller_params,
            {'use_sim_time': use_sim_time}
        ],
        remappings=[
            ('cmd_vel', 'cmd_vel_nav')
        ]
    )

    behavior_server_node = Node(
        package='nav2_behaviors',
        executable='behavior_server',
        name='behavior_server',
        output='screen',
        parameters=[
            behavior_params,
            {'use_sim_time': use_sim_time}
        ]
    )

    waypoint_node = Node(
        package='nav2_waypoint_follower',
        executable='waypoint_follower',
        name='waypoint_follower',
        output='screen',
        parameters=[
            waypoint_params,
            {'use_sim_time': use_sim_time}
        ]
    )

    velocity_smoother_node = Node(
        package='nav2_velocity_smoother',
        executable='velocity_smoother',
        name='velocity_smoother',
        output='screen',
        respawn_delay=2.0,
        parameters=[
            velocity_params,
            {'use_sim_time': use_sim_time}
        ],
        remappings=[
            ('cmd_vel', 'cmd_vel_nav'),
            ('cmd_vel_smoothed', 'cmd_vel')
        ]
    )

    smoother_server_node = Node(
        package='nav2_smoother',
        executable='smoother_server',
        name='smoother_server',
        output='screen',
        parameters=[
            smoother_params,
            {'use_sim_time': use_sim_time}
        ],
    )

    bt_navigator_node = Node(
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',
        parameters=[
            bt_params,
            {'use_sim_time': use_sim_time},
            {
                "default_nav_to_pose_bt_xml": os.path.join(
                    current_pkg,
                    "bts",
                    "bt_planner_controller_behavior.xml"
                )
            },
            {
                "default_nav_through_poses_bt_xml": os.path.join(
                    current_pkg,
                    "bts",
                    "bt_planner_controller_behavior_poses.xml"
                )
            }
        ],
    )

    lifecycle_manager_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_navigation',
        output='screen',
        parameters=[
            {'use_sim_time': use_sim_time},
            {'autostart': True},
            {
                'node_names': [
                    'bt_navigator',
                    'planner_server',
                    'controller_server',
                    'behavior_server',
                    'waypoint_follower',
                    'velocity_smoother',
                    'smoother_server'
                ]
            }
        ]
    )

    return LaunchDescription([
        use_sim_time_arg,

        lifecycle_manager_node,
        bt_navigator_node,
        planner_server_node,
        controller_server_node,
        behavior_server_node,
        waypoint_node,
        velocity_smoother_node,
        smoother_server_node
    ])

This launch file primarily loads nodes for the lifecycle manager, behavior tree, planner, controller, recovery, waypoint tracking, path smoothing, and velocity smoothing. Additionally, except for the lifecycle manager node, each node also loads a configuration file. Next, these configuration files need to be edited.

(1) bt_navigator related configuration files

There are two configuration files related to bt_navigator: an XML file describing the behavior tree, stored in the bts directory, and a YAML format parameter file, stored in the params directory.

In the bts directory, create a new file named bt_planner_controller_behavior.xml and enter the following content:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

Continue by creating a new file named bt_planner_controller_behavior_poses.xml in the bts directory, and enter the following content:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
            </ReactiveSequence>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

In the params directory, create a new file named bt.yaml and enter the following content:

/**:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

(2) Configuration files related to planner_server

In the params directory, create a new file named planner.yaml and enter the following content:

/**:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

/**:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True

      # robot_radius: 0.2
      footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.55
      always_send_full_costmap: False

(3) Controller server related configuration files

In the params directory, create a new file named controller.yaml and enter the following content:

/**:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001

    # failure_tolerance: 0.3
    failure_tolerance: 1.0
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25

    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0

      # max_vel_x: 0.15
      max_vel_x: 0.2
      max_vel_y: 0.0

      # max_vel_theta: 1.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.2
      min_speed_theta: 0.0

      # Add high threshold velocity for turtlebot 3 issue.

      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 1.0
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -1.0
      decel_lim_y: 0.0
      decel_lim_theta: -3.2

      # vx_samples: 20
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025

      # transform_tolerance: 0.2
      transform_tolerance: 1.0
      xy_goal_tolerance: 0.15
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
/**:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: True
      width: 2
      height: 2
      resolution: 0.05

      # robot_radius: 0.20
      footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.5
        cost_scaling_factor: 4.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True

Footprint

• Convert millimeters to meters: length 0.96m, width 0.45m
• Using the robot center (origin of the base_link coordinate system) as the reference, calculate the coordinates of the rectangle's vertices:

Note: Vertices are arranged in clockwise or counterclockwise order, covering the robot's length and width boundaries. (Right-hand coordinate system)

For example, length 960, width 450, length 400

footprint: [[0.48, 0.225], [0.48, -0.225], [-0.48, -0.225], [-0.48, 0.225]]

(4) Configuration files related to behavior_server

In the params directory, create a new file named behavior.yaml and enter the following content:

/**:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 5.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    use_sim_time: True
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

(5) Waypoint Follower Related Configuration Files

In the params directory, create a new file named waypoint.yaml and enter the following content:

/**:
  ros__parameters:
    use_sim_time: True
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 5000

(6) velocity_smoother related configuration files

In the params directory, create a new file named velocity.yaml and enter the following content:

/**:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.1, 0.0, 1.0]
    min_velocity: [-0.1, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

(7) Configuration files related to smoother_server

In the params directory, create a new file named smootherr.yaml and enter the following content:

/**:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    robot_base_frame: base_link
    transform_timeout: 0.1
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      do_refinement: True

3. Launch Integration

When implementing navigation, it relies on map and localization functions. We can integrate the previous localization launch file and the currently written navigation core module launch file into a single launch file to simplify the startup of navigation functionality. Create a new launch file named bringup.launch.py in the launch directory and enter the following content:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false'
    )

    amcl_pkg = get_package_share_directory("mycar_localization")
    nav2_pkg = get_package_share_directory("mycar_navigation2")

    amcl_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                amcl_pkg,
                'launch',
                'mycar_loca.launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                nav2_pkg,
                'launch',
                'nav2.launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': LaunchConfiguration('use_sim_time')
        }.items()
    )

    return LaunchDescription([
        use_sim_time_arg,
        amcl_launch,
        nav2_launch
    ])

4. Edit Configuration File

Open CMakeLists.txt and enter the following content:

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

5. Compilation

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select mycar_navigation2

6. Execution

(1) First, call the following command to start the simulation environment:

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_robot_world.launch.py

(2) Then, in the terminal, navigate to the current workspace and enter the following command to start the navigation function:

. install/setup.bash
ros2 launch mycar_navigation2 bringup.launch.py use_sim_time:=True

(3) Start rviz2, load the nav2_default_view.rviz file under /opt/ros/humble/share/nav2_bringup/rviz, set the initial pose for the robot, then set the target point through Nav2 Goal in the menu bar, and the robot will automatically navigate to the target point.

rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

• rviz2: Launch the rviz2 tool.
• -d: Specifies the .rviz configuration file to load.
• /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz: Path to the .rviz configuration file.

After running this command, rviz2 will start and load the nav2_default_view.rviz configuration file.

First use

As shown in the image above, the left image is for selecting the initial position, and the right image is for selecting the target position.

In the image above, the glowing rings around each obstacle and wall indicate the level of danger — the redder the color, the more dangerous it is for the robot to pass through that area.

Autonomous Exploration SLAM

Navigation depends on maps and localization. For example, in the previous section Navigation Function Integration, the navigation implementation integrated the localization functionality from the Localization AMCL section via a launch file. Similarly, during robot SLAM, map data and localization information are published. Therefore, navigation can also be combined directly with SLAM instead of relying on AMCL, achieving an autonomous exploration SLAM effect.

1. Write the launch file

In the launch directory of the mycar_navigation2 package, create a new launch file named auto_slam.launch.py and enter the following content:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression


def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false'
    )

    slam_backend_arg = DeclareLaunchArgument(
        'slam_backend',
        default_value='cartographer',
        description='SLAM backend: slam_toolbox or cartographer'
    )

    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_backend = LaunchConfiguration('slam_backend')

    slam_toolbox_pkg = get_package_share_directory("mycar_slam_slam_toolbox")
    cartographer_pkg = get_package_share_directory("mycar_slam_cartographer")
    # 从这开始修改: Jazzy auto_slam 引用 mycar_navigation2_jazzy，而不是 Humble 版本包
    nav2_pkg = get_package_share_directory("mycar_navigation2_jazzy")
    # 从这结束

    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                slam_toolbox_pkg,
                'launch',
                'online_sync_launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': use_sim_time
        }.items(),
        condition=IfCondition(
            PythonExpression(["'", slam_backend, "' == 'slam_toolbox'"])
        )
    )

    cartographer_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                cartographer_pkg,
                'launch',
                'cartographer.launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': use_sim_time
        }.items(),
        condition=IfCondition(
            PythonExpression(["'", slam_backend, "' == 'cartographer'"])
        )
    )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                nav2_pkg,
                'launch',
                'nav2.launch.py'
            )
        ),
        launch_arguments={
            'use_sim_time': use_sim_time
        }.items()
    )

    ld = LaunchDescription()
    ld.add_action(use_sim_time_arg)
    ld.add_action(slam_backend_arg)
    ld.add_action(slam_toolbox_launch)
    ld.add_action(cartographer_launch)
    ld.add_action(nav2_launch)
    return ld

2. Compilation

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select mycar_navigation2

3. Execution

(1) First, call the following command to start the simulation environment:

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_robot_world.launch.py

(2) Then, in the terminal, navigate to the current workspace and enter the following command to start the autonomous SLAM function:

. install/setup.bash
ros2 launch mycar_navigation2 auto_slam.launch.py use_sim_time:=True slam_backend:=cartographer

(3) Start rviz2, load the /opt/ros/humble/share/nav2_bringup/rviz file under nav2_default_view.rviz, then set the target point through Nav2 Goal in the menu bar. The robot will automatically navigate to the target point and also perform mapping during navigation.

rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

Multi-vehicle formation

Multi-vehicle platooning technology enables a group of vehicles to operate cooperatively without human intervention through advanced sensors, real-time data analysis, and highly intelligent control systems. As an advanced application of intelligent transportation systems, it has demonstrated significant potential and a wide range of use cases across multiple fields. For example:

Time optimization: Multi-vehicle formation technology eliminates errors and delays in manual operations, reducing labor costs and operational risks.

Space optimization: Multi-vehicle platooning technology can reduce the gaps between vehicles, maximizing road utilization.

Performance optimization: Dense formation driving reduces aerodynamic drag, lowering energy and fuel consumption.

In summary, multi-vehicle formation technology plays a unique role and holds significant value in the field of robotics. This section will introduce how to implement multi-vehicle formation in ROS2.