沉默杀手HomeArchivesTagsCategories
DWA算法(二) dwa_ros 源码
|路径规划DWA算法|
Word count: 933|Reading time: 4 min

我们需要看的是DWAPlannerROS类的源码，继承自nav_core::BaseLocalPlanner。 但是它还用到了base_local_planner的LatchedStopRotateController模块，用于latch机制和到达位置后转向。

initialize

原型是void DWAPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)

其实主要是if( !isInitialized())的部分，直接看里面的内容

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
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();
// base_local_planner::LocalPlannerUtil
// 其实就是把三个参数赋值给 LocalPlannerUtil 的三个成员变量
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

//这里创建了dwa的共享指针，也就是  boost::shared_ptr<DWAPlanner> dp_;
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;

// Warn about deprecated parameters -- remove this block in N-turtle
// 这些不必关注
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);

但是搜索发现，DWA的参数基本都在TrajectoryPlannerROS::initialize里加载。

computeVelocityCommands

根据机器人离目标是否足够接近，路径规划由dwa sampling或者stop and rotate负责。

先是全局路径的处理

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// 开始规划时再次获取位姿
if ( !costmap_ros_->getRobotPose(current_pose_))
{
  ROS_ERROR("Could not get robot pose");
  return false;
}

// TEB的部分全局路径也叫 transformed_plan，应该是因为下面的getLocalPlan参数也是这个名字
std::vector<geometry_msgs::PoseStamped> transformed_plan;
// 获取全局路径
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;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());

上面的planner_util_.getLocalPlan的内容实际是：

1
2
3
4
5
6
7
8
9
10
11
12
base_local_planner::transformGlobalPlan(
      *tf_,
      global_plan_,
      global_pose,
      *costmap_,
      global_frame_,
      transformed_plan)

if(limits_.prune_plan)   
{
    base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}

涉及到了DWA的参数prune_plan，如果需要剪辑全局路径，就进入base_local_planner::prunePlan，此函数在goal_funtions.cpp，一看就是在namespace里。这里不如TEB的剪切全局路径复杂，其实就是把机器人当前位姿身后的部分剪掉，这部分是从路径起点到离位姿的欧氏距离的平方不到1米的点，这个1米已经写死，不能用参数更改，不过并不重要。

updatePlanAndLocalCosts

1
2
3
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_,  transformed_plan, 
     costmap_ros_->getRobotFootprint()  );
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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
void DWAPlanner::updatePlanAndLocalCosts(
      const geometry_msgs::PoseStamped&  global_pose,
      const std::vector<geometry_msgs::PoseStamped>&  new_plan,
      const std::vector<geometry_msgs::Point>& footprint_spec)
{
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i)
    {
      global_plan_[i] = new_plan[i];
    }

    obstacle_costs_.setFootprint(footprint_spec);

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) 
                                             +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);

    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
   // keeping the nose on the path
  if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_)
  {
    alignment_costs_.setScale(path_distance_bias_);
    // costs for robot being aligned with path (nose on path, not ju
    alignment_costs_.setTargetPoses(global_plan_);
  }
  else
  {
    // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
    alignment_costs_.setScale(0.0);
  }
}