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、未進行實車或仿真運動測試。文中時間與漏點例子均為源碼分析或參數換算。