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對齊、真實起終點落格、未知策略、繞行與通道通過空間、引數初始化方式、實際耗時及完整感知到執行鏈；原工程配置和外掛原始碼保持原樣。