waypoint_follower（逐點導航與到點等待）

1.waypoint_follower（逐點導航與到點等待）

1.1.實車先看：是否呼叫逐點導航、是否需要到點等待

本節點收到FollowWaypoints航點列表後，逐個呼叫NavigateToPose，到點後執行一次任務外掛，再前往下一個點。當前外掛是WaitAtWaypoint，每個導航成功的點等待200 ms，即0.2 s，最後一個點也等待。 它不直接跟蹤planner_server輸出的稠密路徑，也不直接計算底盤速度。

客户端发送FollowWaypoints：poses=[A, B, C]
    → waypoint_follower
        → NavigateToPose(A) → bt_navigator → 规划／控制／必要时恢复
            ├─ 导航成功 → WaitAtWaypoint：等待200 ms
            └─ 导航失败 → 记录A的索引；当前stop_on_failure=false，继续
        → NavigateToPose(B) → 同样处理
        → NavigateToPose(C) → 同样处理，成功时最后一个点也等待
        → 返回Action结果及missed_waypoints

當前PB的navigation_launch在獨立程序和元件分支都會載入waypoint_follower，並把它交給生命週期管理器；載入不等於已經收到航點任務。 普通NavigateToPose和NavigateThroughPoses不經過這個航點列表執行器，也不會因為enabled=True自動在途經點暫停。當前BT入口

本次在當前PB原始碼中沒有找到傳送FollowWaypoints的客戶端；Fork的另一套目錄ysu2026_rm_sentry_nav_cod/src/waypoint_editor中則已有waypoint_to_nav2橋接程式碼，通過start_waypoint_following服務傳送該Action。當前PB啟動檔案沒有載入該橋接；是否另行啟動或由外部決策客戶端呼叫，需以實際請求為準，不能直接判斷這組引數永遠不用。已有橋接客戶端

本次安裝的nav2_waypoint_follower為1.1.20，rclcpp為16.0.21。伺服器／WaitAtWaypoint安裝標頭檔案與Nav2對應版本一致；安裝外掛XML註冊nav2_waypoint_follower::WaitAtWaypoint，庫存在；FollowWaypoints安裝介面與該版本一致。下文按這套介面解釋。伺服器原始碼、等待外掛原始碼

1.2.引數

下面原樣保留髮送的7項引數。原始碼和安裝副本的reality段一致；simulation段只有use_sim_time=True不同。本次沒有更改工程YAML，也沒有傳送導航或航點請求。

waypoint_follower:
  ros__parameters:
    use_sim_time: False
    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: 200

1.2.1.ros__parameters（時鐘、任務迴圈與失敗策略）

1.2.1.1.參數列

1.2.1.2.引數詳解

1.2.1.2.1.loop_rate（等待導航結果的迴圈，不是移動控制頻率）

收到非空列表後，伺服器建立WallRate(loop_rate)，每輪處理取消／新目標、傳送下一點、釋出current_waypoint、處理導航狀態，最後處理NavigateToPose客戶端回撥並按節拍等待。20 Hz對應名義50 ms／輪，但回撥、同步任務外掛或取消等待都可能延長這一輪，不能把50 ms寫成取消響應的硬上限。任務迴圈

每個點只在切換到新目標時傳送一次NavigateToPose，不是每秒20次重發。規劃頻率由BT的RateController等決定，控制速度仍由controller_server產生；改loop_rate不會把路徑跟蹤頻率改成同樣的值，也不會把機器人線速度直接提高。控制頻率與速度

當前20 Hz與controller_server的20 Hz數值相同，仍是兩個獨立迴圈。按數值計算，200 ms相當於四個50 ms間隔，但WaitAtWaypoint是一次同步睡眠，期間不會因此執行四輪航點檢查。

1.2.1.2.2.stop_on_failure（繼續任務與結果完整性）

本版將NavigateToPose的ABORTED、CANCELED或目標被拒絕視為導航失敗。導航失敗先把當前索引加入failed_ids；True立即終止當前FollowWaypoints並返回已記錄索引，False則推進到下一個點，不執行失敗點的WaitAtWaypoint。導航失敗及推進分支

索引從0開始。例如[A, B, C]中B導航失敗、其餘成功，當前False會嘗試C，最終可返回Action成功、missed_waypoints=[1]。即使所有點導航失敗，False也可在遍歷完後返回成功並列出全部失敗索引；客戶端必須一起檢查返回列表，不能僅看SUCCEEDED就報告每點到達。

任務外掛失敗的記錄條件與導航失敗不同：只有“任務返回False且stop_on_failure=True”才把該點加入missed。當前WaitAtWaypoint正常返回True，但若以後替換為其他任務外掛，False策略下不能認為missed_waypoints完整涵蓋所有任務執行失敗。任務執行分支

Fork另一目錄的waypoint_to_nav2橋接在SUCCEEDED分支只打印“All waypoints reached successfully!”，沒有檢查missed_waypoints。如果以後使用它，當前False下該日誌可能與實際漏點不符；需要核對客戶端結果處理。本次只記錄這個適配事項，沒有修改該橋接。現有結果處理

True終止的是航點任務流程，不等於由本節點直接發急停。單點導航自己的恢復會先在NavigateToPose中進行；這裡只依據它最後的結果決定是否繼續下一點，不通過改False增加同一點的恢復次數。

1.2.1.2.3.waypoint_task_executor_plugin（ID、類名與到點條件）

伺服器按ID尋找對應的plugin鍵，再建立類並呼叫initialize(node, ID)。所以當前ID為wait_at_waypoint時，需要與下面分組同名；如果只改ID而保留下方舊分組，新ID的外掛配置不會自動從舊分組繼承。載入任務執行器

本節點以NavigateToPose的成功結果作為執行任務的條件，沒有在此處再次檢查真實車速或嚴格位置誤差。當前general_goal_checker的xy容差0.15 m、yaw容差6.28 rad，解釋見控制器頁；因此“成功後等待200 ms”不是另加0.2秒精確對準，也不能替代到點判定和實際停車驗證。到達判定

1.2.2.wait_at_waypoint（到點後的等待外掛）

1.2.2.1.參數列

1.2.2.2.引數詳解

1.2.2.2.1.enabled與waypoint_pause_duration（兩個條件一起決定是否等）

initialize讀取wait_at_waypoint.enabled和wait_at_waypoint.waypoint_pause_duration；如果時長為0，即使YAML寫enabled=True，也把內部is_enabled設為False。processAtWaypoint遇內部停用直接返回True，表示本外掛任務成功，不會終止或取消導航列表。初始化與零時長處理

當前True和200會進入睡眠分支。每個點使用同一時長，外掛沒有按航點索引設定不同等待時間，也沒有根據當前位置、偏差或速度自動延長；需要其他到點任務時應使用相應執行器實現。

同一列表有N個導航成功的點時，名義等待預算為N×0.2 s，包含最後一個點。例如10個點全部成功，合計名義等待2 s；這是引數換算，不含路途、恢復、回撥排程和任務切換耗時，也不是本次實測。

1.2.2.2.2.200的單位與模擬時間

外掛把整數200轉成std::chrono::milliseconds，再呼叫節點clock的sleep_for。因此單位明確為毫秒；5000才表示5秒，不要把當前200解釋為200秒，也不要為了表達0.2秒把此整數引數直接寫成浮點0.2。等待呼叫

這裡雖傳入chrono時長，等待仍按節點時鐘工作。本次rclcpp 16.0.21的sleep_for實際等待now()+時長；use_sim_time=False時ROS時鐘通常使用系統時間，True且ROS時間啟用時則等待/clock前進到目標時刻。模擬暫停或時鐘不再前進，200 ms不保證在200 ms牆鍾時間後返回。對應rclcpp等待實現

如模擬時間均勻以真即時間的0.5倍前進，0.2 s模擬等待名義對應0.4 s真即時間；這是時間比例示例，未驗證當前模擬即時倍率。loop_rate使用WallRate，也不能改變同步ROS時鐘睡眠的結束條件。

1.2.2.2.3.等待不會檢查停車，也不會即時處理取消

WaitAtWaypoint沒有cmd_vel釋出器，不檢查車輪速度、不持續鎖定位置，也不清除fake_vel_transform的cmd_spin。它只推遲下一個目標的傳送；實際停止由NavigateToPose完成處理和速度下游保證。當前附加自旋可能繼續，因此等待不代表底盤所有速度分量都為零。速度轉換與附加自旋

processAtWaypoint在航點主迴圈內同步呼叫，sleep_for期間不檢查FollowWaypoints取消／搶佔。普通取消不會直接喚醒這個睡眠；請求要等外掛返回、迴圈再次到達檢查分支後處理。加大loop_rate無法讓阻塞中的檢查更頻繁，延長暫停時長則可能延長這段響應等待。

原始碼忽略sleep_for的bool返回值，之後仍返回True。若ROS關閉或時間源切換使睡眠提前返回False，這個True也不證明已完整等待200 ms；本次沒有做時鐘切換或取消實驗。睡眠返回約定

1.2.3.Action介面、啟動條件與動態引數

1.2.3.1.FollowWaypoints和NavigateThroughPoses的區別

本版FollowWaypoints請求只有PoseStamped[] poses，反饋current_waypoint為從0開始的當前點索引，結果missed_waypoints為失敗索引列表；沒有迴圈次數、起始索引或整次任務時限欄位。不能僅啟動該節點就自動重複巡邏；需要客戶端再次發請求或由上層任務邏輯組織。1.1.20 Action定義

伺服器逐點只填寫NavigateToPose的pose，behavior_tree保持空字串，正常走bt_navigator的預設單點XML；這裡不是改為執行預設多點XML。航點請求應攜帶正確frame_id、位置和有效四元數，當前工程通常按map表達，名稱空間也須與follow_waypoints／navigate_to_pose服務端一致。逐點請求構造

空poses列表在本版直接返回成功，沒有實際導航或到點任務。正常非空請求每次從索引0開始；導航失敗跳過後也不會自動回頭補點。取消處理會向其NavigateToPose客戶端呼叫async_cancel_all_goals，並等待取消響應，不是本節點直接釋出零速度。取消處理

1.2.3.2.動態引數（伺服器有回撥，外掛值只在初始化讀取）

1.1.20伺服器在生命週期activate時安裝動態引數回撥，deactivate時移除。本頁需要區分以下更新條件：動態回撥

伺服器回撥會返回引數設定成功，但只處理loop_rate和stop_on_failure的上述型別；其他外掛引數即使ROS層顯示新值，也不能據此證明外掛快取已變化。loop_rate和暫停時長在本版按整數讀取，數值型別也需保持正確。

1.2.3.3.本次核對與驗證範圍

• 已核對： 安裝版本、伺服器／外掛標頭檔案、註冊名和庫、FollowWaypoints介面；原始碼與安裝副本的兩套引數和navigation_launch；當前PB的客戶端檢索及另一目錄已有橋接。
• 原始碼能確認： 七項引數的讀取條件；200 ms與最後一點等待；導航失敗繼續及任務失敗記錄差異；WallRate與ROS時鐘睡眠；取消的阻塞邊界和動態更新範圍。
• 實物使用時核對： 實際Action入口、航點座標與到點判據，是否需要跳過失敗點，以及客戶端是否讀取missed_waypoints；需要暫停時再觀察有效停留時間、附加自旋和取消響應。
• 本次範圍： 只整理文件和目錄，未修改導航配置、橋接、BT或外掛，未發FollowWaypoints／NavigateToPose、未進行實車或模擬運動測試。文中時間與漏點例子均為原始碼分析或引數換算。