Humble Navigation Simulation

1.Navigation Overview

1.1.Navigation Overview

Concept

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

Nav2, also known as Navigation2, inherits from the ROS navigation stack. It adopts the same cutting-edge technologies as autonomous vehicles and has been optimized and adapted specifically for the navigation needs of mobile robots and ground robots. This project empowers mobile robots with the ability to traverse complex environments, enabling them to complete various user-defined application tasks, and is applicable to almost any type of robot dynamics. Nav2 not only supports robot movement from point A to point B, but can also set intermediate poses and execute multiple tasks such as object tracking, full-coverage navigation, and more. As a production-grade, high-quality navigation framework trusted by more than 100 companies worldwide, Nav2 has earned widespread acclaim for its exceptional reliability and stability.

Function

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

• Nav2 provides diverse navigation capabilities, supports input from various sensors such as LiDAR and cameras, acquires environmental information in real time, and enables intelligent path planning and precise motion control. It supports multiple navigation modes to meet different application requirements and offers strong extensibility, allowing seamless integration with ROS 2 components.

• Nav2 delivers excellent performance, employing efficient algorithms and data processing technologies to ensure rapid and stable responses during robot navigation.

• In terms of safety, Nav2 uses multiple obstacle avoidance algorithms and parameter adjustment functions, sets safety zones, and provides diagnostic monitoring to ensure safe robot navigation.

In summary, Nav2 provides strong support for robot navigation and applications. Whether in industrial automated production or service-oriented intelligent robots, Nav2 can play an important role, enhancing the autonomy and intelligence level of robots.

1.2.Navigation Installation

With the help of Ubuntu's package manager, Nav2 can be installed in binary form. The installation command is as follows:

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

in the instruction<ros2-distro>Please replace it with the currently used ROS2 version.

1.3.Navigation Conditions

To deeply learn Nav2, it is necessary to understand ROS2 basic robot knowledge and to have a simulation or physical robot as a practice environment.

ROS2basics

• ROS2 communication : Understand the basic concepts in ROS2, such as Nodes, Topics, Services, and Actions, and how to implement communication between nodes through these mechanisms.

• Lifecycle management : Be familiar with Lifecycle Nodes in ROS2 and understand the state transition processes of node startup, configuration, activation, deactivation, and cleanup. This is crucial for managing complex ROS2 systems.

• rviz2: Be 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. Be familiar with how to write and modify URDF files so that the robot structure can be accurately represented in simulation or a real environment.

• TF coordinate transforms: Master the use of the TF (Transform) library and understand the importance of coordinate transforms in robot navigation. Learn how to set and query transform relationships between different coordinate frames to ensure that the robot can accurately localize itself and its surrounding environment.

Practice environment

• Simulation environment: Set up a simulation environment compatible with ROS2, 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, provide odometry feedback, publish sensor data, and TF coordinate transforms, etc.

• Physical robot: If conditions permit, prepare a physical robot. This robot should have similar functions to those in the simulation environment, namely the ability to receive velocity commands, provide odometry feedback, publish sensor data such as LiDAR, and publish TF coordinate transforms. At the same time, ensure that the physical robot has the ROS2 system installed and the corresponding drivers and algorithm libraries configured.

• Nav2 software package: Install and configure the Nav2 software package and its dependencies. Nav2 is a comprehensive software package for robot navigation in ROS2. It includes multiple functional modules such as path planning, obstacle avoidance, and local and global map maintenance. Ensure that the Nav2 software package is compatible with your ROS2 version and has been properly configured as needed.

By mastering the above foundational knowledge and preparing a practical environment, you will be better able to learn and apply Nav2 for the development and debugging of robot navigation tasks.

1.4.Navigation Parameter Reference

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

2.SLAM Localization and Mapping

Concept

SLAM (Simultaneous Localization and Mapping) is a key technology in robotics and autonomous navigation that allows a robot to map an unknown environment.

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

The following lists 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 package and Cartographer SLAM package in 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 environment SLAM library developed by Google. Although it was originally developed for ROS, adapted versions have been created for ROS 2, allowing it to be used within the ROS 2 ecosystem.

To start a SLAM project in ROS 2, specific sensors (such as LiDAR or cameras) are also required, and the computer must have sufficient computing power to run SLAM algorithms, as well as familiarity with ROS 2 knowledge of nodes, topics, services, actions, parameters, etc., to achieve effective data processing and communication. As the project develops, advanced features such as dynamic reconfiguration, 3D mapping, and path planning may also need to be considered.

Function

It must first be stated 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 complete the construction of an environment map without the involvement of Nav2. It relies on sensor data to perceive the environment and estimates the robot's position and pose through algorithms, thereby constructing a map of the environment. Meanwhile, Nav2 also has autonomous navigation capabilities. Even without a SLAM map as input, it can rely on environmental information from other sources for navigation.

Complementarity: Although the two are technically independent, their combination brings significant advantages. Nav2 can use the precise map created by SLAM for efficient path planning, ensuring that the robot can navigate smoothly to the target position. SLAM, in turn, can leverage 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 construction.

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

2.1.slam_toolbox Overview

Concept

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

Features

• Algorithm Integration Includes SLAM algorithms based on Kalman filters and particle filters, as well as graph optimization-based SLAM algorithms, etc.

• Data Processing Provides functions such as data fusion, data preprocessing, and anomaly detection, supporting the processing of data from multiple sensors such as LiDAR, cameras, and IMUs.

• Map Construction Utilizes sensor data and robot motion information to construct accurate maps and performs updates and optimization.

• Modular Design Supports users in inserting their own algorithms or replacing existing modules to achieve a highly customized SLAM solution.

Advantages

• Open-source nature and flexibility As open-source software, SLAM Toolbox provides source code and documentation, allowing users to freely modify and extend the algorithms.

• Support for multiple sensors It supports data from various sensors such as LiDAR, cameras, and IMUs, adapting to the needs of different application scenarios.

• High efficiency It leverages the features of modern C++ to optimize performance, ensuring efficient operation even on resource-constrained hardware.

• Broad application prospects It is applicable to multiple fields such as academic research, UAV navigation, autonomous vehicles, indoor robots, and industrial automation.

2.2.slam_toolbox安装

With the Advanced Packaging Tools (APT) package manager, slam_toolbox can be installed using a binary method. The installation command is as follows:

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

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

in the instruction<ros2-distro>Please replace it with the currently used ROS2 version.

2.3.slam_toolbox Node Description

There are two commonly used nodes in slam_toolbox: sync_slam_toolbox_node and async_slam_toolbox_node.

sync_slam_toolbox_node:

This is a synchronous node that waits for all sensor data to arrive before processing, ensuring data integrity and consistency and improving localization and mapping accuracy. However, synchronous processing may cause latency, making it more suitable for scenarios with high requirements for data consistency and accuracy and low real-time requirements.

async_slam_toolbox_node:

Unlike a synchronous node, this is an asynchronous node that can immediately process received data, reducing latency and improving response speed. However, asynchronous processing may result in data that is not fully synchronized, and the localization and mapping results may be less accurate than with the synchronous approach. Therefore, it is more suitable for scenarios with high real-time requirements and relatively low requirements for data consistency and accuracy.

When deciding whether to use sync_slam_toolbox_node or async_slam_toolbox_node, a trade-off must be made based on application requirements and environment. If real-time performance is critical and a certain degree of localization or mapping error is acceptable, choose async_slam_toolbox_node. If there are higher requirements for data consistency and accuracy, and real-time performance is not the primary consideration, then choose sync_slam_toolbox_node. In addition, the main difference between the two lies in the data processing method, while the topics published, topics subscribed to, services provided, and parameters used by the two nodes are all the same.

1. Subscribed topics

Although /odom is not subscribed to, /odom needs to be published in order to change the coordinates.

2. Published topics

3. Published services

4. Parameters

   1. Solver parameters

• solver_plugin 用于 karto 扫描解算器的非线性解算器类型。选项：solver_plugins::CeresSolver, - solver_plugins::SpaSolver, solver_plugins::G2oSolver. Default: solver_plugins::CeresSolver.

• ceres_linear_solver Ceres 使用的线性求解器。选项：SPARSE_NORMAL_CHOLESKY、SPARSE_SCHUR、ITERATIVE_SCHUR、CGNR。默认为 SPARSE_NORMAL_CHOLESKY。

• ceres_preconditioner 与该求解器一起使用的预处理器。选项：JACOBI、IDENTITY（none）、SCHUR_JACOBI。默认为JACOBI。

• ceres_trust_strategy 信任区域策略。行搜索策略没有公开，因为它们对于这种用途表现不佳。选项：LEVENBERG_MARQUARDT、DOGLEG。默认值：LEVENBERG_MARQUARDT。

• ceres_dogleg_type 如果信任策略是 DOGLEG，则使用dogleg策略。选项：TRADITIONAL_DOGLEG、SUBSPACE_DOGLEG。默认值：TRADITIONAL_DOGLEG

• ceres_loss_function 拒绝异常值的损失函数类型。没有一个等于损失平方。选项：None、HuberLoss、CauchyLoss。默认值：None。

• mode “建图”或“定位”模式，用于 Ceres 问题创建中的性能优化

  • Toolbox参数

• odom_frame 里程计坐标系

• map_frame 地图坐标系

• base_frame 基坐标系

• scan_topic 扫描主题名， 注意是/scan 不是scan

• scan_queue_size 扫描消息对队列长度。在异步模式下应始终设置为 1

• use_map_saver 实例化地图服务程序并自行订阅map主题

• map_file_name 启动时加载的位姿图文件的名称（如果可用）

• map_start_pose 启动建图/定位时的位姿（如果可用）

• map_start_at_dock 在dock（第一个节点）处启动姿势图加载（如果可用）。如果同时设置了pose和dock，优先使用pose

• debug_logging 更改日志以进行调试

• throttle_scans 在同步模式下限制的扫描次数

• transform_publish_period 里程计odom变换发布周期。 0 不会发布变换。

• map_update_interval 更新 2D 占用地图的时间间隔

• enable_interactive_mode 是否允许启用交互模式。交互模式将保留映射到其 ID 的激光扫描缓存，以便在交互模式下进行可视化。结果，该进程的内存将会增加。在定位和长期建图模式下可以手动禁用此功能，因为它们会随着时间的推移增加内存利用率。对于建图或连续建图模式均有效。

• position_covariance_scale 从扫描匹配发布姿势时缩放位置协方差的量。这可用于调整下游定位滤波器中位姿的影响。协方差表示测量的不确定性，因此扩大协方差将减小位姿对下游滤波器的影响。默认值：1.0

• yaw_covariance_scale 从扫描匹配发布位姿时缩放偏航协方差的量。请参阅position_covariance_scale 的描述。默认值：1.0

• resolution 生成的 2D 占用图的分辨率

• max_laser_range 用于 2D 占用地图光栅化的最大激光范围

• minimum_time_interval 在同步模式下处理的扫描之间的最短持续时间

• transform_timeout 查找转换 TF 超时时间限制

• tf_buffer_duration 存储 TF 消息以供查询的时间。如果在同步模式下以倍速脱机运行，则设置高一些。

• stack_size_to_use 将堆栈大小重置为的字节数，以启用文件的序列化/反序列化。自由默认值为 40000000，但越少越好。

• minimum_travel_distance 处理新扫描之前的最小行进距离

  • 匹配器参数

• use_scan_matching 是否使用扫描匹配来优化里程位姿

• use_scan_barycenter 是否使用重心或扫描位姿

• minimum_travel_heading 合理更新的最小航向变化

• scan_buffer_size 缓冲到链中的扫描次数，也用作定位模式循环缓冲区中的扫描次数

• scan_buffer_maximum_scan_distance 从缓冲区中删除之前扫描，距离之前位姿的最大距离

• link_match_minimum_response_fine 精细分辨率通过的阈值链接匹配算法响应

• link_scan_maximum_distance 有效链接扫描之间的最大距离

• Loop_search_maximum_distance 循环闭合时考虑的扫描距离的最大阈值

• do_loop_close 是否进行循环闭合（如果不确定，答案是“true”）

• Loop_match_minimum_chain_size 寻找循环闭合的扫描的最小链长度

• Loop_match_maximum_variance_coarse 粗略搜索中传递给细化的阈值方差

• Loop_match_minimum_response_coarse 粗略搜索中环路闭合算法的阈值响应要传递给细化

• Loop_match_minimum_response_fine 精细搜索中循环闭合算法的阈值响应传递给细化

• correlation_search_space_dimension 搜索网格大小以进行扫描相关性

• correlation_search_space_resolution 搜索网格分辨率以进行扫描相关性

• correlation_search_space_smear_deviation 用于平滑响应的多模态涂抹量

• loop_search_space_dimension 循环闭合算法的搜索网格的大小

• loop_search_space_resolution 搜索网格分辨率以进行循环闭合

• loop_search_space_smear_deviation 用于平滑响应的多模态涂抹量

• distance_variance_penalty 应用于匹配扫描的惩罚，因为它与里程姿势不同

• angle_variance_penalty 应用于匹配扫描的惩罚，因为它与里程姿势不同

• fine_search_angle_offset 用于测试精细扫描匹配的角度范围

• rough_search_angle_offset 用于测试粗略扫描匹配的角度范围

• coarse_angle_resolution 在扫描匹配中测试的偏移范围内的角度分辨率

• minimum_angle_penalty 确保尺寸不会膨胀的最小惩罚角度

• minimum_distance_penalty 扫描可以确保大小不会爆炸的最小惩罚

• use_response_expansion 如果没有找到可行的匹配，是否自动增加搜索网格大小

2.4.slam_toolbox基本使用

1. Preparation

In the src directory, first call the following command to create a feature 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 package, create a new launch directory and a params directory. Under the launch directory, create a newonline_sync_launch.pyfile 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

该launch文件主要是加载了slam_toolbox下的sync_slam_toolbox_node节点，并且会从当前功能包的params下读取一个名为mapper_params_online_sync.yaml的配置文件。这个配置文件还不存在，接下来需要在params目录下新建mapper_params_online_sync.yaml文件，并输入如下内容：

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 according to the actual situation.

3. Edit configuration file

OpenCMakeLists.txt and enter the following content:

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

4. Compile

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select mycar_slam_slam_toolbox

5. Execute

   1. First, run 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 the use of simulation time.

   Finally, it should be noted that this section uses thesync_slam_toolbox_node node, i.e., mapping in synchronous mode, while the asynchronous mapping nodeasync_slam_toolbox_node is used similarly to the synchronous one.

We use keyboard-controlled nodes to control the robot and run across the entire map,

Black: obstacle

White: obstacle-free area

Gray: Unknown region

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

The localization means that SLAM will publish a /tf, which will contain the coordinate transformation between the robot and the map.

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

Such a design allows the entire coordinate tree to be a chain structure, avoiding base_link or base_foot_print having two parent classes, which would increase the amount of computation.

Notes:

1. sudden map corruption
   1. If your environment is simple, the map may suddenly become corrupted while running. In this case, it is most likely a do_loop_closing issue. Try setting do_loop_closing to false and test again. This thing is used by SLAM to detect shapes with laser and match against previously traversed paths. If it matches a similar shape, it will connect the current trajectory segment with previous trajectories, and then re-optimize the entire pose graph to make the overall map more consistent. This is mainly to prevent odom drift, because odom will drift, and when the robot finally returns to the starting point, SLAM may think the position is off by tens of centimeters, but it can be corrected by lidar.
   2. In narrow passages, try to go straight through as much as possible, do not rotate in place, etc. If it breaks in such narrow passages, it is most likely a scan matching related parameter issue.

2.5.Cartographer Overview

Concept

Cartographer is a graph-optimization-based laser SLAM algorithm library introduced by Google, supporting the construction of 2D and 3D maps. It combines data from lidar and an inertial measurement unit (IMU) to achieve real-time, accurate localization and mapping through efficient algorithms.

Function

• Parallel scan matching Use parallel computing technology to accelerate scan matching and improve mapping efficiency.

• Pose graph optimization Estimate the robot's pose and the map's topology through graph optimization techniques to reduce accumulated errors.

• Real-time map update Update the map in real time while the robot is moving to ensure the map's accuracy and timeliness.

• Loop closure detection Identify areas the robot has previously visited through loop closure detection, further reduce accumulated errors, and improve the global consistency of the map.

• Multi-sensor fusion Support the fusion of data from multiple sensors such as LiDAR, IMU, and odometry to improve the accuracy of localization and mapping.

Advantages

• Efficient and stableCartographer's algorithms are carefully designed and optimized to run efficiently and stably in complex environments.

• High precisionIt provides high-precision localization and mapping results through graph optimization and loop closure detection techniques.

• FlexibilityIt supports 2D and 3D map building, adapting to the needs of different application scenarios.

• Open source and freeCartographer is an open source project, and users can obtain and use its source code and documentation for free.

• Community supportIt has an active community support system, and users can get help and support from developers around the world.

2.6.cartographer installation

Using Ubuntu's package manager, Cartographer can be installed in binary form. The installation command is as follows:

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 above two installation instructions, the former is used to install the core library of Cartographer. This package is not directly integrated with ROS2, but exists as an independent algorithm library, providing underlying computational support for map building and localization. The latter is the wrapper of Cartographer in the ROS2 environment. It provides an interface to the ROS2 system, enabling the Cartographer algorithm to run in the ROS2 environment. In addition, in the instruction,<ros2-distro>Please replace with the currently used ROS2 version.

2.7.Cartographer node description

In the Cartographer framework, cartographer_node and cartographer_occupancy_grid_node are two key nodes, each undertaking different roles and functions. The detailed introduction is as follows.

cartographer_node:

Mainly responsible for subscribing to data from various sensors (such as LiDAR, IMU, odometry, etc.) and building maps in real time based on these data. It uses a submap approach to incrementally construct and update the map, ensuring localization accuracy and real-time mapping performance.

cartographer_occupancy_grid_node:

This node is responsible for receivingcartographer_nodethe published submap list (/submap_list), stitching them into a complete occupancy grid map, and then publishing this map. This node is the final stage of map generation, enabling Cartographer to output maps that are human-readable and easy to visualize.

These two nodes work in coordination: the former is responsible for building and updating the map in real time, while the latter is responsible for assembling submaps into a complete grid map and publishing it, enabling Cartographer to efficiently implement SLAM functionality.

1. cartographer_node订阅的话题

2. cartographer_node Published topics

3. cartographer_node published services

4. cartographer_node parameter

cartographer_nodeThe node needs to receive a parameter configuration file, which contains various parameters required for map construction, trajectory tracking, etc.

5. cartographer_occupancy_grid_node订阅的话题

6. cartographer_occupancy_grid_node Published topics

7. cartographer_occupancy_grid_node requested service

8. cartographer_occupancy_grid_node parameters

cartographer_occupancy_grid_nodeThe node needs to configure parameters such as the map resolution and update period to ensure that the generated grid map meets specific accuracy and real-time requirements.

2.8.Basic usage of cartographer

1. Preparation

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

ros2 pkg create mycar_slam_cartographer --dependencies cartographer

2. Write launch files and parameter files

Under the package, create a launch directory and a params directory. Under 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,
    ])

该launch文件主要是加载了cartographer_ros下的cartographer_node与cartographer_occupancy_grid_node节点，并且会从当前功能包的params下读取一个名为mycar.lua的配置文件。这个配置文件还不存在，接下来需要在params目录下新建mycar.lua文件，并输入如下内容：

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 according to the actual situation.

3. Edit configuration file

OpenCMakeLists.txt and enter the following content:

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

4. Compile

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select mycar_slam_cartographer

5. Execute

(1) Please first call the following instruction to start the simulation environment:

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

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

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

(3) Start rviz2, set the Fixed Frame to map, add the map plugin and set the topic to /map, and the created map will be displayed. When the robot moves, the map will also be updated accordingly.

3.Map Service

During SLAM mapping, map data is stored in memory, which also means that once the node is closed, the data will be released along with it. A more reasonable implementation should serialize the constructed map to disk for persistent storage, and later also deserialize and read the map data from disk for other operations. In Nav2, the relevant function packages for map serialization and deserialization have already been encapsulated. This package is:nav2_map_server.

Innav2_map_server , you can interact with the rest of the Nav2 system through topics and service interfaces.nav2_map_serverThere are two important nodes under the package, namelymap_saver_cli andmap_server . Through themap_saver_cli node, you can save the map, while themap_server node can display the map at startup.

3.1.Save Map (Serialization)

3.1.1.Map Save Node Description

Innav2_map_serverthe map save node in ismap_saver_serverand the related information of this node is as follows.

1. Subscribed topics

2. Parameter

• save_map_timeout The maximum waiting time for a map save operation.

• free_thresh_default The probability threshold at 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; the default is true.

3. map_saver_cli

Additionally, for ease of use, based on map_saver_server, an executable program named map_saver_cli is also encapsulated, which can more conveniently set data related to map saving via arguments, and during subsequent execution it also calls map_saver_cli. Its argument list is as follows:

• \-t Subscribed map topic.

• \-f Map storage path.

• \--occ Probability threshold at which a grid cell is considered occupied.

• \--free Probability threshold at which a grid cell is considered unoccupied.

• \--fmt Image format.

• \--mode Map mode, trinary (default) or scale or raw.

3.1.2.Basic map saving operations

Preparation

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

Save Map

After SLAM mapping is complete, enter the workspace in the terminal and call the following command to save the map:

ros2 run nav2_map_server map_saver_cli -f map/my_map

The above command subscribes to the /map topic and saves the data in 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) to generate two files, named:my_map.yamlandmy_map.pgm.

3.1.3.Map Interface

In Nav2, there are mainly two map-related interfaces:

• nav_msgs/msg/MapMetaData Map metadata, including the map width, height, resolution, etc.

• nav_msgs/msg/OccupancyGrid Map raster data, which is generally displayed graphically in rviz.

nav_msgs/msg/MapMetaData

Call instructionros2 interface show nav_msgs/msg/MapMetaDataView the interface format; the displayed content is as follows (comments have been translated into Chinese):

# 它包含了关于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

Invocation instructionros2 interface show nav_msgs/msg/OccupancyGridView the interface format; the displayed content is as follows (comments have been translated into Chinese):

# 它代表一个二维网格地图。
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

3.1.4.Map Storage Format

在 地图保存基本操作 一节中，地图保存后后生成两个文件，这两个文件就是用来存储序列化后的地图数据的。其中，my_map.pgm是一张图片资源，使用图片查看程序打开即可，而my_map.yaml保存的是地图的元数据信息，用于描述图片，内容格式如下：

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 description:

• image 被描述的图片资源路径，可以是绝对路径也可以是相对路径。

• resolution 图片分片率(单位: m/像素)。

• origin 地图中左下像素的二维姿态，为（x，y，z），偏航为逆时针旋转（偏航=0 表示无旋转）。

• occupied_thresh 占用概率大于此阈值的像素被视为完全占用。

• free_thresh 占用率小于此阈值的像素被视为完全空闲。

• negate 是否应该颠倒白色/黑色 自由/占用的语义。

• mode 地图模式，trinary(默认)或scale或raw。

3.2.Read map (deserialize)

3.2.1.Map reading node description

Innav2_map_serverthe map reading node ismap_server; the related information of this node is as follows.

Published topics

Parameter

• frame_id Map coordinate system name.

• topic_name Topic name.

• yaml_filename Map data source.

3.2.2.Basic map reading operations

Preparation

Please first call the following command to create a package under the src directory of the workspace:

ros2 pkg create mycar_map_server --dependencies nav2_map_server

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

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

Read Map

Usagemap_serverWhen reading a map, there are two common methods: using terminal commands and integrating via launch files. Both methods have the same effect and can publish map messages as topics.

colcon build
source ./install/setup.bash

Method 1: Terminal command

In the terminal, enter the workspace and input the following command:

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

Sincemap_serveris a node with a lifecycle, the node next 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

After execution completes, if there are no exceptions, then callros2 topic listto view the/maptopic, indicating that the map message has been published.

Method 2: launch integration

Method 1 requires manual setting ofmap_serverlifecycle, and the steps are somewhat cumbersome. Therefore, we can also integrate this node into the launch file to simplify the startup steps. In the launch directory, create a new file namedmap_server.launch.pyand 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 package'snav2_lifecycle_managerin thelifecycle_managercomponent is used; this component can automatically configure and activate the lifecycle nodes it manages. After building the package and executing this launch file, the final effect is similar to that of method one.

ros2 launch mycar_map_server map_server.launch.py

Display map

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

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

4.AMCL Adaptive Monte Carlo Localization

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

The localization technology in Nav2 is called AMCL, which stands for Adaptive Monte Carlo Localization, an adaptive Monte Carlo localization algorithm based on particle filtering. It performs adaptive localization through Monte Carlo methods, using perception of the robot's surrounding environment and analysis of observation data to determine the robot's position and orientation in the environment. In Nav2, the corresponding package isnav2_amcl.

In AMCL, the core idea of the particle filter is to use a set of particles (samples) to represent the possible positions of the robot on the map. Each particle has a weight that indicates the confidence of the position represented by that particle. 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, thereby achieving an accurate estimate of the robot's position.

4.1.Locating Node Description

功能包nav2_amcl中的核心节点为amcl。该节点相关信息如下。

1. Subscribed topics

2. Published topics

3. Published services

4. Parameters

General parameters

• bond_disable_heartbeat_timeout: When set to true, the heartbeat-based timeout detection between the amcl node and other nodes is disabled. This is typically used when the connection between nodes is very stable and frequent heartbeat detection is not needed to confirm the connection status.

• base_frame_id: Defines the ID of the robot base coordinate frame, usually base_link or a similar name.

• global_frame_id: Defines the ID of the global map coordinate frame, usually map.

• odom_frame_id: Defines the ID of the odometry coordinate frame, usually odom.

• tf_broadcast: When set to true, the amcl node publishes the transform from the odometry coordinate frame to the global map coordinate frame.

• transform_tolerance: Sets the tolerance for TF transforms, used 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_fieldis a commonly used model that takes into account the probability of the laser beam hitting an obstacle.

• laser_max_rangeandlaser_min_range: Set the maximum and minimum detection ranges 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_beamskipand related parameters (beam_skip_distance,beam_skip_threshold,beam_skip_error_threshold) : These parameters are used to control whether to skip the processing of certain laser beams in order to reduce computation. However,do_beamskipis set tofalsewhich means 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 AMCL implementation. In standard AMCL implementations, these parameters may not be used directly.

• max_particlesandmin_particles: Set the maximum and minimum number of particles for the particle filter, respectively.

• resample_interval: Set the number of filter updates required before resampling.

• pf_errandpf_z: These parameters are used to control the performance of the particle filter, but their specific roles may depend on implementation details of AMCL.

Initial pose parameters

• initial_pose: Defines the robot's initial pose (x, y, yaw, z), but in actual use, ifset_initial_poseis set totruethis initial pose may be overridden by the initial pose set via a service request.

• set_initial_pose: When set totrue, it allows the robot's initial pose to be set via a service request.

• always_reset_initial_pose: When set tofalse, it indicates that the initial pose will not be automatically reset at the start of each localization session.

Other parameters

• first_map_only: 当设置为false时，表示amcl将订阅并处理不断更新的地图话题。

• map_topic: 定义地图话题的名称，amcl将订阅这个话题以获取地图信息。

• scan_topic: 定义激光雷达扫描数据话题的名称，amcl将订阅这个话题以获取用于定位的数据。

• save_pose_rate: 设置保存机器人位姿的速率（以Hz为单位）。

• recovery_alpha_fast和recovery_alpha_slow: 这些参数在标准的amcl实现中可能不是直接使用的，它们可能属于某个特定版本的amcl或扩展。

• z_hit、z_rand、z_short、z_max和sigma_hit: 这些参数定义了激光模型中的概率分布，用于计算激光束击中障碍物或随机位置的概率。

4.2.Basic positioning node operations

1. Preparation

Please first call the following command to create a package under the src directory of the workspace:

ros2 pkg create mycar_localization --dependencies nav2_amcl mycar_map_server

2. Write launch files and parameter files

Under the package, create new launch and params folders, and in the launch directory create a new file namedmycar_loca.launch.pyand 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 above code, a amcl node was created, and a configuration file named params was loaded from the amcl.yaml 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 also included to load the map.

Create a new file namedamcl.yamlin 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 Node Location Description.

3. Edit the configuration file

OpenCMakeLists.txt and enter the following content:

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

4. Compile

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select mycar_localization

5. Execution

(1) Please first call the following instruction to start the simulation environment:

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

(2) Then, in the terminal, enter the current workspace and input 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 basic map reading operations add and display the map.

Next, click the rviz2 menu bar's2D Pose EstimateSet an initial pose for the robot in the map.

Here, an approximate robot position and robot orientation need to be given; it does not have to be very accurate, as the robot will gradually calibrate through AMCL during motion.

First click2D Pose Estimate, left-click on the robot's position on the map, press and hold without releasing, drag the mouse toward the robot's facing direction, and when a green arrow like the one below appears, release the mouse.

Then addParticleCloudplugin, set the topic to/particle_cloudand set the under-topicReliability PolicytoBest EffortFinally, 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 exhibit varying degrees of convergence or divergence.

Even if your robot hits a wall, and the robot positions in rviz2 and Gazebo are completely offset, as long as you let the robot move for a while longer, the robot position will be re-estimated. AMCL is very robust.

The position is completely offset as shown in the figure above.

After a period of driving, the pose and position were successfully estimated again.

5.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.

The website below provides the parameter descriptions for each node of the official NAV2 (a very commonly used website when tuning parameters):

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

For English you don't understand, you can use the Chrome extension Immersive Translate to translate it, which can provide Chinese-English bilingual comparison translation 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 look at the version translated by YuXiang ROS (but the website is unstable):

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

5.1.Lifecycle management node description

In Nav2, through thenav2_lifecycle_managerunder the packagelifecycle_managernode manages the lifecycle state transitions of other nodes.lifecycle_managerThrough the ROS 2 lifecycle node mechanism, nodes provide a standardized method to control the state transitions of each node in Nav2. These states include Unconfigured, Inactive, Active, and Finalized. Through fine-grained control of these state transitions,lifecycle_managernodes can ensure that all parts of the Nav2 system start, run, and stop at the correct times, thereby improving the reliability and stability of the system.

Parameter

• /bond_disable_heartbeat_timeout: This parameter relates to communication bonding between nodes. Communication between nodes can be enhanced for reliability through bonding, and heartbeat timeout is a way to detect whether a node is still active. When set to falseit means heartbeat timeout detection is enabled.

• attempt_respawn_reconnection: The value is of bool type. When set to true, it indicates that if a managed node terminates unexpectedly, the lifecycle manager will attempt to restart it.

• autostart: The value is of bool type. When set to true, it indicates that when the lifecycle manager starts, it will automatically attempt to start the nodes it manages.

• bond_respawn_max_duration: The maximum duration for reconnecting or restarting a node.

• bond_timeout: This relates to the communication bonding timeout between nodes. If no message is received from another node within this time, that node is considered disconnected.

• diagnostic_updater:

  • period: The update period of the diagnostic updater. The diagnostic updater is used to collect and publish diagnostic information about node status, and this defines the frequency at which such information is updated.

• node_names: The list of node names managed by this lifecycle manager.

• use_sim_time: Whether to use simulation practice.

5.2.Behavior tree node description

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

https://www.behaviortree.dev/

https://arxiv.org/abs/1709.00084

github链接如下：

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 actual time.

• global_frame: Defines the name of the global coordinate frame, typically the map frame.

• robot_base_frame: Specifies the name of the robot base coordinate frame.

• odom_topic: The ROS topic name for odometry data.

• bt_loop_duration: The duration of the behavior tree execution loop (units may vary depending on implementation).

• default_server_timeout: The default timeout for navigation server operations.

• enable_groot_monitoring: Enables or disables the Groot monitoring feature.

• groot_zmq_publisher_port: The port number for the ZeroMQ publisher in Groot monitoring.

• groot_zmq_server_port: The port number for the ZeroMQ server in Groot monitoring.

• plugin_lib_names: A list of library names containing the plugins required for navigation.

5.3.Planner node description

In the Nav2 navigation framework,nav2_plannerunder the packageplanner_servernode, responsible for handling path planning requests and generating a path from the current position to the target position. When executing, this node depends on the/global_costmap/global_costmapmap messages provided by the node.

1. planner_server Published topics

2. planner_server parameter

• /bond_disable_heartbeat_timeout: This parameter controls whether heartbeat timeout detection is disabled. 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 indicates that heartbeat timeout detection is disabled, which may be used in certain specific network environments or when communication between nodes is known to be very stable.

• GridBased: This is a configuration block specifically used to set the parameters of a grid-based planner. A grid-based planner typically uses a gridded representation of the map to plan paths.

  • allow_unknown: Controls whether the planner allows path planning in unknown (i.e., unexplored) areas of the map.

  • plugin: Specifies the planner plugin to use.

  • tolerance: Sets the tolerance when planning paths, usually 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 goal 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 with which the planner generates new paths. It helps the system monitor the planner's performance and may be used for debugging or performance optimization.

• planner_plugins: This is a list specifying the available planner plugins. In this example, it contains only one GridBased planner, but in theory it can contain multiple different types of planners to suit different navigation needs.

• use_sim_time: This parameter controls whether to use simulated time. In a simulation environment, time is controlled by the simulation software rather than by the actual physical clock. Setting this parameter to true allows the node 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. /global_costmap/global_costmap Subscribed topics

4. /global_costmap/global_costmap Published topics

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 the peer node during inter-node communication binding.

• always_send_full_costmap: Controls whether to always send the complete costmap information instead of only the changed parts.

• clearable_layers: Lists the costmap layers that can be cleared; obstacle information in these layers can be updated or cleared through some means (such as sensor data).

• filters: Defines the list of filters applied to the costmap, used to preprocess or modify map data.

• footprint: Specifies the footprint shape of the robot on the map, i.e., the spatial extent occupied by the robot.

• footprint_padding: Adds extra padding space to the robot's footprint to account for the additional space required during robot motion.

• global_frame: Defines the reference coordinate frame used by the global costmap.

• heightandwidth: Defines the height and width of the global costmap (in number of cells).

• inflation_layer: Configures the parameters of the inflation layer; the inflation layer is used to add a "buffer zone" of a certain width around obstacles to prevent the robot from getting too close to obstacles.

  • cost_scaling_factor: The scaling factor for inflation cost.

  • enabled: Controls whether the inflation layer is enabled.

  • inflate_around_unknown: Controls whether to perform inflation around unknown space.

  • inflate_unknown: Controls whether to treat unknown space as an obstacle and inflate it.

  • inflation_radius: The inflation radius.

  • plugin: Specifies the inflation layer plugin to use.

• lethal_cost_threshold: Defines the cost threshold for considering an obstacle as "lethal" in the costmap.

• map_topic: Specifies the ROS topic to subscribe to for obtaining map information.

• observation_sources: Defines the observation sources of the costmap, i.e., which sensors or data sources are used to update the map.

• obstacle_layer: Configures the parameters of the obstacle layer; the obstacle layer 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_heightandmin_obstacle_height: Defines 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_xandorigin_y: Defines the coordinates of the origin of the global costmap.

• plugins: Lists the enabled costmap plugins, which define how the costmap is built and updated.

• publish_frequency: Defines the frequency at which the costmap is published.

• resolution: Defines the resolution of the costmap, i.e., the actual physical size represented by each cell.

• robot_base_frame: Defines the reference coordinate frame of the robot base.

• robot_radius: Defines the radius of the robot, used to compute the space occupied by the robot on the map.

• rolling_window: Controls whether to use a rolling window (i.e., a dynamically changing map region) instead of a fixed-size map.

• static_layer: Configures the parameters of the static layer; the static layer 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: Specifies 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: Defines the tolerance for coordinate transforms.

• track_unknown_space: Controls whether to track unknown space in the map.

• transform_tolerance: Defines the allowable error range during coordinate transforms.

• trinary_costmap: Controls whether to use a trinary costmap (free, occupied, unknown) instead of only a binary one (free, occupied).

• unknown_cost_value: Defines the value representing unknown space in the costmap.

• update_frequency: Defines the frequency at which the costmap is updated.

• use_maximum: Controls whether to update the costmap using the maximum value among multiple observation sources.

• use_sim_time: Controls whether to use simulated time (useful in simulation environments).

• voxel_layer: Configures the parameters of the voxel layer; the voxel layer uses a voxel grid to represent obstacle information in 3D space.

  • enabled: Controls whether the voxel layer is enabled.

  • footprint_clearing_enabled: Controls whether to clear voxel information within the robot's footprint.

  • mark_thresholdandunknown_threshold: Defines the threshold for treating a voxel as an obstacle or unknown.

  • max_obstacle_heightandmin_obstacle_height: Defines 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 along 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_resolutionandz_voxels: Defines the resolution of the voxel grid along the Z axis and the number of voxels.

5.4.Controller Node Description

In the Nav2 navigation system,nav2_controllerthe packagecontroller_serveris responsible for handling control requests in navigation tasks to ensure that the robot can move along the planned path. Its main function is to generate velocity and direction control commands based on the global or local path calculated by thenav2_plannermodule, i.e., to control the robot to follow the planned path. During execution, this node also depends on the map messages provided by the/local_costmap/local_costmapnode.

1.controller_server订阅的话题

2.controller_server Published topics

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 a path.

  • BaseObstacle: Defines the basic obstacle evaluation parameters used to avoid obstacles during path following.

    • class: Specifies the name of the class, here BaseObstacle, indicating that this is a basic obstacle evaluation component.

    • scale: Defines the weight or degree of influence of this obstacle evaluation in the overall evaluation.

    • sum_scores: Indicates whether to accumulate the scores of multiple obstacles; false may mean using the maximum value or another logic.

  • GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal, Oscillation: These are different evaluation or behavior components in 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, and reducing oscillation.

  • acc_lim_theta, acc_lim_x, acc_lim_y: These parameters define the robot's acceleration limits 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 debug information for the trajectory.

  • Other parameters related to speed, acceleration, time granularity, trajectory generation, etc., jointly define the behavior and performance of the path-following algorithm.

• controller_frequency: Specifies the operating frequency of the controller (possibly FollowPath or another controller), in hertz.

• controller_plugins: Specifies the list of controller plugins to be used; here it contains only FollowPath.

• failure_tolerance: Defines the time or distance tolerance for failure, used to provide some 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 robot's minimum speed thresholds in different directions; falling below these thresholds may be regarded 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 goal.

• qos_overrides: Defines the QoS (Quality of Service) override settings for ROS services or topics, used to adjust the reliability and performance of message passing.

• 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 ROS simulation environments.

4./local_costmap/local_costmap subscribed topics

5./local_costmap/local_costmap Published topics

6./local_costmap/local_costmap Parameters

• /bond_disable_heartbeat_timeout: Whether to disable the heartbeat timeout check between nodes. When set to true means the feature is disabled, possibly to reduce network traffic or adapt to a specific network environment.

• always_send_full_costmap: Whether to always send the full 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 which means that obstacle data in these layers can be cleared.

• filters: Specifies the list of filters applied to the costmap. It is empty here, meaning no filters are applied.

• footprint: The robot's footprint polygon, which defines the robot's physical occupied area in 2D space.

• footprint_padding: The footprint padding, used to add extra space to the robot footprint when computing the costmap.

• global_frame: The name of the global reference frame, usually used for localization and navigation tasks.

• height: The height of the costmap (in number of cells).

• inflation_layer: Configuration of the inflation layer, used to add an inflation region around obstacles so that the robot maintains a safe distance from obstacles.

• lethal_cost_threshold: The lethal cost threshold; cost values above this threshold represent impassable areas.

• map_topic: The name of the subscribed map topic, used to obtain global map information.

• observation_sources: Configuration of observation sources, used to specify which sensor data will be used to update the costmap. It is an empty string here, which may be a default value or a different configuration approach.

• 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 position of the costmap in the global coordinate system.

• plugins: The list of enabled plugins, defining the different layers used by the costmap, such as the obstacle layer and inflation layer.

• publish_frequency: The publishing frequency of the costmap (in Hz).

• resolution: The resolution of the costmap (in meters per cell).

• robot_base_frame: The name of the reference frame for the robot base, used to localize the robot.

• robot_radius: The radius of the robot, used to represent the robot's physical size in the costmap.

• rolling_window: Whether to use a rolling window. When set to true the costmap updates its position and extent 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: Transform tolerance, defining the thresholds for acceptable time difference and angular difference in transforms.

• trinary_costmap: Whether to use a trinary costmap, typically free, occupied, and unknown.

• unknown_cost_value: The cost value of unknown areas in the costmap.

• update_frequency: The update frequency of the costmap (in Hz), which is different from the publishing frequency.

• use_maximum: Whether to use the maximum value when multiple sources provide cost information for the same location.

• 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 3D space into voxels (volume pixels) to improve the processing efficiency of the costmap.

5.5.Restorer 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 other navigation issues. In Nav2, it is implemented by thenav2_behaviorsfunction package'sbehavior_serverimplementation of this function.

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, because a simulation environment can speed up or slow down the passage of time without needing to wait for real time to pass.

• global_frame: Defines the name of the global coordinate frame, which is typically used for localization and path planning in navigation tasks. Here, it is set toodom meaning that odometry data is used as the reference for the global coordinate frame.

• robot_base_frame: Specifies the coordinate frame name of the robot base. This is the reference point on the robot used for localization and motion control, and typically corresponds 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 changes in the robot's position and orientation 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 totruemay mean disabling or adjusting the heartbeat timeout behavior, so as to avoid unnecessary timeout errors in specific situations (such as a simulation environment).

• assisted_teleop,backup,drive_on_heading,spin,wait: These are configuration entries for behavior plugins that may be used in the behavior tree. Each plugin defines a specific behavior that the robot can execute, such as assisted teleoperation, back up, drive in a specified direction, rotate in place, and wait.

• behavior_plugins: Lists the names of the behavior plugins available in the behavior tree.

• cmd_vel_teleop: Specifies the ROS topic name for the velocity control command used for teleoperation.

• costmap_topic: Defines the ROS topic name of the local costmap. The costmap is used to represent obstacles and passable 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 velocity, minimum velocity, and acceleration limits when the robot rotates.

• projection_time: A time parameter related to costmap updates or predicting future obstacle positions.

• footprint_topic: Defines the ROS topic name for publishing the robot footprint, that is, 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 the step size during simulation.

• transform_tolerance: Defines the tolerance range for coordinate transforms, used to handle small differences between different coordinate frames.

5.6.Waypoint Following Node Description

In the Nav2 navigation stack,nav2_waypoint_followerunder the package, the/waypoint_followernode is responsible for tracking a series of waypoints generated by the path planner to ensure that the robot can move safely and accurately along the predetermined path. The main function of this node is to calculate control commands to control the robot's motion based on the current robot position and velocity information, as well as the waypoint list provided by the path planner (such asnav2_global_plannerandnav2_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 actual time for node timing and synchronization. This is particularly useful in simulation environments, which may not provide a clock fully synchronized with real time.

• loop_rate: Defines the main loop rate of the node, i.e., the number of times per second the node executes its primary tasks (such as processing data, publishing information, etc.). This parameter is very important for controlling the node's responsiveness and resource usage.

• stop_on_failure: Indicates whether the node should stop execution when a navigation task encounters an insurmountable obstacle or meets other failure conditions. This is important for ensuring that, in the event of failure, the system can safely stop and wait for further instructions.

• bond_disable_heartbeat_timeout: Concerns the reliability mechanism for inter-node communication. Bond is a mechanism in ROS 2 used for stable communication between nodes, in which heartbeat signals are used to detect whether a node is still active. Setting this parameter to true disables heartbeat timeout detection, which may be useful in certain specific network configurations or application scenarios.

• waypoint_task_executor_plugin: Specifies the plugin to be used when executing waypoint navigation tasks. Waypoint navigation usually involves a series of predefined points that the robot needs to visit in order. This parameter allows the user to specify the particular plugin or algorithm used to execute this type of task.

• wait_at_waypoint: This is a composite parameter used to configure the specific behavior of waiting at waypoints.

  • enabled: Enables or disables the function of waiting upon reaching each waypoint.

  • plugin: Specifies the type of plugin that implements the waiting function. This allows the user to choose different waiting strategies or behaviors as needed.

  • waypoint_pause_duration: Defines the duration of waiting at each waypoint (in milliseconds). This can be used to ensure that the robot has stabilized or has completed certain operations before moving to the next waypoint.

5.7.Path Smoothing Node Description

In the Nav2 framework,nav2_smootherunder the package,smoother_serverthe node loads and runs various smoother plugins to smooth the planned path, enabling the robot to move more smoothly, continuously, and safely. This capability is of great significance 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 the Bond heartbeat timeout feature. In distributed systems, Bond is used to manage connections and heartbeats between nodes, and this parameter is used to adjust 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 footprint (i.e., the area occupied by the robot 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 is the reference point used for localization and movement in robot navigation.

• simple_smoother:

  • do_refinement: Indicates whether to enable the path refinement (or further optimization) process.

  • 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 smaller than this value, the smoothing process is considered complete.

  • w_data: The weight of data terms (such as obstacle distance) during the smoothing process.

  • w_smooth: The weight of the smoothing term (such as path curvature) during the smoothing process.

• smoother_plugins: The list of defined smoothing plugins; here it lists simple_smoother, indicating that this plugin will be used for path smoothing.

• transform_tolerance: The tolerance for coordinate transformations, used to handle uncertainty when transforming between different coordinate frames.

• use_sim_time: Specifies whether to use simulated time instead of actual 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 ROS system's actual time.

5.8.Speed Smoothing Node Description

Nav2框架中的nav2_velocity_smoother包下的velocity_smoother节点主要负责平滑由Nav2框架发送给机器人控制器的速度指令。其核心功能是实现速度和加速度平滑。这一功能对于确保机器人在导航过程中的稳定性和安全性至关重要。

1. Subscribed topics

2. Published topics

3. Parameters

• bond_disable_heartbeat_timeout: 指示是否禁用节点间的心跳超时机制。如果为true，则节点间的心跳检测不会因超时而断开连接。

• deadband_velocity: 定义在哪些速度分量上应用死区平滑（即忽略小于此阈值的微小速度变化）。这里分别为X轴、Y轴和偏航角速度（theta）设置了死区值。

• feedback: 指定速度平滑器的反馈类型。OPEN_LOOP表示开环控制，即不考虑机器人的实际速度反馈进行速度调整。

• max_accel: 定义机器人在各个方向上的最大加速度限制，包括X轴、Y轴和偏航角速度（theta）。

• max_decel: 定义机器人在各个方向上的最大减速度限制，包括X轴、Y轴和偏航角速度（theta）。注意，减速度值以负数表示。

• max_velocity: 定义机器人在各个方向上的最大速度限制，包括X轴、Y轴和偏航角速度（theta）。

• min_velocity: 定义机器人在各个方向上的最小速度限制，包括X轴、Y轴和偏航角速度（theta）。这通常用于避免发送过小的速度指令给底层控制器。

• odom_duration: 与里程计数据相关的参数，但在此上下文中可能不直接用于velocity_smoother节点，可能是遗留或与其他功能相关联。

• odom_topic: 指定里程计数据的ROS话题名称，velocity_smoother节点将订阅此话题以获取机器人的运动信息。

• scale_velocities: 指示是否根据加速度限制同比例调整速度的其他分量。如果为false，则不会进行速度缩放。

• smoothing_frequency: 定义速度平滑操作的执行频率（Hz），即每秒进行多少次平滑计算。

• use_sim_time: 指定是否使用模拟时间而非实际系统时间。这对于仿真环境特别有用，可以确保时间的一致性和可预测性。

• velocity_timeout: 如果在指定时间内未接收到新的速度指令，则velocity_smoother节点将停止发布速度指令，并可能发送零速度指令以停止机器人运动。这是为了防止在失去速度控制时机器人继续移动。

5.9.Navigation function integration (ROS2 Humble and earlier)

1. Preparation

Please first call the following command to create a package under the src directory of the workspace:

ros2 pkg create mycar_navigation2 --dependencies navigation2 nav2_common

2. Write launch files and parameter files

Under the package, create a launch directory, a params directory, and a bts directory.

launch目录下新建nav2.launch.py文件并输入如下内容：

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 such as the lifecycle manager, behavior tree, planner, controller, recoveries, waypoint tracking, path smoothing, and velocity smoothing. In addition, 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

bt_navigator相关配置文件有两个，分别是描述行为树的xml文件，以及yaml格式的参数文件，前者存储在bts目录下，后者存储在params目录下。

In the bts directory, create a new file namedbt_planner_controller_behavior.xmland 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 to create a new file namedbt_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>

Create a new file namedbt.yamlin the params directory, 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) planner_server related configuration files

Create a new file namedplanner.yamlin the params directory 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

Create a new file namedcontroller.yamlin the params directory, 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.96 m, width 0.45 m

• Using the robot center (base_linkcoordinate system origin) as the reference, calculate the coordinates of the rectangle's vertices:

说明：顶点按顺时针或逆时针顺序排列，覆盖机器人长宽边界。（右手坐标系）

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) behavior_server related configuration files

Create a new file namedbehavior.yamlin the params directory, 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

Create a new file namedwaypoint.yamlin the params directory 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

Create a new file namedvelocity.yamlin the params directory, 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) smoother_server related configuration files

Create a new file namedsmootherr.yamlin the params directory, 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 is necessary to rely on map and localization functions. We can integrate the previous localization launch and the launch of the currently written navigation core module into a single launch file to simplify the startup of the navigation function. Create a new launch file namedbringup.launch.pyin 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

OpenCMakeLists.txt and enter the following content:

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

5. Compilation

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select mycar_navigation2

6. Execution

(1) Please first call the following instruction to start the simulation environment:

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

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

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

（3）启动rviz2，加载/opt/ros/humble/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，为机器人设置初始位姿后，再通过菜单栏的Nav2 Goal设置目标点，机器人就可以自动导航至目标点了。

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

• rviz2: Start the rviz2 tool.

• -d: Specify the .rviz configuration file to be loaded.

• /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz: The path of 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 figure above, the left image is for selecting the initial position, and the right image is for selecting the target position.

In the above figure, the halo near each obstacle and wall indicates the degree of danger; the redder the color, the more dangerous it is for the robot to pass through that area.

5.10.Autonomous Exploration SLAM

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

1. Write a launch file

In the launch directory of the package mycar_navigation2, create a new launch file namedauto_slam.launch.pyand 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. Compile

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select mycar_navigation2

3. Execution

(1) Please first call the following instruction to start the simulation environment:

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

(2) Then, in the terminal, enter the current workspace and input 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）启动rviz2，加载/opt/ros/humble/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，再通过菜单栏的Nav2 Goal设置目标点，机器人就可以自动导航至目标点，并且导航中还会实现建图的功能。

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

6.Multi-vehicle platooning

Multi-vehicle platooning technology, through advanced sensors, real-time data analysis, and highly intelligent control systems, enables a group of vehicles to operate cooperatively without human intervention. As an advanced intelligent transportation system application, it has already demonstrated enormous potential and broad application scenarios in multiple fields. For example:

Time optimization: Multi-vehicle platooning technology eliminates errors and delays in manual operation, reducing human resource costs and operational risks.

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

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

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