planner_server（Theta*全局路徑規劃）

1.planner_server（Theta*全局路徑規劃）

1.1.實車先看：規劃什麼時候運行，怎樣在距離與障礙代價之間取捨

當前planner_server使用GridBased這個插件ID加載Theta，在global_costmap中從起點搜索到目標的二維路徑。* GridBased是工程指定的名稱，實際算法由plugin字符串決定；它不表示此節點必定運行NavFn或普通A*。

BT：ComputePathToPose／ComputePathThroughPoses，planner_id=GridBased
    → planner_server：等待全局图current、取得起点、转换起终点到全局坐标
    → GridBased：ThetaStarPlanner
        ├─ 读内存中的global_costmap，当前坐标系map
        ├─ 4／8邻居展开 + 视线检查，尝试连接更远的父节点
        ├─ 路径长度费用 + 地图经过费用 + 到目标的启发估计
        └─ 回溯折线路径，按实际地图格距补充路径点
    → Action结果path，另向plan话题发布可视化路径
    → FollowPath：当前PID控制器跟踪，输出速度

規劃器直接使用它內部創建的global_costmap對象，不靠訂閲2 Hz的costmap圖片規劃。它輸出位置路徑，不直接發底盤速度，也不規劃整車爬坡能力或動態障礙的未來運動。全局圖與當前輸入、路徑執行

本次安裝的nav2_planner、nav2_theta_star_planner及nav2_costmap_2d均為1.1.20。Theta*算法和插件封裝安裝頭文件與該版本一致，插件XML確實註冊nav2_theta_star_planner/ThetaStarPlanner，庫文件存在；以下按這套實現説明。插件封裝、算法及費用定義

1.2.參數

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

planner_server:
  ros__parameters:
    use_sim_time: False
    expected_planner_frequency: 10.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_theta_star_planner/ThetaStarPlanner" # In Iron and older versions, "/" was used instead of "::"
      how_many_corners: 8
      w_euc_cost: 1.0
      w_traversal_cost: 10.0
      w_heuristic_cost: 1.0

1.2.1.ros__parameters（服務器時鐘、耗時檢查與插件ID）

1.2.1.1.參數表

1.2.1.2.參數詳解

1.2.1.2.1.expected_planner_frequency（100 ms是警告門檻）

服務器在configure時讀取正值並計算max_planner_duration=1／expected_planner_frequency。當前為0.1 s；完成有效路徑、準備結果後，若測得時長超過它，打印Planner loop missed its desired rate警告，然後仍按成功返回。值為0或負值時將門檻置0，關閉這項完成後檢查；需要關閉時可明確設0，不能把負值當正常頻率。門檻初始化、完成後的警告

計時從進入規劃處理、取得服務器參數鎖後開始，使用節點now；包括之後等待costmap current、取得／轉換位姿、插件搜索及準備／發佈路徑的耗時，不只是Theta*展開節點的時間。ComputePathThroughPoses檢查的是本次多個目標分段規劃的總耗時，不是每段各有100 ms獨立限時。

插件內部另用steady_clock打印createPlan耗時，其範圍和時鐘與服務器planning_time不同。仿真暫停或時間跳變時，不能把ROS時間結果直接當牆鍾實耗時，也不能據兩種日誌不同就認定算法計時錯誤。

把10改20，只把警告門檻從100 ms變50 ms；不會讓BT從3 Hz變20 Hz，也不會使地圖從5 Hz變20 Hz。需要改變實際規劃節奏時，應查看當前BT及系統耗時。行為樹調度

1.2.1.2.2.planner_plugins（ID和算法類名各有職責）

當前服務器按GridBased讀取GridBased.plugin，構造ThetaStarPlanner並配置。兩棵默認樹的ComputePathToPose／ComputePathThroughPoses都顯式寫planner_id="GridBased"；若改ID，需要同步列表、分組和請求來源。單改類名而保留ID，可以更換該ID對應算法，但參數也須按新實現核對。

1.1.20只有一個插件且請求ID為空時，服務器可警告並使用唯一插件；顯式傳入錯誤非空ID不會走這個兜底，將得到空路徑並失敗。當前BT顯式ID正確，不依賴兜底。按ID選擇規劃器

1.2.2.GridBased（Theta*鄰居與費用權重）

1.2.2.1.參數表

1.2.2.2.參數詳解

1.2.2.2.1.how_many_corners（4／8鄰居不限制最終連線方向）

算法的moves數組先列上下左右四個鄰居，再列四個對角鄰居；當前8全部參與展開。同時Theta*會檢查當前節點與更遠父節點之間的視線，如果可連通且費用更低，就改變父關係形成更直接的折線。因此4並不保證最終路徑只含水平／豎直段，8也不是隻允許八種最終航向。鄰居數組、視線連接與鄰居展開

8每次最多檢查更多鄰居，但能否更快到目標、打開多少節點，取決於地圖和費用；不能直接説“8一定更快”或“4一定更省總計算”。當前8是合法默認，初次測試通常保持；要比較，應在相同地圖、起終點和有效分辨率下看實際路徑、耗時與nodes_opened。

configure階段只接受4或8，否則成員回退8並警告；ROS參數列表仍可能顯示原非法值。動態回調直接把整數寫入成員，沒有同樣校驗，超過8還可能讓展開循環越過固定數組。因此只在合法4／8間選擇，不用任意整數試探；本文未運行非法配置。初始化檢查與動態分支

1.2.2.2.2.w_euc_cost與w_traversal_cost（長度與經過費用怎樣相加）

算法為候選節點使用f=g+h排序。g累計已經走過的長度和地圖經過費用，h估計當前位置到目標的距離；相鄰一步的費用可按以下式子理解，視線捷徑則按其實際檢查的格子累計經過費用：

长度费用 = w_euc_cost × hypot(相邻／连接端点的格索引差)
普通格的转换费用 c_scaled = 26 + 0.9 × 内部代价值c
单格经过费用 = w_traversal_cost × (c_scaled / 252)²
h = 内部w_heuristic_cost × hypot(当前格到目标格的索引差)

源碼真正的偏置是26，不能照附近註釋中的25代替。距離使用格索引差：橫／縱一步為1，對角一步為√2；在有效格距0.05 m時，平面移動1 m相當於20格距離，而不是直接給長度費用乘1。實際全局格距由靜態地圖決定。費用函數

只為説明當前w_traversal_cost=10的公式，計算普通格和鄰居擴展的未知格費用如下；不代表實車地圖讀數或整條路徑總分：

例如橫向走入c=200格，當前相鄰g增量約1+6.68241；走入空閒格約1+0.10645。只把w_traversal_cost從10改20，會將上述經過費用翻倍，但橫向長度項仍為1。直線捷徑的檢查／計費流程另有視線分支，不能把這張單格表簡單乘路徑點數量作為實際總費用。

在地圖已經正確表達障礙和膨脹的前提下，提高經過權重通常更重繞開高代價格，提高長度權重通常更重少繞路；二者是相對取捨，不保證某次路徑長度、耗時或最小離牆距離單調變化。空閒格費用也不為0，繞得更長同樣有經過費用，不是隻對彩色格子收費。

當前全局圖cost_scaling_factor=4、inflation_radius=0.7先決定代價場，w_traversal_cost=10再決定這套代價怎樣進入搜索。係數4越大，外圍代價下降越快；此時單靠權重10不能説明一定離牆更遠。應一起看真實輪廓、有效格距、膨脹和路徑。膨脹參數與Theta*

1.2.2.2.3.w_heuristic_cost（外部配置未使用，內部h仍參與）

當前configure沒有聲明或讀取GridBased.w_heuristic_cost，也沒有在動態回調處理它；代碼直接按下面賦值：

内部w_heuristic_cost = w_euc_cost < 1.0 ? w_euc_cost : 1.0

所以當前內部值確實是1，但它來自w_euc_cost=1，不來自這一行YAML。“數值碰巧一樣”不能作為參數已生效的證據。 算法getHCost仍讀取內部成員計算h並參與f排序，不應把此配置無效誤解成沒有啓發搜索。內部賦值、h計算

這些是源碼分支計算，不是本次運行參數或路徑對比。當前無需通過反覆調該YAML鍵試驗快慢；如希望獨立暴露啓發權重，需要改變插件實現並重新核對算法條件，不能只新增參數。本文保留原值，沒有修改插件源碼。

1.2.2.2.4.動態改w_euc_cost不會重新計算內部啓發權重

初始化會設置w_euc_cost和對應h權重；動態回調只更新w_euc_cost成員，沒有執行上述min公式。因此同一個表面配置可能因“重啓初始化”或“運行中改值”得到不同內部組合：

動態降低到0.5後，h與長度費用的關係也與啓動時的設計不同，不能繼續沿用重啓後的推斷來解釋路徑和搜索最優性。比較參數組合時應使用一致的初始化方式，尤其降低w_euc_cost時，優先重新配置／重啓並核對有效行為。動態修改GridBased.w_heuristic_cost也不能補上缺少的成員更新。當前動態回調

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

1.2.3.1.參數表

這兩項是當前Theta*插件實際支持的默認參數；本次沒有向工程YAML追加。

1.2.3.2.未知區、不可用代價與起終點條件

當前常規搜索只放行內部代價c<252的普通格，255另按allow_unknown判斷；因此252、253、254普通格都不可用，不能把“不是254致命障礙”理解成一定可規劃經過。視線分支對轉換費用採用<252比較，普通整數格對應同樣的0～251通過範圍；放行未知時用253作為該分支的轉換費用進行計費。普通格與未知格檢查

同一格的起終點另走快捷分支：此版本只顯式拒絕254，否則返回一個位於起點的姿態；沒有執行上述常規allow_unknown／252～253檢查。因此allow_unknown=False不能描述成插件所有分支都絕對不返回未知格路徑。這裏只記錄源碼邊界，沒有運行這個特殊請求。同格快捷分支

實物global_costmap頂層track_unknown_space=True，仿真頂層未寫而默認False；此外當前三態地圖保存／加載還有灰205在free=0.25下變空閒的問題。若未知已被上游改成0，此處allow_unknown=False也無法恢復它。全局未知表示、已驗證的地圖讀寫條件

通用Humble筆記中的GridBased示例使用NavFn，tolerance、use_astar等參數不能直接套給當前Theta*。當前插件以起終點落格進行搜索，常規回溯得到網格中心位置，再線性補點；它沒有獨立目標位置容差參數。controller_server的xy_goal_tolerance=0.15則是執行到達判定，不能用來讓本插件自動替換被佔用的目標格。

1.2.3.3.最後朝向、路徑補點與當前PID

當前use_final_approach_orientation=False時，常規路徑最後姿態使用請求目標朝向；True時多點路徑用最後兩點的atan2方向，特定單點情況使用起點方向。這個開關修改返回路徑的最後四元數，不修改機械人本體姿態，也不自動發角速度。

本版本linearInterpolation主要填寫中間路徑點的x、y，沒有為每個中間姿態生成沿路徑的有效朝向四元數；不能把路徑中每個orientation當成已經規劃好的車輛航向。當前PID的enable_rotation=False、use_rotate_to_heading=False，goal_checker.yaw_goal_tolerance=6.28，也不會因末端朝向設置就要求最終轉到指定yaw。末端方向與補點、當前控制器條件

補點使用global_costmap的有效格距，不能固定認定就是YAML初始0.05。它在Theta*折線段上補位置點，沒有自動生成曲率連續曲線、轉彎半徑約束或速度軌跡；與下一塊smoother_server的服務是不同步驟。當前兩棵默認XML均未調用SmoothPath，單有節點或BT庫並不證明平滑已經運行。

1.2.4.請求等待、路徑驗證、動態參數與本次核對

1.2.4.1.地圖等待與取消的邊界

服務器規劃前調用waitForCostmap，以100 Hz檢查global_costmap是否current；這不是讓地圖以100 Hz更新。1.1.20該等待循環沒有本塊可配置的超時，也沒有在循環內檢查本次Action取消，Theta*搜索循環同樣沒有取消檢查。expected_planner_frequency的100 ms只在成功完成後檢查，不能終止持續等待或搜索。等待實現

若請求長時間無結果，應先查靜態地圖、TF和各層current，再看搜索耗時；把expected_planner_frequency調小隻是放寬警告門檻。反過來，current=True也不證明感知持續新鮮：當前地形來源缺省expected_update_rate=0，可以繼續使用最後一份觀測，詳見全局圖章節。

1.2.4.2.返回非空路徑與is_path_valid不是同一檢查

服務器取得插件路徑後，validatePath在此版本主要檢查是否非空；不是再次完整執行每個路徑段的車體掃掠碰撞驗證。另有is_path_valid服務檢查已有路徑的剩餘點；當前圓半徑模式下，拒絕253／254及地圖外點，但沒有複用Theta*的252門檻和allow_unknown策略，不能把二者當成一致的通行判據。返回路徑檢查、已有路徑檢查

當前兩棵默認BT未調用IsPathValid，不能因為已加載對應庫就認定它每輪驗證路徑。全局規劃、該服務和局部PID抽樣檢查各有範圍；實際通過空間仍需正確地圖、輪廓與車輛執行驗證。

1.2.4.3.動態修改的實際範圍

當前插件動態回調對權重和how_many_corners缺少完整範圍校驗，也沒有像服務器那樣圍繞整次規劃處理使用同一參數鎖；不要假設一組運行中修改與重啓初始化完全等價。本文沒有修改運行節點參數。服務器回調、插件回調

1.2.4.4.啓動、接口與驗證範圍

機械人namespace為/red_standard_robot1時，節點名為/red_standard_robot1/planner_server，Action／plan／is_path_valid通常位於該機械人namespace下，不自動在話題中再加planner_server一層。global_costmap另有自己的子namespace。

• 本次已核對： 相關包版本、Theta*註冊XML和庫存在；算法及封裝安裝頭文件與1.1.20一致；源碼／安裝副本的兩套planner_server段一致，差異僅時鐘；兩棵默認BT均請求GridBased且目標規劃節奏為3 Hz，未調用SmoothPath或IsPathValid。
• 源碼能確認： 8項原配置的使用條件；10 Hz僅對應100 ms完成後警告；4／8鄰居與視線連接；長度按格數、經過費用偏置26及平方公式；YAML啓發權重未讀取、內部初始化公式與動態不同步；未知／普通格門檻、同格快捷分支、末端方向和等待／取消邊界。
• 本次驗證範圍： 費用和權重表為源碼公式計算，未啓動新的規劃Action、未比較實車路線或運行中參數，不將庫／頭文件存在當成已驗證整套運行行為。
• 實物待核實： 有效全局圖格距與代價、map／TF對齊、真實起終點落格、未知策略、繞行與通道通過空間、參數初始化方式、實際耗時及完整感知到執行鏈；原工程配置和插件源碼保持原樣。