前言

在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;}// 更新了新的全局路径后，清除latchlatchedStopRotateController_.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功能包中。

ROS运动规划学习六---dwa_local_planner相关推荐

ROS运动规划学习三---move_base
文章目录前言一.move_base工程结构二.move_base简要分析 1.MoveBase类的声明---move_base.h 2.MoveBase类的实现---move_base.cpp ...
ROS运动规划学习五---global_planner
文章目录前言一.global_planner功能包结构二.planner_core 1.执行过程 2. calculatePotentials() 3. getPlanFromPotential ...
运动规划学习笔记4——探索OMPL
运动规划学习笔记4--探索OMPL A.OMPL编译与安装 B.OMPL使用 B1.基本定义 B2.路径可视化 B3.API Overview C.代码附录 C1.Geometric Planning ...
rk3288 img打包工具_【个人开源】机器人运动规划学习工具箱使用说明
最近的课题与机器人的运动规划有关.回顾过去的学习经历,深感机械出身的我们编程实践与总结能力实在太弱,以前的一些课题就拿matlab随便写一写m文件出个结果了事,许多后来发现有用的课程或作业,要么找不到 ...
深蓝学院-机器人运动规划学习笔记-第一章
第一课移动机器人运动规划 Motion planning for mobile robots Introduction Course outline Typical planning methods ...
无人机核心技术之运动规划
转载自:https://mp.weixin.qq.com/s/VHaqsfNhl9yStMX--Gtx6g 无人机核心技术之运动规划奕木怒飞垂云 1周前近年来无人机已广泛落地应用在农业.军工甚至 ...
六自由度机器人（机械臂）运动学建模及运动规划系列（二）——运动学分析
本篇主要介绍六轴机械臂的运动学分析. 运动学分析是工业机器人研究和应用的重要内容,是运动控制的基础,主要研究机器人末端坐标系与基坐标系的转换关系,分为正运动学和逆运动学分析两部分. 另外,对于刚刚学习 ...
ROS实验笔记之——基于Prometheus的无人机运动规划
本博文基于Prometheus项目来学习无人机的运动规划.关于该项目的配置可以参考<ROS实验笔记之--基于Prometheus自主无人机开源项目的学习与仿真> Demo演示基于2D-L ...
ROS moveit 机械臂避障运动规划
机械臂moveit编程(python) moveit默认使用的运动规划库OMPL支持臂章规划,这里选用RRT算法,使用move group中的PlanningSceneInterface()添加障碍物 ...

ROS运动规划学习六---dwa_local_planner

文章目录

前言

一、dwa_local_planner结构

二、setPlan、initialize、isGoalReached

三、computeVelocityCommands()

总结

ROS运动规划学习六---dwa_local_planner相关推荐

最新文章

热门文章