ROS运动规划学习六---dwa_local_planner
文章目录
- 前言
- 一、dwa_local_planner结构
- 二、setPlan、initialize、isGoalReached
- 三、computeVelocityCommands()
- 总结
前言
在ROS navigation导航框架中局部轨迹规划包含dwa_localplanner和trajectory_planner,后者位于base_local_planner中。
经过之前ROS运动规划三—move_base的学习，move_base功能包中global_planner订阅move_base_simple/goal话题，拿到目标点位置，进行全局规划，新建线程，调用makePlan()函数进行全局规划，获得全局路径，之后传给controller_plan，之后传给局部规划器tc_,若局部规划器选择的是dwa,则调用局部轨迹规划器dwa中的setPlan() 函数 ，进行局部规划。利用局部轨迹规划器computeVelocityCommands()函数计算速度，下发给底盘。
一、dwa_local_planner结构
dwa_local_planner的结构看着比较简单，然后继续往下看就，，，，，，打扰了。
二、setPlan、initialize、isGoalReached
在ROS运动规划学习二—nav_core中了解到局部轨迹规划器要继承base_local_planner.h中的BaseLocalPlanner基类，重写四个纯虚函数computeVelocityCommands()、isGoalReached()、setPlan()、initialize()，之后被move_base调用。
- 首先介绍初始化函数（局部规划器的名字、tf转换、代价地图），如果没有初始化，则进行初始化操作：位于dwa_planner_ros.cpp中
void DWAPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costmap_2d::Costmap2DROS* costmap_ros)
{
//没有初始化，进行初始化操作
if (! isInitialized()) {
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
// 更新局部规划器的代价地图
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
//调用planner_util_中的成员函数
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());
//创建局部规划器
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
if( private_nh.getParam( "odom_topic", odom_topic_ ))
{
odom_helper_.setOdomTopic( odom_topic_ );
}
initialized_ = true;
nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");
nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");
nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");
nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");
nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");
//参数服务器中的参数设置
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
else{
ROS_WARN("This planner has already been initialized, doing nothing.");
}
}
执行完毕，初始化完成。
- setPlan()
接收全局规划的路径， 进行局部规划。dwa_planner_ros.cpp主要调用的是dwa局部规划器类中的指针dp_->setPlan()，在dwa_planner.cpp中
bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
// 更新了新的全局路径后，清除latch
latchedStopRotateController_.resetLatching();
ROS_INFO("Got new plan");
return dp_->setPlan(orig_global_plan);
}
dwa_planner.cpp中的成员函数setPlan()接收全局路径，调用planner_util_中的成员函数setPlan()，位于local_planner_util.cpp(base_local_planner功能包)中：
bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
oscillation_costs_.resetOscillationFlags();
return planner_util_->setPlan(orig_global_plan);
}
local_planner_util.cpp中的成员函数setPlan()对全局路径进行处理：
bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_){
ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
return false;
}
//清除下全局路径，存入自己的成员变量
global_plan_.clear();
global_plan_ = orig_global_plan;
return true;
}
- isGoalReached()
先判断初始化；然后判断能否得到机器人位姿，不能返回false；判断到达终点，返回false，调用的是latchedStopRotateController_类中的成员函数isGoalReached()判断。
bool DWAPlannerROS::isGoalReached() {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
ROS_INFO("Goal reached");
return true;
} else {
return false;
}
}
三、computeVelocityCommands()
局部规划器的核心。首先获取机器人的位姿；计算转换之后的路径，将全局坐标系下的全局路径转换到机器人坐标系下，结果存入transformed_plan，调用的是base_local_planner包中的local_planner_util.cpp；获得全局路径以后判断是否为空，为空则返回，之后调用dwa_local_planner中的updatePlanAndLocalCosts()函数更新全局路径。然后判断是否达到目标点，到达目标点，发出空的路径规划,调用的是latchedStopRotateController类的对象，也在base_local_planner包中；没有达到终点，调用dwaComputeVelocityCommands()函数计算速度指令，发布全局路径。
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
return false;
}
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(cmd_vel,limits.getAccLimits(),dp_->getSimPeriod(),
&planner_util_,odom_helper_,current_pose_,
boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
}
else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector<geometry_msgs::PoseStamped> empty_plan;
publishGlobalPlan(empty_plan);
}
return isOK;
}
}
dwaComputeVelocityCommands()函数
初始化，初始化成功使用 OdometryHelperRos的对象odom_helper_从里程计得到当前速度，位于base_local_planner包中；再获得机器人底盘的坐标系名；之后调用dwa_planner的函数findBestPath()计算出下发速度以及局部轨迹；提取速度x,y,z传递；同时还要判断轨迹是否有效，如果无效轨迹，返回false,有效提取信息，放入local_plan，发布出去。
bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel)
{
if(! isInitialized()){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
geometry_msgs::PoseStamped drive_cmds;
drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
std::vector<geometry_msgs::PoseStamped> local_plan;
if(path.cost_ < 0) {
ROS_DEBUG_NAMED("dwa_local_planner",
"The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
local_plan.clear();
publishLocalPlan(local_plan);
return false;
}
for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
geometry_msgs::PoseStamped p;
p.header.frame_id = costmap_ros_->getGlobalFrameID();
p.header.stamp = ros::Time::now();
p.pose.position.x = p_x;
p.pose.position.y = p_y;
p.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p_th);
tf2::convert(q, p.pose.orientation);
local_plan.push_back(p);
}
publishLocalPlan(local_plan);
return true;
}
DWAPlanner::findBestPath()
里面主要用到的是base_local_planner 中的SimpleTrajectoryGenerator类的initialise()函数，产生采样速度，得到速度采样空间，以及SimpleScoredSamplingPlanner类中的findBestTrajectory()函数，通过打分函数生成最优轨迹。
base_local_planner::Trajectory DWAPlanner::findBestPath(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel,
geometry_msgs::PoseStamped& drive_velocities)
{
......
}
dwa_local_planner中定义了七个打分器：
- oscillation_costs_：运动打分判断，是否震荡，是：代价大
- obstacle_costs_：障碍物碰撞检测打分，碰到障碍物，代价值增大
- path_costs_：局部轨迹（根据当前速度外推出的轨迹）与局部路径（规划的路径）对比，局部轨迹离局部路径的横向偏差小，其代价值就小
- goal_costs_：局部轨迹与局部路径的终点进行对比，希望距离小
- goal_front_costs_：局部轨迹与局部路径的最终点的朝向一致
- alignment_costs_：局部轨迹与局部路径的朝向一致
- twirling_costs_：机器人旋转不要太大。
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_;
base_local_planner::TwirlingCostFunction twirling_costs_;
总结
本文简要的对dwa_local_planner进行分析，dwa_local_planner只是调用其他类的成员函数进行局部规划，起到桥梁作用，主要处理在base_local_planner功能包中。