Jazzy導航仿真

導航概述

導航簡述

概念

機器人導航是指在沒有人為干預的情況下，機器人可以自主地從一個位置移動到另一個位置。在ROS2中，導航實現最為常用的框架是Nav2。

Nav2也即Navigation2，它繼承自ROS的導航堆棧。它採用了與自動駕駛車輛相同的前沿技術，並經過優化和改造，專門適用於移動機器人和地面機器人的導航需求。這個項目賦予移動機器人穿越複雜環境的能力，使其能夠完成用戶定義的各種應用任務，幾乎適用於任何類型的機器人動力學。Nav2不僅支持機器人從A點到B點的移動，還能設置中間姿態，並執行多種任務，如物體跟蹤、全面覆蓋導航等。作為全球100多家公司信賴的生產級高質量導航框架，Nav2以其卓越的可靠性和穩定性贏得了廣泛讚譽。

作用

Nav2在機器人導航領域作用顯著，主要表現在以下幾個方面。

• Nav2具備多樣的導航功能，支持激光雷達、攝像頭等多種傳感器輸入，實時獲取環境信息，實現智能路徑規劃和精確運動控制。它支持多種導航模式，滿足不同應用需求，並具備強大的擴展性，可與ROS 2組件無縫集成。
• Nav2性能卓越，採用高效算法和數據處理技術，確保機器人在導航過程中響應迅速且穩定。
• 安全方面，Nav2採用多種避障算法和參數調節功能，設定安全區域，提供診斷監控，確保機器人安全導航。

總之，Nav2為機器人的導航和應用提供了強大的支持。無論是工業領域的自動化生產，還是服務領域的智能機器人，Nav2都能夠發揮重要作用，提升機器人的自主性和智能化水平。

導航安裝

藉助於Ubuntu的包資源管理器，可以使用二進制的方式安裝Nav2，安裝指令如下：

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

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

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

指令中的<ros2-distro>請替換成當前所使用的ROS2版本。

導航條件

深入學習Nav2，需瞭解ROS2機器人基礎知識，並配備仿真或實體機器人作為實踐環境。

ROS2基礎

• ROS2通信 ：瞭解ROS2中的節點（Nodes）、話題（Topics）、服務（Services）、動作（Actions）等基本概念，以及如何通過這些機制實現節點間的通信。
• 生命週期管理 ：熟悉ROS2中的生命週期節點（Lifecycle Nodes），理解節點的啟動、配置、激活、去激活、清理等狀態轉換過程，這對於管理複雜的ROS2系統至關重要。
• rviz2： 熟練使用rviz2進行可視化調試，包括如何設置不同的顯示類型（如點雲、地圖、路徑等），以及如何通過rviz2與ROS2節點進行交互（如設置導航目標點、觀察機器人狀態等）。
• URDF： 瞭解URDF（Unified Robot Description Format）文件，這是ROS中用於描述機器人模型的一種XML格式。熟悉如何編寫和修改URDF文件，以便在仿真或實際環境中準確地表示機器人結構。
• TF座標變換： 掌握TF（Transform）庫的使用，理解座標變換在機器人導航中的重要性。學會如何設置和查詢不同座標系之間的變換關係，以確保機器人能夠準確地定位自身和周圍環境。

實踐環境

• 仿真環境： 搭建一個ROS2兼容的仿真環境，並配置好相應的機器人模型、傳感器（如激光雷達、相機等）和仿真世界。確保仿真環境中的機器人能夠接收速度指令、反饋里程計消息、發佈傳感器數據和TF座標變換等。
• 實體機器人： 如果條件允許，準備一臺實體機器人。這臺機器人應該具備與仿真環境中相似的功能，即能夠接收速度指令、反饋里程計消息、發佈激光雷達等傳感器數據以及發佈TF座標變換。同時，確保實體機器人已經安裝了ROS2系統，並且已經配置好相應的驅動程序和算法庫。
• Nav2軟件包： 安裝並配置好Nav2軟件包及其依賴項。Nav2是ROS2中用於機器人導航的綜合性軟件包，它包含了路徑規劃、避障、局部和全局地圖維護等多個功能模塊。確保Nav2軟件包與您的ROS2版本兼容，並且已經根據需要進行了適當的配置。

通過掌握上述基礎知識並準備好實踐環境，您將能夠更好地學習和應用Nav2進行機器人導航任務的開發與調試。

導航參數參考

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

SLAM 定位與建圖

概念

SLAM（Simultaneous Localization and Mapping，即時定位與建圖）是機器人學和自動導航領域的一種關鍵技術，允許機器人在未知環境中繪製環境地圖。

在ROS 2下進行SLAM，通常涉及使用專門為ROS 2開發的SLAM相關軟件包。這些軟件包利用ROS 2的通訊框架（如話題、服務和action）來處理接收到的傳感器數據，包括激光雷達、相機、慣性測量單元等。

以下列舉了一些常見的在ROS2中使用的SLAM系統：

1. SLAM Toolbox ：SLAM Toolbox 是一種在 ROS 2 中普遍使用的 SLAM 解決方案，由 Steve Macenski 維護。它是為了替代原有的ROS中著名的gmapping包和Cartographer SLAM包而開發的，提供了2D激光SLAM領域中的多種功能。
2. Cartographer for ROS 2 ：Cartographer是由Google開發的一個2D和3D環境SLAM庫。儘管它最初是為ROS開發的，但已經有為ROS 2創建的適配版本，可以在ROS 2生態系統內使用。

要在ROS 2中開始一個SLAM項目，還需要具體的傳感器（例如激光雷達或攝像頭），計算機要有足夠的算力來運行SLAM算法，以及熟悉ROS 2節點、話題、服務、action、參數等知識，以實現有效的數據處理和通信。隨著項目的發展，可能還需要考慮動態重配置、3D建圖和路徑規劃等高級功能。

作用

必須先說明的是：SLAM與Nav2並沒有直接的依賴關係，它們是兩種相對獨立的技術框架。然而，在實際應用中，這兩者卻展現出緊密的聯繫與互補性。

獨立性： SLAM能夠獨立完成環境地圖的構建，無需Nav2的介入。它依靠傳感器數據來感知環境，並通過算法估計機器人的位置與姿態，從而構建出環境的地圖。與此同時，Nav2也具備自主導航的能力。即使沒有SLAM地圖作為輸入，它也能依賴其他來源的環境信息進行導航。

互補性： 儘管兩者在技術上各自獨立，但它們的結合卻帶來了顯著的優勢。Nav2能夠利用SLAM創建的精確地圖進行高效的路徑規劃，確保機器人能夠順利導航至目標位置。而SLAM則能借助Nav2的導航控制功能，在機器人移動過程中實時收集環境信息，從而進一步提高地圖構建的效率和準確性。

因此，儘管SLAM與Nav2在技術上各自獨立，但在構建完整的機器人導航系統中，它們的互補性使得機器人能夠在複雜環境中實現更加自主、精準的建圖或導航。

slam_toolbox概述

概念

SLAM Toolbox是一個基於開源軟件的用來構建 2D地圖 的工具集，旨在為研究人員和開發者提供一個快速構建和實現SLAM算法的平臺。它集成了多種常用的SLAM算法，並提供了豐富的數據處理和濾波函數，以及地圖構建和環境建模的工具。

功能

• 算法集成 包含基於卡爾曼濾波器和粒子濾波器的SLAM算法，以及基於圖優化的SLAM算法等。
• 數據處理 提供數據融合、數據預處理和異常檢測等功能，支持激光雷達、攝像頭、IMU等多種傳感器數據的處理。
• 地圖構建 利用傳感器數據和機器人運動信息構建準確的地圖，並進行更新和優化。
• 模塊化設計 支持用戶插入自己的算法或替換現有模塊，實現高度定製化的SLAM解決方案。

優點

• 開源性和靈活性 作為開源軟件，SLAM Toolbox提供了源代碼和文檔，用戶可以自由修改和擴展算法。
• 多種傳感器支持 支持激光雷達、攝像頭、IMU等多種傳感器數據，適應不同應用場景的需求。
• 高效性 利用現代C++的特性優化性能，確保在資源受限的硬件上也能高效運行。
• 廣泛的應用前景 適用於學術研究、無人機導航、自動駕駛汽車、室內機器人和工業自動化等多個領域。

slam_toolbox安裝

藉助於Advanced Packaging Tools(APT)包資源管理器，可以使用二進制的方式安裝slam_toolbox，安裝指令如下：

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

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

指令中的<ros2-distro>請替換成當前所使用的ROS2版本。

slam_toolbox節點說明

在slam_toolbox中常用的節點有兩個：sync_slam_toolbox_node和async_slam_toolbox_node。

sync_slam_toolbox_node：

這是一個同步節點，會等待所有傳感器數據到達後再處理，確保數據完整性和一致性，提高定位和建圖準確性。但同步處理可能導致延遲，更適合對數據一致性和準確性要求高、實時性要求不高的場景。

async_slam_toolbox_node：

與同步節點不同，這是一個異步節點，可以立即處理已接收的數據，減小延遲，提高響應速度。但異步處理可能導致數據不完全同步，定位和建圖結果可能不如同步方式準確。因此，它更適合對實時性要求高、對數據一致性和準確性要求相對較低的場景。

在選擇使用sync_slam_toolbox_node還是async_slam_toolbox_node時，需根據應用需求和環境權衡。若實時性關鍵且能接受一定定位或建圖誤差，選擇async_slam_toolbox_node。若對數據一致性和準確性要求較高，且實時性非首要考慮，則選擇sync_slam_toolbox_node。另外二者的主要區別在於數據處理的方式，而兩個節點發布的話題、訂閱的話題、發佈的服務以及使用的參數等都是一樣的。

1. 訂閱的話題

雖然不訂閱/odom,但是需要發佈/odom,以改變座標。

2. 發佈的話題

3. 發佈的服務

4. 參數
   1. 求解器參數

• 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 如果沒有找到可行的匹配，是否自動增加搜索網格大小

slam_toolbox基本使用

1. 準備工作

在src目錄下，請先調用如下指令在工作空間的src目錄下創建一個功能包：

ros2 pkg create mycar_slam_slam_toolbox --dependencies slam_toolbox

2. 編寫launch文件與參數文件

在功能包下，新建launch目錄和params目錄，launch目錄下新建online_sync_launch.py文件並輸入如下內容：

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

配置文件的內容需要根據實際情況進行動態調整。

3. 編輯配置文件

打開CMakeLists.txt 並輸入如下內容：

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

4. 編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select mycar_slam_slam_toolbox

5. 執行
   1. 請先調用如下指令啟動仿真環境：

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

   3. 然後在終端下進入當前工作空間，輸入如下指令：

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

   5. 啟動rviz2，將Fixed Frame設置為map，添加map插件並將話題設置為/map，即可顯示slam_toolbox創建的地圖了，當機器人運動時，地圖也會隨之更新。
   6. use_sim_time:=True參數表示使用仿真的時間。
   
     最後需要說明的是，本節內容使用的是sync_slam_toolbox_node 節點，即以同步方式建圖，而異步建圖節點async_slam_toolbox_node 的使用與同步類似。

我們用鍵盤控制節點去控制機器人跑滿整張地圖，

黑色：障礙物

白色：無障礙物區

灰色：未知區

SLAM是建圖與定位，以上就是建圖，那麼定位是啥呢？

定位就是Slam會發佈一個/tf，這裡面會包含機器人到map之間的座標變換。

這個/tf發佈的具體是map到odom的座標變換，所以需要你自己去處理odom和base_link之間的座標關係。

這樣的設計，可以讓整條座標樹是一個鏈式結構，避免base_link或者base_foot_print出現兩個父類，這樣計算量會變大。

注意事項：

1. 突然爛圖
   1. 如果你的環境很簡單，那麼跑圖的時候可能會突然爛圖，這個時候，大概率是do_loop_closing的問題，你把do_loop_closing設置成false再試試。 這個東西是SLAM用激光檢測形狀來匹配之前走過的路，如果他匹配到的形狀差不多他會把當前這段軌跡和以前的軌跡連起來，然後重新優化整張 pose graph，讓地圖整體更一致。 主要是為了防止odom會漂，因為 odom 會漂，機器人最後回到起點時，SLAM 可能覺得位置差了幾十釐米，但是可以通過激光雷達矯正。
   2. 在窄通道盡量直線過去，不要原地旋轉等等，這種窄通道炸了大概率是scan matching相關參數的問題。

cartographer概述

概念

Cartographer是Google推出的一套基於圖優化的激光SLAM算法庫，支持二維和三維地圖的構建。它結合了激光雷達和慣性測量單元（IMU）的數據，通過高效的算法實現實時、準確的定位和建圖。

功能

• 並行掃描匹配 利用並行計算技術加快掃描匹配速度，提高建圖效率。
• 位姿圖優化 通過圖優化技術估計機器人的姿態和地圖的拓撲結構，減少累積誤差。
• 實時地圖更新 在機器人移動過程中實時更新地圖，確保地圖的準確性和時效性。
• 迴環檢測 通過迴環檢測識別機器人曾經訪問過的區域，進一步減少累積誤差，提高地圖的全局一致性。
• 多傳感器融合 支持激光雷達、IMU、里程計等多種傳感器數據的融合，提高定位和建圖的精度。

優點

• 高效穩定 Cartographer的算法經過精心設計和優化，能夠在複雜環境中高效穩定地運行。
• 高精度 通過圖優化和迴環檢測技術提供高精度的定位和建圖結果。
• 靈活性 支持二維和三維地圖構建，適應不同應用場景的需求。
• 開源免費 Cartographer是開源項目，用戶可以免費獲取和使用其源代碼和文檔。
• 社區支持 擁有活躍的社區支持體系，用戶可以獲取來自全球開發者的幫助和支持。

cartographer安裝

藉助於Ubuntu的包資源管理器，可以使用二進制的方式安裝cartographer，安裝指令如下：

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

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

上述兩條安裝指令中，前者用於安裝cartographer的核心庫，這個包不直接與ROS2集成，而是作為一個獨立的算法庫存在，為地圖構建和定位提供底層的計算支持。後者則是cartographer在ROS2環境下的封裝，它提供了與ROS2系統的接口，使得Cartographer算法能夠在ROS2環境中運行。另外指令中的<ros2-distro>請替換成當前所使用的ROS2版本。

cartographer節點說明

在Cartographer框架中，cartographer_node和cartographer_occupancy_grid_node是兩個關鍵的節點，它們各自承擔著不同的角色和功能。詳細介紹如下。

cartographer_node：

主要負責訂閱來自各種傳感器的數據（如激光雷達、IMU、里程計等），並基於這些數據實時構建地圖。它採用子圖（submap）的方法來逐步構建和更新地圖，確保定位的準確性和建圖的實時性。

cartographer_occupancy_grid_node：

該節點負責接收cartographer_node發佈的子圖列表（/submap_list），並將其拼接成完整的柵格地圖（occupancy grid map），然後發佈這個地圖。這個節點是地圖生成的最終環節，它使得Cartographer能夠輸出人類可讀且易於可視化的地圖。

這兩個節點的協同工作，前者負責實時構建和更新地圖，後者則負責將子圖拼接成完整的柵格地圖併發布，使得Cartographer能夠高效地實現SLAM功能。

1. cartographer_node訂閱的話題

2. cartographer_node發佈的話題

3. cartographer_node發佈的服務

4. cartographer_node參數

cartographer_node節點需要接收一個參數配置文件，該配置文件包含了地圖構建、軌跡跟蹤等所需的各項參數。

5. cartographer_occupancy_grid_node訂閱的話題

6. cartographer_occupancy_grid_node發佈的話題

7. cartographer_occupancy_grid_node請求的服務

8. cartographer_occupancy_grid_node參數

cartographer_occupancy_grid_node節點需要配置地圖的分辨率和更新週期等參數，以確保生成的柵格地圖滿足特定的精度和實時性要求。

cartogarpher基本使用

1. 準備工作

在src目錄下，請先調用如下指令在工作空間的src目錄下創建一個功能包：

ros2 pkg create mycar_slam_cartographer --dependencies cartographer

2. 編寫launch文件與參數文件

在功能包下，新建launch目錄和params目錄，launch目錄下新建cartographer.launch.py文件並輸入如下內容：

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

配置文件的內容需要根據實際情況進行動態調整。

3. 編輯配置文件

打開CMakeLists.txt 並輸入如下內容：

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

4. 編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select mycar_slam_cartographer

5. 執行

（1）請先調用如下指令啟動仿真環境：

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

（2）然後在終端下進入當前工作空間，輸入如下指令：

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

（3）啟動rviz2，將Fixed Frame設置為map，添加map插件並將話題設置為/map，即可顯示創建的地圖了，當機器人運動時，地圖也會隨之更新。

地圖服務

SLAM建圖時，地圖數據是保存在內存中的，這也意味著，一旦節點關閉，數據也會一併被釋放，而更合理的實現應該是將構建的地圖序列化到的磁盤以持久化存儲，並且後期還要通過反序列化讀取磁盤的地圖數據以做其他操作。在Nav2中已經已經封裝好了地圖序列化和反序列化的相關功能包，該包是：nav2_map_server。

在nav2_map_server中，可以通過話題和服務接口與Nav2系統的其餘部分進行交互。nav2_map_server包下有兩個重要的節點，分別是map_saver_cli和map_server，通過map_saver_cli節點則可以保存地圖，而map_server節點則可以在啟動時顯示地圖。

保存地圖(序列化)

地圖保存節點說明

在nav2_map_server中的地圖保存節點是map_saver_server，該節點相關信息如下。

1. 訂閱的話題

2. 參數

• save_map_timeout 保存地圖操作的最大等待時間。
• free_thresh_default 柵格單元被認為未被佔用的概率閾值。
• occupied_thresh_default 柵格單元被認為佔用的概率閾值。
• map_subscribe_transient_local 節點重啟後消息不保留，默認為 true。

3. map_saver_cli

另外，而為了便於使用，在map_saver_server的基礎之上還封裝了一個名為map_saver_cli的可執行程序，它可以以實參的方式更方便的設置地圖保存相關數據，並且後續執行時也是調用map_saver_cli，其實參列表如下：

• -t 訂閱的地圖話題。
• -f 地圖存儲路徑。
• --occ 柵格單元被認為佔用的概率閾值。
• --free 柵格單元被認為未被佔用的概率閾值。
• --fmt 圖片格式。
• --mode 地圖模式，trinary(默認)或scale或raw。

地圖保存基本操作

準備工作

請先啟動仿真或實體機器人，然後啟動SLAM相關節點，實現基本的建圖功能。

保存地圖

SLAM建圖完畢，在終端下進入工作空間，調用如下指令保存地圖：

ros2 run nav2_map_server map_saver_cli -f map/my_map

上述指令將訂閱/map話題，並把/map話題裡的數據保存為文件，在工作空間下的map目錄(需要自行創建該目錄，否則將會拋出異常)中，生成兩個文件，分別名為：my_map.yaml和my_map.pgm。

地圖接口

在Nav2中地圖相關的接口主要有兩個：

• nav_msgs/msg/MapMetaData 地圖元數據，包括地圖的寬度、高度、分辨率等。
• nav_msgs/msg/OccupancyGrid 地圖柵格數據，一般會在rviz中以圖形化的方式顯示。

nav_msgs/msg/MapMetaData

調用指令ros2 interface show nav_msgs/msg/MapMetaData查看接口格式，顯示如下內容（註釋已漢化）：

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

調用指令ros2 interface show nav_msgs/msg/OccupancyGrid查看接口格式，顯示如下內容（註釋已漢化）：

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

地圖存儲格式

在 地圖保存基本操作 一節中，地圖保存後後生成兩個文件，這兩個文件就是用來存儲序列化後的地圖數據的。其中，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

參數解釋：

• image 被描述的圖片資源路徑，可以是絕對路徑也可以是相對路徑。
• resolution 圖片分片率(單位: m/像素)。
• origin 地圖中左下像素的二維姿態，為（x，y，z），偏航為逆時針旋轉（偏航=0 表示無旋轉）。
• occupied_thresh 佔用概率大於此閾值的像素被視為完全佔用。
• free_thresh 佔用率小於此閾值的像素被視為完全空閒。
• negate 是否應該顛倒白色/黑色 自由/佔用的語義。
• mode 地圖模式，trinary(默認)或scale或raw。

讀取地圖(反序列化)

地圖讀取節點說明

在nav2_map_server中的地圖讀取節點是map_server，該節點相關信息如下。

發佈的話題

參數

• frame_id 地圖座標系名稱。
• topic_name 話題名稱。
• yaml_filename 地圖數據源。

地圖讀取基本操作

準備工作

請先調用如下指令在工作空間的src目錄下創建一個功能包：

ros2 pkg create mycar_map_server --dependencies nav2_map_server

在功能包下，新建launch文件夾，並在CMakeLists.txt中添加如下配置：

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

讀取地圖

使用map_server讀取地圖時，常用的方式有兩種，分別是使用終端指令與launch文件集成。兩種方式效果一致，都可以以話題的方式發佈地圖消息。

colcon build
source ./install/setup.bash

方式1：終端指令

請在終端下進入工作空間，輸入如下指令：

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

由於map_server是具有生命週期的節點，所以接下來還需要對節點進行配置和激活，請新開終端執行如下指令：

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

執行完畢若無異常，再調用ros2 topic list即可查看到/map話題了，說明地圖消息已經被髮布了。

方式2：launch集成

方式1需要手動設置map_server生命週期，步驟稍顯繁瑣，因此，我們還可以將該節點集成進launch文件，以簡化啟動步驟。在launch目錄下新建名為map_server.launch.py的文件，並輸入如下內容：

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])

在該文件中，使用了功能包中nav2_lifecycle_manager中的lifecycle_manager組件，該組件可以自動的配置、激活其所管理的具有生命週期的節點。構建功能包後並執行該launch文件，其最終效果與方式一類似。

ros2 launch mycar_map_server map_server.launch.py

顯示地圖

打開rviz2，然後添加Map插件，並將話題設置為/map，並將該話題的Durability Policy

選項設置為Transient Local，就可以正常顯示地圖數據了。

AMCL自適應蒙特卡洛定位

定位是機器人在已知地圖上確定自身位置的過程，為機器人的導航提供了基礎信息。

Nav2中的定位技術技術稱之為AMCL，全稱Adaptive Monte Carlo Localization，即自適應蒙特卡洛定位，是一種基於粒子濾波器的定位算法。它通過蒙特卡洛方法進行自適應定位，利用對機器人周圍環境的感知和觀測數據的分析，來確定機器人在環境中的位置和姿態。在Nav2中對應的功能包為nav2_amcl。

在AMCL中，粒子濾波器的核心思想是使用一組粒子（樣本）來代表機器人在地圖上的可能位置。每個粒子都有一個權重，表示該粒子所代表的位置的置信度。算法會根據機器人的運動模型和傳感器數據來更新這些粒子的位置和權重。隨著時間的推移，粒子會逐漸收斂到機器人實際位置附近，從而實現對機器人位置的準確估計。

定位節點說明

功能包nav2_amcl中的核心節點為amcl。該節點相關信息如下。

1.訂閱的話題

2.發佈的話題

3.發佈的服務

4.參數

通用參數

• bond_disable_heartbeat_timeout: 設置為true時，禁用amcl節點與其他節點之間基於心跳的超時檢測。這通常用於當節點之間的連接非常穩定，不需要頻繁的心跳檢測來確認連接狀態時。
• base_frame_id: 定義機器人基座標系的ID，通常是base_link或類似的名稱。
• global_frame_id: 定義全局地圖座標系的ID，通常是map。
• odom_frame_id: 定義里程計座標系的ID，通常是odom。
• tf_broadcast: 設置為true時，amcl節點會發布從里程計座標系到全局地圖座標系的變換。
• transform_tolerance: 設置TF變換的容忍度，用於處理TF樹中的時間不一致性。
• use_sim_time: 設置為true時，amcl將使用ROS 2的模擬時間（如果可用）。這在仿真環境中很有用。

激光模型參數

• laser_model_type: 設置激光模型類型，likelihood_field是一種常用的模型，它考慮了激光束擊中障礙物的概率。
• laser_max_range和laser_min_range: 分別設置激光雷達的最大和最小探測範圍。
• laser_likelihood_max_dist: 設置激光模型考慮的最大距離，超過這個距離的數據將被忽略。
• do_beamskip和相關參數（beam_skip_distance、beam_skip_threshold、beam_skip_error_threshold）: 這些參數用於控制是否跳過某些激光束的處理，以減少計算量。然而，do_beamskip被設置為false，意味著不跳過任何激光束。

粒子濾波器參數

• alpha1到alpha5: 這些參數用於控制粒子濾波器中的權重更新過程，但它們的具體作用可能因amcl的實現而異。在標準的amcl實現中，這些參數可能不是直接使用的。
• max_particles和min_particles: 分別設置粒子濾波器的最大和最小粒子數。
• resample_interval: 設置在重採樣前需要的濾波更新次數。
• pf_err和pf_z: 這些參數用於控制粒子濾波器的性能，但它們的具體作用可能依賴於amcl的實現細節。

初始位姿參數

• initial_pose: 定義了機器人的初始位姿（x, y, yaw, z），但在實際使用中，如果set_initial_pose被設置為true，則這個初始位姿可能會被通過服務請求設置的初始位姿所覆蓋。
• set_initial_pose: 設置為true時，允許通過服務請求來設置機器人的初始位姿。
• always_reset_initial_pose: 設置為false時，表示不會在每個定位會話開始時自動重置初始位姿。

其他參數

• 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: 這些參數定義了激光模型中的概率分佈，用於計算激光束擊中障礙物或隨機位置的概率。

定位節點基本操作

1.準備工作

請先調用如下指令在工作空間的src目錄下創建一個功能包：

ros2 pkg create mycar_localization --dependencies nav2_amcl mycar_map_server

2.編寫launch文件與參數文件

在功能包下，新建launch和params文件夾，在launch目錄下新建名為mycar_loca.launch.py的文件，並輸入如下內容：

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,
    ])

在上述代碼中，創建了amcl節點，並從params目錄加載了名為amcl.yaml的配置文件，且由於amcl也是擁有生命週期的節點，所以將其添加進了生命週期管理器。最後，定位必須依賴於地圖信息，因此又包含了 地圖讀取基本操作 中的launch文件，以加載地圖。

在params目錄下新建名為amcl.yaml的文件，並輸入如下內容：

/**:
  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

關於參數的具體含義，可以參考 定位節點說明 中參數相關內容。

3.編輯配置文件

打開CMakeLists.txt 並輸入如下內容：

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

4.編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select mycar_localization

5.執行

（1）請先調用如下指令啟動仿真環境：

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

（2）然後在終端下進入當前工作空間，輸入如下指令：

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

（3）啟動鍵盤控制節點以作備用：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

（4）在rviz2中，將Fixed Frme設置為map，添加TF插件，按照 地圖讀取基本操作 添加並顯示地圖。

接下來，點擊rviz2菜單欄的2D Pose Estimate在地圖中為機器人設置一個初始位姿。

這裡需要給一個大概的機器人位置和機器人的朝向，不是很準確也可以，機器人會在運動中逐漸通過AMCL校準。

先點擊2D Pose Estimate，左鍵在地圖上點擊機器人所在位置，長摁別鬆手，鼠標往機器人朝向的位置劃，出現下方這種綠色箭頭，再鬆手即可。

再添加ParticleCloud插件，將話題設置為/particle_cloud，並將話題下Reliability Policy設置為Best Effort，最後使用鍵盤控制機器人運動時，會發現，機器人周邊會出現點雲，並且隨著機器人的運動，點雲會出現不同程度的收斂或發散。

即便你的機器人撞牆了，rviz2和Gazebo的機器人位置完全偏移了，只要再讓機器人運動一會兒，機器人位置會被重新預估出來，AMCL非常強。

如上圖位置已經完全偏移了。

經過一段時間行駛，姿態位置再次被預估成功。

導航服務器

本節將介紹導航服務器中核心節點，實現基本導航功能，並將導航與SLAM集成，以實現自主探索建圖。

下方網站是官方NAV2的各個節點的參數說明(調參的時候非常常用的網站):

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

看不懂的英文可以用沉浸式翻譯這個chrome插件來翻譯,可以進行中英文對照翻譯,非常好用.

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

不會用這個插件的話,也可以看魚香ROS翻譯好的版本(但是網站不穩定):

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

生命週期管理節點說明

Nav2中通過的nav2_lifecycle_manager功能包下的lifecycle_manager節點管理其他節點的生命週期狀態轉換。lifecycle_manager節點通過ROS 2的生命週期節點（Lifecycle Node）機制，提供了一種標準化的方法來控制Nav2中各個節點的狀態轉換。這些狀態包括未配置（Unconfigured）、非活動（Inactive）、活動（Active）和結束（Finalized）等。通過精細控制這些狀態轉換，lifecycle_manager節點能夠確保Nav2系統的各個部分在正確的時機啟動、運行和停止，從而提高系統的可靠性和穩定性。

參數

• /bond_disable_heartbeat_timeout: 該參數與節點之間的通信綁定（bonding）有關。節點之間的通信可以通過綁定來增強可靠性，而心跳超時是檢測節點是否仍然活躍的一種方式。設置為false意味著啟用心跳超時檢測。
• attempt_respawn_reconnection: 取值為bool值，設置為true時，表示如果管理的節點意外終止，生命週期管理器將嘗試重新啟動它。
• autostart: 取值為bool值，設置為true時，表示在生命週期管理器啟動時，它將自動嘗試啟動其管理的節點。
• bond_respawn_max_duration: 重新連接或重新啟動節點時的最大持續時間。
• bond_timeout: 與節點之間的通信綁定超時有關。如果在這個時間內沒有收到來自另一個節點的消息，則認為該節點已經斷開連接。
• diagnostic_updater:
  • period: 診斷更新器的更新週期。診斷更新器用於收集和發佈有關節點狀態的診斷信息，定義了這些信息更新的頻率。
• node_names: 由該生命週期管理器管理的節點名稱列表。
• use_sim_time:是否使用仿真實踐。

行為樹節點說明

行為樹（BT） 是一種在智能體（如機器人或電腦遊戲中的虛擬實體）中構建不同任務之間切換結構的方式。它是一種形式化的圖形建模語言，以層次化的節點組織為特徵，用於描述和規劃複雜系統中各種實體的交互和決策。nav2_bt_navigator功能包下的/bt_navigator是Nav2中的行為樹導航器節點，它實現了基於行為樹的導航策略。行為樹是一種用於描述複雜行為的樹狀結構，通過組合不同的行為節點，可以靈活地定義機器人的導航行為，包括路徑規劃、避障、恢復等。以下是官網。

https://www.behaviortree.dev/

https://arxiv.org/abs/1709.00084

github鏈接如下：

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

BT可視化工具Groot2下載：

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

1. 訂閱的話題

2. 請求的Action

3. 參數

• use_sim_time: 指定是否使用模擬時間而非實際時間。
• global_frame: 定義全局座標系的名稱，通常為地圖座標系。
• robot_base_frame: 指定機器人基座的座標系名稱。
• odom_topic: 里程計數據的ROS話題名稱。
• bt_loop_duration: 行為樹執行循環的持續時間（單位可能根據實現而異）。
• default_server_timeout: 導航服務器操作的默認超時時間。
• enable_groot_monitoring: 啟用或禁用Groot監控功能。
• groot_zmq_publisher_port: Groot監控中ZeroMQ發佈者的端口號。
• groot_zmq_server_port: Groot監控中ZeroMQ服務器的端口號。
• plugin_lib_names: Nav2 Jazzy 官方示例說明該參數用於添加自定義 BT 插件；Nav2 內置 BT 插件會自動加載，所以沒有自定義 BT 插件時不要配置空列表。

規劃器節點說明

在Nav2 導航框架中nav2_planner功能包下的planner_server節點，負責處理路徑規劃請求，生成從當前位置到目標位置的路徑。該節點在執行時需要依賴於/global_costmap/global_costmap節點提供的地圖消息。

1. planner_server發佈的話題

2. planner_server 參數

• /bond_disable_heartbeat_timeout: 這個參數控制是否禁用心跳超時檢測，在ROS 2中，節點之間的通信綁定（bonding）機制用於增強通信的可靠性。心跳超時是檢測節點是否仍然活躍的一種機制。如果該參數設置為true，則表示禁用了心跳超時檢測，這可能在某些特定的網絡環境下或當確信節點間通信非常穩定時使用。
• GridBased: 這是一個配置塊，專門用於設置基於網格的規劃器的參數。基於網格的規劃器通常使用地圖的網格化表示來規劃路徑。
  • allow_unknown: 控制規劃器是否允許在地圖的未知（即未探索）區域中規劃路徑。
  • plugin: 指定使用的規劃器插件。
  • tolerance: 設置規劃路徑時的容忍度，通常用於考慮機器人尺寸和定位的不確定性。
  • use_astar: 控制是否使用A* 算法進行路徑規劃*。A** 算法是一種啟發式搜索算法，能夠找到從起點到終點的最短路徑。
  • use_final_approach_orientation: 控制規劃器是否在路徑的終點附近考慮機器人的最終朝向。
• expected_planner_frequency: 這個參數表示對規劃器生成新路徑的頻率的預期值。它幫助系統監控規劃器的性能，並可能用於調試或性能優化。
• planner_plugins: 這是一個列表，指定了可用的規劃器插件。在這個例子中，它只包含了一個GridBased規劃器，但理論上可以包含多個不同類型的規劃器，以適應不同的導航需求。
• use_sim_time: 這個參數控制是否使用模擬時間。在仿真環境中，時間是由仿真軟件控制的，而不是由實際的物理時鐘控制的。將此參數設置為true允許節點在仿真環境中正常運行，而無需依賴實際的系統時間。這對於開發和測試導航算法非常有用。

3. /global_costmap/global_costmap訂閱的話題

4. /global_costmap/global_costmap發佈的話題

5. /global_costmap/global_costmap參數

• /bond_disable_heartbeat_timeout: 控制是否禁用心跳超時檢測，這在節點間通信綁定時用於監控對方節點的活躍度。
• always_send_full_costmap: 控制是否總是發送完整的代價地圖信息，而不是僅發送變化的部分。
• clearable_layers: 列出可以被清除的代價圖層，這些圖層中的障礙物信息可以通過某種方式（如傳感器數據）被更新或清除。
• filters: 定義應用於代價地圖的過濾器列表，用於預處理或修改地圖數據。
• footprint: 指定機器人在地圖上的足跡形狀，即機器人佔據的空間範圍。
• footprint_padding: 為機器人的足跡添加額外的填充空間，以考慮機器人運動時的額外空間需求。
• global_frame: 定義全局代價地圖所使用的參考座標系。
• height和width: 定義全局代價地圖的高度和寬度（以單元格數計）。
• inflation_layer: 配置膨脹層的參數，膨脹層用於在障礙物周圍增加一定寬度的“緩衝區”，以避免機器人與障礙物過近。
  • cost_scaling_factor: 膨脹成本的縮放因子。
  • enabled: 控制膨脹層是否啟用。
  • inflate_around_unknown: 控制是否在未知空間周圍進行膨脹。
  • inflate_unknown: 控制是否將未知空間視為障礙物並進行膨脹。
  • inflation_radius: 膨脹的半徑。
  • plugin: 指定使用的膨脹層插件。
• lethal_cost_threshold: 定義代價地圖中視為“致命”障礙物的成本閾值。
• map_topic: 指定訂閱以獲取地圖信息的ROS話題。
• observation_sources: 定義代價地圖的觀測源，即哪些傳感器或數據源用於更新地圖。
• obstacle_layer: 配置障礙物層的參數，障礙物層負責處理傳感器觀測到的障礙物信息。
  • combination_method: 定義如何組合多個觀測源的信息。
  • enabled: 控制障礙物層是否啟用。
  • footprint_clearing_enabled: 控制是否清除機器人足跡內的障礙物信息。
  • max_obstacle_height和min_obstacle_height: 定義障礙物的高度範圍。
  • observation_sources: 指定障礙物信息的來源。
  • plugin: 指定使用的障礙物層插件。
  • scan: 包含與激光掃描相關的配置，如掃描數據的處理方式。
• origin_x和origin_y: 定義全局代價地圖原點的座標。
• plugins: 列出啟用的代價地圖插件，這些插件定義瞭如何構建和更新代價地圖。
• publish_frequency: 定義發佈代價地圖的頻率。
• resolution: 定義代價地圖的分辨率，即每個單元格代表的實際物理尺寸。
• robot_base_frame: 定義機器人基座的參考座標系。
• robot_radius: 定義機器人的半徑，用於計算機器人在地圖上的佔用空間。
• rolling_window: 控制是否使用滾動窗口（即動態變化的地圖區域）而非固定大小的地圖。
• static_layer: 配置靜態層的參數，靜態層負責處理地圖中的靜態障礙物信息。
  • enabled: 控制靜態層是否啟用。
  • map_subscribe_transient_local: 控制是否訂閱瞬態本地地圖更新。
  • map_topic: 指定靜態地圖的ROS話題（如果不同於全局地圖）。
  • plugin: 指定使用的靜態層插件。
  • subscribe_to_updates: 控制是否訂閱地圖更新。
  • transform_tolerance: 定義座標變換的容忍度。
• track_unknown_space: 控制是否跟蹤地圖中的未知空間。
• transform_tolerance: 定義在座標變換過程中允許的誤差範圍。
• trinary_costmap: 控制是否使用三態代價地圖（空閒、佔用、未知），而不是僅使用二態（空閒、佔用）。
• unknown_cost_value: 定義在代價地圖中表示未知空間的值。
• update_frequency: 定義更新代價地圖的頻率。
• use_maximum: 控制是否使用多個觀測源中的最大值來更新代價地圖。
• use_sim_time: 控制是否使用仿真時間（在仿真環境中很有用）。
• voxel_layer: 配置體素層的參數，體素層使用體素網格來表示三維空間中的障礙物信息。
  • enabled: 控制體素層是否啟用。
  • footprint_clearing_enabled: 控制是否清除機器人足跡內的體素信息。
  • mark_threshold和unknown_threshold: 定義將體素視為障礙物或未知的閾值。
  • max_obstacle_height和min_obstacle_height: 定義體素表示的障礙物的高度範圍。
  • observation_sources: 指定體素信息的來源。
  • origin_z: 定義體素網格在Z軸上的原點。
  • plugin: 指定使用的體素層插件。
  • publish_voxel_map: 控制是否發佈體素地圖。
  • scan: 包含與激光掃描相關的配置，如掃描數據的處理方式。
  • z_resolution和z_voxels: 定義體素網格在Z軸上的分辨率和體素數。

控制器節點說明

在Nav2導航系統中nav2_controller功能包的controller_server負責處理導航任務中的控制請求，確保機器人能夠按照規劃的路徑進行移動。其主要功能是根據nav2_planner模塊計算出的全局或局部路徑，生成速度、方向控制的命令，即控制機器人沿著規劃好的路徑行走。該節點在執行時還需要依賴於/local_costmap/local_costmap節點提供的地圖消息。

1.controller_server訂閱的話題

2.controller_server發佈的話題

3.controller_server參數

• FollowPath: 這個部分定義了一個名為FollowPath的插件或配置集，它可能是一個路徑跟隨行為或算法的配置。它包含了多個子參數和子配置，用於定義如何跟隨路徑。
  • BaseObstacle: 定義了基本的障礙物評估參數，用於在路徑跟隨過程中避免障礙物。
    • class: 指定了類的名稱，這裡是BaseObstacle，表示這是一個基本障礙物評估組件。
    • scale: 定義了該障礙物評估在整體評估中的權重或影響程度。
    • sum_scores: 指示是否累加多個障礙物的分數，false可能表示使用最大值或其他邏輯。
  • GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal, Oscillation: 這些都是路徑跟隨過程中的不同評估或行為組件，每個都有其特定的參數和用途，如對齊目標、保持與目標或路徑的距離、減少振盪等。
  • acc_lim_theta, acc_lim_x, acc_lim_y: 這些參數定義了機器人在不同方向上的加速度限制。
  • critics: 指定了哪些評估組件（或“批評家”）將被用於路徑跟隨決策。
  • debug_trajectory_details: 指示是否發佈軌跡的詳細調試信息。
  • 其他與速度、加速度、時間粒度、軌跡生成等相關的參數，共同定義了路徑跟隨算法的行為和性能。
• controller_frequency: 指定了控制器（可能是FollowPath或其他控制器）的運行頻率，以赫茲為單位。
• controller_plugins: 指定了將要使用的控制器插件列表，這裡只包含了FollowPath。
• failure_tolerance: 定義了容忍失敗的時間或距離，用於在評估控制器是否失敗時提供一定的緩衝。
• general_goal_checker: 定義了一個通用的目標檢查器，用於確定機器人是否已達到其目標位置和方向。
• goal_checker_plugins: 指定了將要使用的目標檢查器插件列表。
• min_theta_velocity_threshold, min_x_velocity_threshold, min_y_velocity_threshold: 這些定義了機器人在不同方向上的最小速度閾值，低於這些閾值可能被視為停止或靜止。
• odom_topic: 指定了里程計信息的ROS主題。
• progress_checker: 定義了一個進度檢查器，用於評估機器人是否在向目標移動。
• qos_overrides: 定義了ROS服務或主題的QoS（服務質量）覆蓋設置，用於調整消息傳遞的可靠性和性能。
• speed_limit_topic: 指定了速度限制信息的ROS主題。
• use_sim_time: 指示是否使用模擬時間，這在ROS仿真環境中非常有用。

4./local_costmap/local_costmap訂閱的話題

5./local_costmap/local_costmap發佈的話題

6./local_costmap/local_costmap參數

• /bond_disable_heartbeat_timeout: 是否禁用節點間的心跳超時檢查。當設置為true時，表示禁用該功能，可能用於減少網絡通信量或適應特定網絡環境。
• always_send_full_costmap: 是否總是發送完整的成本圖。當設置為true時，節點將不依賴於增量更新，而是始終發送完整的成本圖數據。
• clearable_layers: 指定可以被清除的層列表。在這個例子中，包括obstacle_layer、voxel_layer和range_layer，這意味著這些層中的障礙物數據可以被清除。
• filters: 用於指定應用於成本圖的過濾器列表。此處為空，表示沒有應用任何過濾器。
• footprint: 機器人的足跡多邊形，定義了機器人在二維空間中的物理佔用區域。
• footprint_padding: 足跡的填充量，用於在計算成本圖時給機器人足跡添加額外的空間。
• global_frame: 全局參考座標系的名稱，通常用於定位和導航任務。
• height: 成本圖的高度（以單元格數量計）。
• inflation_layer: 膨脹層的配置，用於在障礙物周圍添加一定範圍的膨脹區域，使機器人與障礙物保持安全距離。
• lethal_cost_threshold: 致命成本閾值，超過此閾值的成本值表示不可通行的區域。
• map_topic: 訂閱的地圖主題名稱，用於獲取全局地圖信息。
• observation_sources: 觀察源的配置，用於指定哪些傳感器數據將被用於更新成本圖。此處為空字符串，可能是默認值或配置方式的不同。
• obstacle_layer: 障礙物層的配置，用於處理來自傳感器（如激光雷達）的障礙物數據。
• origin_x,origin_y: 成本圖原點的X和Y座標，定義了成本圖在全局座標系中的位置。
• plugins: 啟用的插件列表，定義了成本圖使用的不同層（如障礙物層、膨脹層等）。
• publish_frequency: 成本圖的發佈頻率（以Hz為單位）。
• resolution: 成本圖的分辨率（以米/單元格計）。
• robot_base_frame: 機器人基座的參考座標系名稱，用於定位機器人。
• robot_radius: 機器人的半徑，用於在成本圖中表示機器人的物理尺寸。
• rolling_window: 是否使用滾動窗口。當設置為true時，成本圖將隨著機器人的移動而更新其位置和範圍。
• track_unknown_space: 是否跟蹤未知空間。在某些情況下，這可能用於處理未探索或未知的區域。
• transform_tolerance: 變換容差，定義了接受變換的時間差和角度差的閾值。
• trinary_costmap: 是否使用三態成本圖（通常是自由、佔用、未知）。
• unknown_cost_value: 未知區域在成本圖中的成本值。
• update_frequency: 成本圖的更新頻率（以Hz為單位），不同於發佈頻率。
• use_maximum: 是否在多個源提供相同位置的成本信息時使用最大值。
• use_sim_time: 是否使用模擬時間而非系統時間。這在仿真環境中很有用。
• voxel_layer: 體素層的配置，用於將三維空間劃分為體素（體積像素），以提高成本圖的處理效率。

恢復器節點說明

恢復行為是機器人導航過程中一個至關重要的部分，它允許機器人在遇到障礙、卡住或其他導航問題時採取一系列預定義的動作來嘗試恢復。在Nav2中由nav2_behaviors功能包的behavior_server實現這一功能。

1.訂閱的話題

2.發佈的話題

3.提供的Action服務器

4.參數

• use_sim_time: 該參數指定是否使用模擬時間而非實際時間。這在仿真環境中非常有用，因為仿真環境可以加速或減速時間流逝，而不需要等待實際時間的流逝。
• global_frame: 定義全局座標系的名稱，該座標系通常用於導航任務中的定位和路徑規劃。在這裡，它被設置為odom，意味著使用里程計數據來作為全局座標系的參考。
• robot_base_frame: 指定機器人基座的座標系名稱。這是機器人上用於定位和運動控制的參考點，通常與機器人的物理中心或驅動輪的中心相對應。
• odom_topic: 這是一個ROS話題名稱，用於發佈里程計數據。里程計數據包含了機器人隨時間推移的位置和姿態變化信息，是導航和定位系統的關鍵輸入之一。
• /bond_disable_heartbeat_timeout：這個參數可能用於配置ROS節點之間的心跳檢測機制。將其設置為true可能意味著禁用或調整心跳超時的行為，以便在特定情況下（如仿真環境）避免不必要的超時錯誤。
• assisted_teleop,backup,drive_on_heading,spin,wait: 這些是行為樹中可能使用的行為插件的配置項。每個插件都定義了機器人可以執行的一種特定行為，如輔助遙操作、後退、按指定方向行駛、原地旋轉和等待。
• behavior_plugins: 列出了在行為樹中可用的行為插件名稱。
• cmd_vel_teleop: 指定了用於遙操作的速度控制命令的ROS話題名稱。
• costmap_topic: 定義了局部代價地圖的ROS話題名稱，代價地圖用於表示環境中的障礙物和可通行區域。
• cycle_frequency: 定義了導航系統更新其狀態和規劃新路徑的頻率（以赫茲為單位）。
• max_rotational_vel,min_rotational_vel,rotational_acc_lim: 這些參數定義了機器人旋轉時的最大速度、最小速度和加速度限制。
• projection_time: 與代價地圖的更新或預測未來障礙物位置有關的時間參數。
• footprint_topic: 定義了發佈機器人足跡（即機器人佔據的空間）的ROS話題名稱。
• simulate_ahead_time,simulation_time_step: 這些參數與仿真環境相關，可能用於控制仿真過程中的時間流逝和步長。
• transform_tolerance: 定義了座標變換時的容差範圍，用於處理不同座標系之間的微小差異。

航點跟隨節點說明

在Nav2 導航堆棧中，nav2_waypoint_follower包下的/waypoint_follower節點負責跟蹤由路徑規劃器生成的一系列航點（waypoints），以確保機器人能夠沿著預定的路徑安全、準確地移動。該節點的主要功能是根據當前機器人位置和速度信息，以及由路徑規劃器（如nav2_global_planner和nav2_local_planner）提供的航點列表，計算出控制指令來控制機器人的運動。這些控制指令可能包括線性和角速度命令，或者更具體的運動學或動力學命令，具體取決於機器人的類型和配置。

1.提供的Action服務器

2.請求的Action服務

3.參數

• use_sim_time: 指定是否使用模擬時間而非實際時間進行節點的計時和同步。這在仿真環境中特別有用，因為仿真環境可能無法提供與真實時間完全同步的時鐘。
• loop_rate: 定義了節點的主循環速率，即節點每秒執行其主要任務（如處理數據、發佈信息等）的次數。這個參數對於控制節點的響應性和資源使用非常重要。
• stop_on_failure: 指明當導航任務遇到無法克服的障礙或達到其他失敗條件時，節點是否應該停止執行。這對於確保在失敗情況下系統能夠安全地停止並等待進一步指令很重要。
• bond_disable_heartbeat_timeout: 涉及節點間通信的可靠性機制。Bond是ROS 2中用於節點間穩定通信的一種機制，其中心跳信號用於檢測節點是否仍然活躍。將此參數設置為true會禁用心跳超時檢測，這可能在某些特定的網絡配置或應用場景中是有用的。
• waypoint_task_executor_plugin: 指定了在執行路徑點導航任務時要使用的插件。路徑點導航通常涉及一系列預先定義的點，機器人需要按順序訪問這些點。這個參數允許用戶指定用於執行這種類型任務的特定插件或算法。
• wait_at_waypoint: 這是一個複合參數，用於配置在路徑點等待的特定行為。
  • enabled: 啟用或禁用在到達每個路徑點時等待的功能。
  • plugin: 指定實現等待功能的插件類型。這允許用戶根據需要選擇不同的等待策略或行為。
  • waypoint_pause_duration: 定義了在每個路徑點處等待的持續時間（以毫秒為單位）。這可以用於確保機器人在移動到下一個路徑點之前已經穩定或已經完成了某些操作。

路徑平滑節點說明

在Nav2框架中nav2_smoother功能包下的smoother_server節點通過加載和運行各種平滑器插件，對規劃出的路徑進行平滑處理，使得機器人能夠更流暢、連續且安全地移動。這一功能對於提高機器人的導航性能和減少硬件磨損具有重要意義。

1.訂閱的話題

2.發佈的話題

3.提供的Action服務器

4.參數

• /bond_disable_heartbeat_timeout: 指示是否禁用Bond的心跳超時功能。在分佈式系統中，Bond用於管理節點間的連接和心跳，此參數用於調整心跳相關的行為。
• costmap_topic: 代價地圖數據的ROS話題名稱，通常是全局代價地圖的原始數據。
• footprint_topic: 機器人足跡（即機器人在地圖上的佔用區域）的ROS話題名稱，用於在全局代價地圖中表示機器人的物理尺寸。
• robot_base_frame: 指定機器人基座的座標系名稱，這是機器人導航中用於定位和移動的參考點。
• simple_smoother:
  • do_refinement: 指示是否啟用路徑的細化（或進一步優化）過程。
  • max_its: 平滑過程中允許的最大迭代次數，用於控制平滑算法的收斂時間。
  • plugin: 平滑插件的類型，這裡是nav2_smoother::SimpleSmoother，表示使用簡單的平滑算法。
  • tolerance: 平滑算法的收斂容差，當路徑變化小於此值時，認為平滑過程已完成。
  • w_data: 平滑過程中數據項（如障礙物距離）的權重。
  • w_smooth: 平滑過程中平滑項（如路徑曲率）的權重。
• smoother_plugins: 定義的平滑插件列表，這裡列出了simple_smoother，表示將使用此插件進行路徑平滑。
• transform_tolerance: 座標變換的容差，用於處理不同座標系之間的轉換時的不確定性。
• use_sim_time: 指定是否使用模擬時間而非實際時間。在仿真環境中，這通常設置為true，以匹配仿真器的虛擬時間；在真實環境中，應設置為false以使用ROS系統的實際時間。

速度平滑節點說明

Nav2框架中的nav2_velocity_smoother包下的velocity_smoother節點主要負責平滑由Nav2框架發送給機器人控制器的速度指令。其核心功能是實現速度和加速度平滑。這一功能對於確保機器人在導航過程中的穩定性和安全性至關重要。

1.訂閱的話題

2.發佈的話題

3.參數

• 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節點將停止發佈速度指令，並可能發送零速度指令以停止機器人運動。這是為了防止在失去速度控制時機器人繼續移動。

導航功能集成

本節 Jazzy 版本的配置依據來自 Jazzy 官方安裝路徑裡的示例文件和 Nav2 官方配置文檔：/opt/ros/jazzy/share/nav2_bringup/params/nav2_params.yaml、/opt/ros/jazzy/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml、/opt/ros/jazzy/share/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml，以及 Nav2 的配置說明頁面。下面的工程參數使用當前工作空間中已經跑通的 mycar_navigation2_jazzy 配置。

1.準備工作

請先調用如下指令在工作空間的src目錄下創建一個功能包：

ros2 pkg create mycar_navigation2_jazzy --dependencies navigation2 nav2_common

2.編寫launch文件與參數文件

在功能包下，新建launch目錄、params目錄和bts目錄。

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_jazzy")

    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
    ])

該launch文件主要是加載了生命週期管理器、行為樹、規劃器、控制器、恢復器、航點跟蹤、路徑平滑以及速度平滑等節點。並且除了生命週期管理器節點外，每個節點都還會加載一個配置文件，接下來需要編輯這些配置文。

（1）bt_navigator相關配置文件

bt_navigator相關配置文件有兩個，分別是描述行為樹的xml文件，以及yaml格式的參數文件，前者存儲在bts目錄下，後者存儲在params目錄下。

請在bts目錄下，新建一個名為bt_planner_controller_behavior.xml的文件並輸入如下內容：

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions specific to planning / control as well as general system issues.
  This will be continuous if a kinematically valid planner is selected.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
            <Sequence>
              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>
      </PipelineSequence>
      <Sequence>
        <Fallback>
          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
        </Fallback>
        <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" error_code_id="{spin_error_code}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_code_id}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

繼續在bts目錄下新建一個名為bt_planner_controller_behavior_poses.xml的文件，並輸入如下內容：

<!--
  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
   and it also has recovery actions specific to planning / control as well as general system issues.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <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="{selected_planner}" error_code_id="{compute_path_error_code}"/>
            </ReactiveSequence>
            <Sequence>
              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>
      </PipelineSequence>
      <Sequence>
        <Fallback>
          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
        </Fallback>
        <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" error_code_id="{spin_error_code}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

在params目錄下新建一個名為bt.yaml的文件，並輸入如下內容：

/**:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

（2）planner_server相關配置文件

在params目錄下新建一個名為planner.yaml的文件，並輸入如下內容：

/**:
  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
      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相關配置文件

在params目錄下新建一個名為controller.yaml的文件，並輸入如下內容：

/**:
  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: 1.0
    progress_checker_plugins: ["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.6
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.6
      min_speed_theta: 0.0
      acc_lim_x: 1.6
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -1.6
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      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
      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

• 將毫米轉換為米：長0.96m，寬0.45m
• 以機器人中心（base_link座標系原點）為基準，計算矩形頂點座標：

說明：頂點按順時針或逆時針順序排列，覆蓋機器人長寬邊界。（右手座標系）

例如長960寬450長400

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

（4）behavior_server相關配置文件

在params目錄下新建一個名為behavior.yaml的文件，並輸入如下內容：

/**:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_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"
    local_frame: odom
    global_frame: map
    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相關配置文件

在params目錄下新建一個名為waypoint.yaml的文件，並輸入如下內容：

/**:
  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相關配置文件

在params目錄下新建一個名為velocity.yaml的文件，並輸入如下內容：

/**:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.6, 0.0, 1.0]
    min_velocity: [-0.6, 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相關配置文件

在params目錄下新建一個名為smootherr.yaml的文件，並輸入如下內容：

/**:
  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集成

導航實現時，需要依賴於地圖與定位功能，我們可以在一個launch文件中集成之前的定位launch以及當前編寫的導航核心模塊的launch，以簡化導航功能的啟動，在launch目錄下新建一個名為bringup.launch.py的launch文件，並輸入如下內容：

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_jazzy")

    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.編輯配置文件

打開CMakeLists.txt 並輸入如下內容：

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

5.編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select mycar_navigation2_jazzy

6.執行

（1）請先調用如下指令啟動仿真環境：

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

（2）然後在終端下進入當前工作空間，輸入如下指令啟動導航功能：

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

（3）啟動rviz2，加載/opt/ros/jazzy/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，為機器人設置初始位姿後，再通過菜單欄的Nav2 Goal設置目標點，機器人就可以自動導航至目標點了。

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

• rviz2：啟動 rviz2 工具。
• -d：指定要加載的 .rviz 配置文件。
• /opt/ros/jazzy/share/nav2_bringup/rviz/nav2_default_view.rviz：.rviz 配置文件的路徑。

運行該命令後，rviz2 將啟動並加載 nav2_default_view.rviz 配置文件。

先使用

如上圖，左圖是選擇初始位置，右圖是選擇目標位置。

上圖中，每個障礙物和牆附近的光圈是危險程度，顏色越紅表示機器人經過此地越危險。

自主探索SLAM

導航需要依賴於地圖與定位，例如在上一節 導航功能集成 中，導航實現時就是以launch文件的方式集成了 定位AMCL 一節中的定位功能，而機器人SLAM時，也是發佈地圖數據與定位信息的，所以導航時也可以不借助於amcl而是直接與SLAM結合，達到自主探索的SLAM效果。

1.編寫launch文件

在功能包mycar_navigation2_jazzy的launch目錄下新建名為auto_slam.launch.py的launch文件，並輸入如下內容：

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")
    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.編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select mycar_navigation2_jazzy

3.執行

（1）請先調用如下指令啟動仿真環境：

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

（2）然後在終端下進入當前工作空間，輸入如下指令啟動自主SLAM功能：

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

（3）啟動rviz2，加載/opt/ros/jazzy/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，再通過菜單欄的Nav2 Goal設置目標點，機器人就可以自動導航至目標點，並且導航中還會實現建圖的功能。

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

多車編隊

多車編隊技術通過先進的傳感器、實時數據分析和高度智能化的控制系統，使得一組車輛能夠在沒有人工干預的情況下實現協同運行，作為一種先進的智能交通系統應用，已經在多個領域展現出其巨大的潛力和廣泛的應用場景。比如：

時間優化：多車編隊技術消除了人為操作中的誤差和延誤，降低了人力資源成本和運營風險。

空間優化：多車編隊技術可以減少車輛間的空隙，最大化道路利用率。

性能優化：密集編隊行駛減少了空氣動力學阻力，降低了能耗和燃料消耗。

總之，多車編隊技術在機器人領域有著獨特的作用和價值。本節將介紹如何在ROS2中實現多車編隊。