Humble導航模擬

1.導航概述

1.1.導航簡述

概念

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

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

作用

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

• Nav2具備多樣的導航功能，支援雷射雷達、攝像頭等多種感測器輸入，即時獲取環境資訊，實現智慧路徑規劃和精確運動控制。它支援多種導航模式，滿足不同應用需求，並具備強大的擴充套件性，可與ROS 2元件無縫整合。

• Nav2效能卓越，採用高效演算法和資料處理技術，確保機器人在導航過程中響應迅速且穩定。

• 安全方面，Nav2採用多種避障演算法和引數調節功能，設定安全區域，提供診斷監控，確保機器人安全導航。

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

1.2.導航安裝

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

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

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

1.3.導航條件

深入學習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進行機器人導航任務的開發與除錯。

1.4.導航引數參考

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

2.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在技術上各自獨立，但在構建完整的機器人導航系統中，它們的互補性使得機器人能夠在複雜環境中實現更加自主、精準的建圖或導航。

2.1.slam_toolbox概述

概念

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

功能

• 演算法整合 包含基於卡爾曼濾波器和粒子濾波器的SLAM演算法，以及基於圖最佳化的SLAM演算法等。

• 資料處理 提供資料融合、資料預處理和異常檢測等功能，支援雷射雷達、攝像頭、IMU等多種感測器資料的處理。

• 地圖構建 利用感測器資料和機器人運動資訊構建準確的地圖，並進行更新和最佳化。

• 模組化設計 支援使用者插入自己的演算法或替換現有模組，實現高度定製化的SLAM解決方案。

優點

• 開源性和靈活性 作為開源軟體，SLAM Toolbox提供了原始碼和文件，使用者可以自由修改和擴充套件演算法。

• 多種感測器支援 支援雷射雷達、攝像頭、IMU等多種感測器資料，適應不同應用場景的需求。

• 高效性 利用現代C++的特性最佳化效能，確保在資源受限的硬體上也能高效執行。

• 廣泛的應用前景 適用於學術研究、無人機導航、自動駕駛汽車、室內機器人和工業自動化等多個領域。

2.2.slam_toolbox安裝

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

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

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

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

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

2.4.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相關引數的問題。

2.5.cartographer概述

概念

Cartographer是Google推出的一套基於圖最佳化的雷射SLAM演算法庫，支援二維和三維地圖的構建。它結合了雷射雷達和慣性測量單元（IMU）的資料，通過高效的演算法實現即時、準確的定位和建圖。

功能

• 並行掃描匹配 利用平行計算技術加快掃描匹配速度，提高建圖效率。

• 位姿圖最佳化 通過圖最佳化技術估計機器人的姿態和地圖的拓撲結構，減少累積誤差。

• 即時地圖更新 在機器人移動過程中即時更新地圖，確保地圖的準確性和時效性。

• 迴環檢測 通過迴環檢測識別機器人曾經訪問過的區域，進一步減少累積誤差，提高地圖的全域性一致性。

• 多感測器融合 支援雷射雷達、IMU、里程計等多種感測器資料的融合，提高定位和建圖的精度。

優點

• 高效穩定 Cartographer的演算法經過精心設計和最佳化，能夠在複雜環境中高效穩定地執行。

• 高精度 通過圖最佳化和迴環檢測技術提供高精度的定位和建圖結果。

• 靈活性 支援二維和三維地圖構建，適應不同應用場景的需求。

• 開源免費 Cartographer是開源專案，使用者可以免費獲取和使用其原始碼和文件。

• 社群支援 擁有活躍的社群支援體系，使用者可以獲取來自全球開發者的幫助和支援。

2.6.cartographer安裝

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

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

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

2.7.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節點需要配置地圖的解析度和更新週期等引數，以確保生成的柵格地圖滿足特定的精度和即時性要求。

2.8.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，即可顯示建立的地圖了，當機器人運動時，地圖也會隨之更新。

3.地圖服務

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

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

3.1.儲存地圖(序列化)

3.1.1.地圖儲存節點說明

在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。

3.1.2.地圖儲存基本操作

準備工作

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

儲存地圖

SLAM建圖完畢，在終端下進入工作空間，呼叫如下指令儲存地圖：

ros2 run nav2_map_server map_saver_cli -f map/my_map

上述指令將訂閱/map話題，並把/map話題裡的資料儲存為檔案，在工作空間下的map目錄(需要自行建立該目錄，否則將會丟擲異常)中，生成兩個檔案，分別名為：my_map.yaml和my_map.pgm。

3.1.3.地圖介面

在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

3.1.4.地圖儲存格式

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

3.2.讀取地圖(反序列化)

3.2.1.地圖讀取節點說明

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

釋出的話題

引數

• frame_id 地圖座標系名稱。

• topic_name 話題名稱。

• yaml_filename 地圖資料來源。

3.2.2.地圖讀取基本操作

準備工作

請先呼叫如下指令在工作空間的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，就可以正常顯示地圖資料了。

4.AMCL自適應蒙特卡洛定位

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

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

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

4.1.定位節點說明

功能包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: 這些引數定義了雷射模型中的機率分佈，用於計算雷射束擊中障礙物或隨機位置的機率。

4.2.定位節點基本操作

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非常強。

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

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

5.導航伺服器

本節將介紹導航伺服器中核心節點，實現基本導航功能，並將導航與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

5.1.生命週期管理節點說明

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:是否使用模擬實踐。

5.2.行為樹節點說明

行為樹（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: 包含導航所需外掛的庫名稱列表。

5.3.規劃器節點說明

在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軸上的解析度和體素數。

5.4.控制器節點說明

在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: 體素層的配置，用於將三維空間劃分為體素（體積畫素），以提高成本圖的處理效率。

5.5.恢復器節點說明

恢復行為是機器人導航過程中一個至關重要的部分，它允許機器人在遇到障礙、卡住或其他導航問題時採取一系列預定義的動作來嘗試恢復。在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: 定義了座標變換時的容差範圍，用於處理不同座標系之間的微小差異。

5.6.航點跟隨節點說明

在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: 定義了在每個路徑點處等待的持續時間（以毫秒為單位）。這可以用於確保機器人在移動到下一個路徑點之前已經穩定或已經完成了某些操作。

5.7.路徑平滑節點說明

在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系統的實際時間。

5.8.速度平滑節點說明

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

5.9.導航功能整合(ROS2 Humble及之前)

1.準備工作

請先呼叫如下指令在工作空間的src目錄下建立一個功能包：

ros2 pkg create mycar_navigation2 --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")

    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的檔案並輸入如下內容：

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

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

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

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

/**:
  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相關配置檔案

在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

      # 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相關配置檔案

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

• 將毫米轉換為米：長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:
    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相關配置檔案

在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.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相關配置檔案

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

    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

6.執行

（1）請先呼叫如下指令啟動模擬環境：

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

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

. 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：啟動 rviz2 工具。

• -d：指定要載入的 .rviz 配置檔案。

• /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz：.rviz 配置檔案的路徑。

執行該命令後，rviz2 將啟動並載入 nav2_default_view.rviz 配置檔案。

先使用

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

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

5.10.自主探索SLAM

導航需要依賴於地圖與定位，例如在上一節 導航功能整合 中，導航實現時就是以launch檔案的方式集成了 定位AMCL 一節中的定位功能，而機器人SLAM時，也是釋出地圖資料與定位資訊的，所以導航時也可以不借助於amcl而是直接與SLAM結合，達到自主探索的SLAM效果。

1.編寫launch檔案

在功能包mycar_navigation2的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")
    # 从这开始修改: 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.編譯

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

colcon build --packages-select mycar_navigation2

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 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.多車編隊

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

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

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

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

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