DWA和base_local_planner的关系
在base_local_planner包中有两个文件叫trajectory_planner.cpp 以及对应的ros实现，其和DWA是同一层的。
由于nav_core提供了统一的接口，因此我们可以先看看统一的接口有哪些，那我们便知道每一个算法里比较重要的函数有哪些。
nav_core包里的base_local_planner.h文件
//最为关键的地方，计算机器人下一刻的速度
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
//判断是否到达目标点
virtual bool isGoalReached() = 0;
//加载全局路径
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
//初始化
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
下面我们就先看看base_local_planner的computeVelocityCommands的主要实现框架
bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
//检查初始化、检查是否已经到达目标点...略
transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan);
//如果已经到达目标点，姿态还没到
if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_))
{
tc_->updatePlan(transformed_plan);
//所以这个函数里最关键的子函数是findBestPath
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
return true;
}
tc_->updatePlan(transformed_plan);
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
//然后又是转换，然后就发布出速度了...
}
接下来我们看一下TrajectoryPlanner的findBestPath的实现框架，Come on~
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities)
{
//...
Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2],
acc_lim_x_, acc_lim_y_, acc_lim_theta_);
//...
}
顺藤摸瓜，一睹createTrajectories的内部实现，这个函数是轨迹采样算法，可以说是一个非常关键的函数。
Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta)
{
//检查最终点是否是有效的，判断变量在updatePlan中被赋值
if( final_goal_position_valid_ )
{
double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
}
//是否使用dwa算法， sim_peroid_是1/controller_frequency_，暂时不清楚sim_period_和sim_time_的区别
if (dwa_)
{
max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);
max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
}
else
{
max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);
max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
}
//...先忽略其中的逻辑，只要知道按照不同的规则生成路径，调用的子函数是generateTrajectory
}
这个子函数的作用就是生成路径，并且评分
void TrajectoryPlanner::generateTrajectory
{
//主要有两大作用：
//生成路径和速度
vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
//计算位置
x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
//对路径进行评分
if (!heading_scoring_)
{
//
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
}
else
{
cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
}
//这里的顺序与源码不同，我觉得总分来看更有组织性
//该轨迹与全局路径的相对距离
path_dist = path_map_(cell_x, cell_y).target_dist;
//距离目标点距离
goal_dist = goal_map_(cell_x, cell_y).target_dist;
//离障碍物距离
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)));
}
综上所述，其整一个逻辑顺序就是computeVelocityCommands->findBestTrajectory --> createTrajectories --> generateTrajectory
最终，选择分数最低的轨迹，发布出去。这便是整个局部规划器的实现思路和逻辑。下一篇，谈谈DWA。
