BaseLocalPlanner是所有局部路徑規劃器的基類,所有的局部路徑規劃器都是它的插件
BaseLocalPlanner接口:
computeVelocityCommands
isGoalReached
setPlan
Initialize
1、以DWAPlannerROS爲例:
a) 外部首先調用的是initialize函數,塞入外面弄好的costmap
b) DWAPlannerROS是對DWAPlanner的外部封裝,DWAPlanner是實際的DWA算法實現類
c) Costmap會被塞入plannerutils中,然後該util將會傳入DWAPlanner的構造函數
d) 實際使用時,會通過一些判斷,類似於一個狀態機,進行具體方法選擇。主要是在快到點以及一些特殊狀態時候不用DWA進行計算。
2、ROS中DWA解析
a) DWA其實最重要的就是各種速度評分函數及其權重的設計以及選擇
b) 本DWA中主要使用瞭如下幾種評分函數:
base_local_planner::OscillationCostFunction oscillation_costs_;
base_local_planner::ObstacleCostFunction obstacle_costs_;
base_local_planner::MapGridCostFunction path_costs_;
base_local_planner::MapGridCostFunction goal_costs_;
base_local_planner::MapGridCostFunction goal_front_costs_;
base_local_planner::MapGridCostFunction alignment_costs_;
c) 所有的CostFunction都是繼承於TrajectoryCostFunction
d) TrajectoryCostFunction的主要接口如下:包含了評分函數所需要的一切接口,初始化,權重設定、獲得,評分計算
i. Prepare
ii. scoreTrajectory
iii. setScale
iv. getScale
e) 所有的costfunction都塞在一個TrajectoryCostFunction*的容器中,塞入順序需要注意,因爲任何一個costfunction返回負評分都會讓評價速度被拋棄,因此可以利用這個特性提高算法效率
除了costfunction是必須的外,還需要一個軌跡生成器,DWA中使用 base_local_planner::SimpleTrajectoryGenerator,塞在了base_local_planner::TrajectorySampleGenerator*的容器中
Costfunction和generator都初始化好之後,都塞在了scored_sampling_planner_中,該變量爲 base_local_planner::SimpleScoredSamplingPlanner類型,繼承於純虛基類base_local_planner::TrajectorySearch
f) 實際供外部調用的接口爲
findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec)
該函數中,首先進行generator_的初始化,然後調用scored_sampling_planner_的findBestTrajectory接口。findBestTrajectory的第一個參數爲軌跡結果,第二個參數爲遍歷過得所有軌跡。在findBestTrajectory函數中,首先進行generator生成軌跡,之後利用costfunction進行評分。
g) 因此,馬上要考慮的問題就出現了,那就是generator如何生成軌跡,以及costfuntion如何進行評分。
h) DWA中的generator如前所述,只有一個,是SimpleTrajectoryGenerator,該類會進行所有軌跡的計算,軌跡中會保存軌跡點、速度、評分等各種因素,最後下發的速度指令也是在這裏選擇確定的