DWA 算法分析
版权声明：本⽂为博主原创⽂章，未经博主允许不得转载。 blog.csdn/u013158492/article/details/50512900
DWA Local Planner这部分是属于Local planner，在Navigation中有两个包： dwa_local_planner 和ba_local_planner 查看了dwa_local_planner ，发现其实际是调⽤的ba_local_planner 中的函数，⽽ba_local_planner 中包含了两种planner ：DWA 或者Trajectory Rollout approach 。所以结论就是，只需要搞清楚ba_local_planner
的执⾏就OK。
ba_local_planner package 实际是继承于BaLocalPlanner ：
对于基类接⼝定义：
因此，派⽣类必须实现基类的接⼝。类TrajectoryPlannerROS 的⽅法：
在成员变量中，重要的⼏个变量如下：
其实最重要的就是去找到代价最⼩代价的velocity。通过在三个维度（x，y，theta）的速度，加速度的采样，得到候选velocity，然后对这些velocity做cost 评判，评判的标准是：那么，这⾥⾯的path_dist ，goal_dist ，occ_cost ，heading_diff 是怎么计算的呢？
occ_cost 的计算很简单，直接通过costmap就能得到这个值:
class TrajectoryPlannerROS : public nav_core::BaLocalPlanner namespace nav_core {
枸骨树的药用价值class BaLocalPlanner{
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
virtual bool isGoalReached() = 0;
virtual bool tPlan(const std::vector<geometry_msgs::PoStamped>& plan) = 0;
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0; virtual ~BaLocalPlanner(){}
protected:
BaLocalPlanner(){}
};
};
WorldModel* world_model_; ///< @brief The world model that the controller will u
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will u
costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will u
std::vector<geometry_msgs::PoStamped> global_plan_;
长城的传说故事if (!heading_scoring_) {
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
} el {
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost + 0.3 * heading_diff;
}
double footprint_cost = footprintCost(x_i, y_i, theta_i);
occ_cost = std::max( std::max(occ_cost, footprint_cost),
double(costmap_.getCost(cell_x, cell_y)));
计算path_dist，goal_dist：
path_dist = path_map_(cell_x, cell_y).target_dist;
goal_dist = goal_map_(cell_x, cell_y).target_dist;
那么如何更新path_dist，goal_dist: 需要costmap_ , global_plan_
path_map_.tTargetCells(costmap_, global_plan_);
goal_map_.tLocalGoal(costmap_, global_plan_);
⾸先看 成员函数tTargetCells：
//update what map cells are considered path bad on the global_plan
void MapGrid::tTargetCells(const costmap_2d::Costmap2D& costmap,const std::vector<geometry
_msgs::PoStamped>& global_plan) 这个函数会根据global_plan更新costmap，如何做到的呢？
//将adjusted_global_plan的信息，打包成MapCell类型，塞到path_dist_queue
小孩睡觉for (i = 0; i < adjusted_global_plan.size(); ++i) {
double g_x = adjusted_global_plan[i].po.position.x;
double g_y = adjusted_global_plan[i].po.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap(g_x, g_y, map_x, map_y) && Cost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
MapCell& current = getCell(map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(¤t);
started_path = true;
} el if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance(path_dist_queue, costmap);
于是重点来了，最后⼀⾏computeTargetDistance(path_dist_queue, costmap);，这个函数的实际算法实现：
void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){ MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!pty()){
current_cell = dist_queue.front();
dist_queue.pop();
if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
藕炖排骨的做法}
}
}
if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;自杀英语
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
新生儿手脚脱皮if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;洗手的重要性
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
}
}
上述代码检查current_cell 的四个邻接cell， 然后不断的扩散，每个cell相对于之前的cell，更新target_dist :
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell, const costmap_2d::Costmap2D& costmap){
//if the cell is an obstacle t the max path distance
unsigned char cost = Cost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return fal;
}
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
鏖战是什么意思
check_cell->target_dist = new_target_dist;
}
return true;
}
