ROS局部規劃模塊分析

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,該類會進行所有軌跡的計算,軌跡中會保存軌跡點、速度、評分等各種因素,最後下發的速度指令也是在這裏選擇確定的


發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章