ROS运动规划学习六---dwa_local_planner_dwaplannerros:

---

前言

在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.");
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

执行完毕，初始化完成。

• 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);
  }
1
2
3
4
5
6
7
8
9
10

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);
  }
1
2
3
4

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;
}
1
2
3
4
5
6
7
8
9
10

• 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;
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

三、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;
      }
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

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;
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

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)
      {
      ......
      }
1
2
3
4
5
6
7

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_;
1
2
3
4
5
6
7

---

总结

本文简要的对dwa_local_planner进行分析，dwa_local_planner只是调用其他类的成员函数进行局部规划，起到桥梁作用，主要处理在base_local_planner功能包中。