smoother_server（全局路徑平滑）

1.smoother_server（全局路徑平滑）

1.1.實車先看：節點已經加載，默認導航是否真的調用平滑

當前navigation_launch會啓動並管理smoother_server，但兩棵默認行為樹都沒有SmoothPath節點。 當前正常路徑鏈是ComputePath→FollowPath，所以這組平滑參數雖在插件配置時讀取，默認導航不會因此自動平滑Theta*路徑。

当前默认链路：
    ComputePath → planner_server／Theta* → path → FollowPath／PID

后续若明确接入路径平滑：
    ComputePath → 原路径 → SmoothPath Action → smoother_server
                                              └─ simple_smoother
                                                  → 返回平滑路径与完成标志
    FollowPath必须使用返回的平滑路径，才会改变实际跟踪输入

另一条速度处理链：
    controller_server输出速度 → velocity_smoother → fake_vel_transform

smoother_server處理nav_msgs/Path中的位置，不直接平滑cmd_vel；velocity_smoother處理速度指令，是另一節點。Theta*自身的線性補點也不等於已經調用本平滑器。規劃與路徑補點、當前控制鏈

本次nav2_smoother、nav2_behavior_tree及nav2_costmap_2d均為1.1.20。SimpleSmoother、分段工具和服務器安裝頭文件與該版本一致；安裝XML註冊nav2_smoother::SimpleSmoother，相關庫存在。以下按該實現解釋，不能直接套後續版本新增的細化次數或約束參數。SimpleSmoother實現、服務器實現

1.2.參數

下面保留髮送的完整6項原參數。當前源碼／安裝副本的reality段一致；simulation段只有use_sim_time=True不同。

smoother_server:
  ros__parameters:
    use_sim_time: False
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

“當前運行不使用”在本頁表示：插件已配置，但當前默認BT未發起平滑計算。 數值和開關的調節效果描述的是後續實際調用SmoothPath時的條件，不表示改完本塊就會影響當前導航。

1.2.1.ros__parameters（時鐘與平滑器ID）

1.2.1.1.參數表

1.2.1.2.參數詳解

1.2.1.2.1.smoother_plugins（加載與實際調用分開核對）

當前configure讀取列表，按simple_smoother.plugin加載類，再把此ID登記到服務器。啓用SmoothPath後，可明確請求smoother_id="simple_smoother"；只有一個插件且請求ID為空時，1.1.20會警告並使用唯一插件，顯式錯誤非空ID則失敗。加載與ID選擇

navigation_launch把smoother_server列入生命週期管理，並提供獨立進程／組件兩種啓動方式；bt_navigator也配置了nav2_smooth_path_action_bt_node庫。這些説明服務器和BT節點可用，不能證明當前樹調用了它。本次已檢查兩份默認XML及安裝副本，SmoothPath數量均為0。

如果以後選用其他XML或外部客户端，是否平滑以實際請求為準。本頁沒有修改默認行為樹；原路徑只有被請求平滑、返回結果又接到FollowPath後，才改變執行輸入。當前BT調度

1.2.2.simple_smoother（迭代停止與重複細化）

1.2.2.1.參數表

1.2.2.2.參數詳解

1.2.2.2.1.SimpleSmoother實際修改什麼（原位置約束與鄰居拉平）

對符合點數條件的路徑段，算法保留段首、段尾的位置，只更新中間點x和y，z不參與迭代。當前未顯式寫w_data與w_smooth，使用源碼默認0.2與0.3；每個座標按下面規則更新：

新坐标 ← 当前新坐标
       + w_data ×（本次smoothImpl输入的原坐标 − 当前新坐标）
       + w_smooth ×（前一个新坐标 + 后一个新坐标 − 2 × 当前新坐标）

w_data把點拉回本輪輸入位置，w_smooth把相鄰點的局部折線拉平。遍歷過程中使用new_path的鄰居，其中一部分已經在本輪更新，不是所有點只依據同一份上一輪快照同時更新。

這裏的data不是障礙距離，平滑項也不是直接求車輛曲率或轉彎半徑；地圖主要用來拒絕某些候選點，不按1～252軟代價給平滑方向連續打分。因此它可能削弱Theta*原先按w_traversal_cost選擇的離障礙取捨；是否更適合跟蹤，需要看結果路徑和實際車體空間。更新公式與點檢查

1.2.2.2.2.tolerance（所有中間座標更新量之和，不是位置精度）

每輪先將change清零，然後累加各中間點兩次座標更新的abs(更新後−更新前)。循環條件為change≥tolerance，所以必須嚴格小於1e-10才因正常收斂退出。

例如10箇中間點的x、y各改變1e-6 m，合計change=20×1e-6=2e-5 m，遠大於當前1e-10，即使單個點變化已經很小仍繼續。這只是公式示例，不是本次平滑輸出。

相同單點變化下，長段的change通常更大；不能把它理解成每個點都允許1e-10 m誤差，也不能把它和控制器0.15 m的終點到達容差比較為同一精度要求。這個門檻檢測的是迭代變化是否足夠小，不是原路徑偏離量、真實定位誤差、離牆距離或平滑後的最大麴率。

調大可能減少遍歷次數，調小可能增加次數；結果形狀還受兩項權重、輸入點密度、細化及退出原因共同影響。當前1e-10很嚴格，但默認BT未調用，沒有實測耗時問題，不能僅憑數值小就宣佈當前導航必須改它。change統計與停止條件

1.2.2.2.3.max_its（次數上限、時間限制與部分結果）

smoothImpl每輪先its+=1，再檢查its≥max_its。當前1000意味着最多完整執行1～999輪；若第999輪後仍未收斂，下一輪計數到1000便返回False。若已經收斂，則不會為了湊次數繼續運行。

超過次數、超過本次時間或候選點進入禁止格時，函數會把path恢復為last_path，即上一次完整可接受遍歷的路徑，並更新近似朝向，返回False；它不總是恢復到最初未平滑路徑。第一次遍歷完成前失敗時，last_path才仍是本輪原輸入。

因此“未完成”可能帶有部分平滑結果；增大max_its不保證得到更多有效結果，也不保證克服時間或碰撞退出。時間來自SmoothPath請求的max_smoothing_duration，與這個計數上限各自作用。次數／時間退出

各長段的成功標誌用短路&&累計：一個長段返回False後，後續長段不會再調用smoothImpl，而會保留對應輸入。was_completed=False不能解釋為每段都已嘗試迭代、只是最後沒有達到同一收斂門檻。

1.2.2.2.4.do_refinement（最多四次額外調用，不是一次額外遍歷）

第一次smoothImpl正常收斂後，True允許遞歸調用smoothImpl(new_path,...)。每次遞歸有自己的its計數和輸入路徑；前一次平滑結果成為下一次“原位置”約束，所以細化可以繼續改變形狀，並不一直把所有點拉向最初Theta*路徑。

refinement_ctr在每次smooth請求開始時置0，最多增加到4；一個正常收斂長段可以經歷基礎調用加4次細化，合計最多5次smoothImpl。計數在整次請求內共享，不是每個分段都保證另外做4次；若前面長段消耗了計數，後面段就沒有同樣的細化次數。

每次調用都重新計算自己的迭代計數，因此max_its=1000不是整次請求最多1000遍。細化也不插點、不改變點數量；此處的“refinement”主要表示重新平滑已有位置。細化遞歸

當前還有兩個使用邊界需要明確：遞歸傳入同一個max_time值並重新開始內部計時，沒有按上一層已耗時間持續扣減；遞歸調用的bool返回值也沒有傳回上一層。因此True時總時長可能超出表面單次預算，外層返回True也不能證明每次細化均完整收斂。這裏只記錄代碼路徑，未測試其實際超時幅度。

1.2.2.3.分段、短路徑與朝向

路徑先按相鄰位置向量點積為負的折返，以及“位置幾乎不動而yaw變化”的原地轉向條件分段；只有end−start>9才調用平滑，相當於包含首尾至少11個點。10點及更短段保持輸入位置和朝向，不能通過調tolerance或max_its讓當前這條短段條件消失。分段工具、長段條件

11點不等於固定米數：若恰好等間距5 cm、沒有切段，首尾相隔約0.5 m只是一個直線例子；地圖格距、Theta*插值和分段規則都會改變實際長度。平滑保留各段端點位置，因此折返分界點不被跨段一起抹平。

對參與平滑的長段，輸出工具按相鄰點方向更新近似yaw，並用輸入第1→第2點方向與第1點已有yaw判斷是否倒車；最後一點的輸入朝向保留，重合點還可能跳過更新。不是每個路徑點的四元數都無條件被修正。

當前Theta*中間點沒有逐點生成有效沿路徑朝向，而虛擬gimbal_yaw_fake也不等於真實底盤朝向；若後續調用平滑，要核對倒車推斷和輪廓方向。當前PID關閉旋轉且yaw容差較大，輸出yaw變了不等於底盤自動跟着轉向。近似朝向更新、規劃器朝向條件

1.2.3.相關源碼默認項（原YAML未顯式配置）

1.2.3.1.參數表

以下6項為兩項平滑權重和服務器地圖／輪廓輸入。原參數塊沒有顯式配置，本次也未向工程追加。

1.2.3.2.權重之外，地圖怎麼參與

SimpleSmoother內部對候選中間點查代價：超過MAX_NON_OBSTACLE=252且不等於NO_INFORMATION=255時退出，即253、254拒絕，255未知放行。它不對1～252建立避障優化梯度，也沒有繼承Theta的allow_unknown參數。Theta常規搜索拒絕252，而本平滑點檢查允許252，兩者條件不同。內部候選檢查

因此不能僅憑原規劃器禁止未知就保證平滑路徑仍不會進入未知。內部檢查只檢查更新點，不能稱為完整車體掃掠驗證；該代碼還沒有檢查worldToMap的bool返回值就使用格索引，輸入和更新點是否始終在地圖內需要核對，不能把地圖外候選理解成可靠拒絕。

smoother_server從話題取得地圖快照，默認使用global_costmap/costmap_raw；與planner_server直接讀內存不同，地圖新鮮度受全局圖目標2 Hz發佈及接收影響，實際發佈頻率仍受更新循環和負載制約。沒接到地圖時getCostmap會拋出“Costmap is not available”，不是通過增大迭代次數就可繼續平滑。地圖訂閲與快照

服務器複製輸入path後直接交給插件，SimpleSmoother沒有把輸入路徑自動轉換到地圖座標系。當前路徑應和全局代價圖一樣在map中表達；僅有TF可用，也不證明輸入odom路徑會被自動換到map。服務器傳入路徑

1.2.4.SmoothPath請求、結果與後置檢查（不是原6項節點參數）

1.2.4.1.請求與BT端口

這些端口由nav2_smooth_path_action_bt_node提供，當前樹沒有實例化SmoothPath，故3.0 s／False只是該BT節點若被使用且省略端口時的默認值，不是當前已有平滑請求。BT端口默認、Action接口

1.2.4.2.時間預算與結果狀態要一起看

插件smooth以steady_clock統計整次處理的已耗時間，進入每個長段前計算剩餘預算；smoothImpl再以自己的steady_clock計時，每輪遍歷前檢查。預算不是可以中斷單輪循環的硬實時截止，也不涵蓋獲取地圖等計時前工作和服務器後置檢查的所有成本；細化遞歸重複使用預算的問題見前文。

服務器把插件返回值放入was_completed。即使False，只要沒有異常且可選後置檢查通過，它仍可以將Action設為SUCCEEDED，返回部分路徑；日誌裏的“未在指定時間完成”還可能對應次數或內部碰撞退出，不能只憑這句日誌認定唯一原因是超時。服務層結果處理

BT插件on_success會寫smoothed_path、smoothing_duration、was_completed並返回SUCCESS，不會自動因為was_completed=False轉為FAILURE。後續若要求“必須完成才跟蹤”，需要在調用流程中明確處理完成標誌；當前未接入平滑，本文沒有替用户增加這套流程。BT成功回調

1.2.4.3.check_for_collisions（服務層另外檢查輪廓）

True時，服務器對結果中的每個離散姿態調用CostmapTopicCollisionChecker，使用地圖和發佈輪廓；取到的輪廓先轉到robot_base_frame，再放到各路徑姿態上檢查。這與SimpleSmoother迭代裏的中心點代價檢查是兩條路徑，False不會關閉插件內部點檢查。

當前服務器默認robot_base_frame=base_link，而全局輪廓參考gimbal_yaw_fake。啓用後要核對二者的實際TF、原點偏移、圓／多邊形輪廓及路徑yaw語義，不能只複製兩個話題就認定幾何已經一致。輪廓換系、服務層碰撞檢查

該檢查器按輪廓評分≥254拒絕，並會在缺圖／缺輪廓、地圖外等異常條件返回False；仍是對離散路徑姿態的檢查，不能當成包含點間運動、定位誤差和執行偏差的完整實車驗證。服務器在後置檢查前已經發布plan_smoothed，因此看見該話題有路徑，也不能單獨證明Action最終成功或後置檢查通過。檢查器與閾值

1.2.5.動態參數、啓動與驗證範圍

1.2.5.1.運行中修改的實際範圍

當前SimpleSmoother沒有為tolerance、max_its、do_refinement、w_data、w_smooth設置動態參數回調；服務器也沒有動態重建平滑器或地圖／輪廓訂閲的路徑。ROS參數設置成功不表示這些已讀取成員更新，應修改配置後重新配置／重啓，並核對加載值和實際請求。use_sim_time屬於ROS時鐘機制，應與整套節點時間域一起處理。

例如namespace為/red_standard_robot1，節點名為/red_standard_robot1/smoother_server，接口通常為/red_standard_robot1/smooth_path和/red_standard_robot1/plan_smoothed；地圖默認訂閲/red_standard_robot1/global_costmap/costmap_raw。名稱與輸入參考系需要各自核對。

1.2.5.2.本次已核對與後續實物檢查

• 本次已核對： nav2_smoother 1.1.20、註冊XML與庫；SimpleSmoother、分段工具及服務器安裝頭文件與固定版本一致；源碼／安裝副本兩套參數一致且僅時鐘不同；navigation_launch與默認XML安裝副本一致，兩棵樹SmoothPath數量均為0。
• 源碼能確認： 6項原配置的讀取和使用條件；總更新量收斂門檻；每調用的次數及提前退出；≥11點分段條件；端點位置保留、已有點重複細化、最多4次額外調用及共享計數；遞歸預算／返回值邊界；默認權重、地圖快照、內部點檢查與服務後置檢查的區別；Action狀態與was_completed分離。
• 本次驗證範圍： 沒有發起SmoothPath、修改運行參數或默認BT，也沒有實測平滑耗時、路徑形狀、碰撞或底盤跟蹤效果；迭代更新量及點數長度示例為源碼計算。
• 實物待核實： 若後續接入，檢查實際請求與FollowPath輸入、地圖新鮮度／參考系、輪廓TF和yaw語義、原路徑到平滑路徑的通過空間、部分結果處理、總規劃鏈延遲及跟蹤表現；原工程參數與代碼保持原樣。