dwa_planner代码解析

ROS

namespace dwa_local_planner

class DWAPlanner
class DWAPlannerROS : public nav_core::BaseLocalPlanner （ROS Wrapper for the DWAPlanner that adheres to the BaseLocalPlanner interface and can be used as a plugin for move_base.）

先看dwa_planner_ros，因为它包含了dwa_planner头文件；参考https://blog.csdn.net/qq_41925420/article/details/94542751
https://blog.csdn.net/gophae/article/details/101393017
一、dwa_planner_ros.cpp
2.void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level)
// Callback to update the local planner's parameters based on dynamic reconfigure。参数配置，先是配置自身类中的，然后配置base_local_planner::LocalPlannerLimits中的
base_local_planner::LocalPlannerLimits limits;
limits.max_vel_trans = config.max_vel_trans;
limits.min_vel_trans = config.min_vel_trans;
limits.max_vel_x = config.max_vel_x;
limits.min_vel_x = config.min_vel_x;
limits.max_vel_y = config.max_vel_y;
limits.min_vel_y = config.min_vel_y;
limits.max_vel_theta = config.max_vel_theta;
limits.min_vel_theta = config.min_vel_theta;
limits.acc_lim_x = config.acc_lim_x;
limits.acc_lim_y = config.acc_lim_y;
limits.acc_lim_theta = config.acc_lim_theta;
limits.acc_lim_trans = config.acc_lim_trans;
limits.xy_goal_tolerance = config.xy_goal_tolerance;
limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
limits.prune_plan = config.prune_plan;
limits.trans_stopped_vel = config.trans_stopped_vel;
limits.theta_stopped_vel = config.theta_stopped_vel;
planner_util_.reconfigureCB(limits, config.restore_defaults);

3 DWAPlannerROS::DWAPlannerROS() : initialized_(false),
odom_helper_("odom"), setup_(false) { }
//注意学习实现方法

4 void DWAPlannerROS::initialize
shared_ptr的管理机制其实并不复杂，就是对所管理的对象进行了引用计数，当新增一个shared_ptr对该对象进行管理时，就将该对象的引用计数加一；减少一个shared_ptr对该对象进行管理时，就将该对象的引用计数减一，如果该对象的引用计数为0的时候，说明没有任何指针对其管理，才调用delete释放其所占的内存。

//警告不推荐使用的参数-在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");

void warnRenamedParameter(const ros::NodeHandle& nh, const std::string current_name, const std::string old_name)
     {
       if (nh.hasParam(old_name))
       {
         ROS_WARN("Parameter %s is deprecated (and will not load properly). Use %s instead.", old_name.c_str(), current_name.c_str());
       }
     }

     } 

5 bool DWAPlannerROS::setPlan(const std::vector& orig_global_plan)
//在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlan将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。
//setPlan负责获取全局路径，DWAPlannerROS::setPlan获取后，转发给DWAPlanner::setPlan，
//恢复振荡标志位再转发给LocalPlannerUtil::setPlan，这样层层叠叠的调用很有层次感，这样每当产生一个新的全局路径都第一时间提供给全局——局部转化以及剪裁功能使用。

LatchedStopRotateController：
理想情况下，局部路径规划器可以让机器人准确停到它应该停止地方。然而在现实中，由于传感器噪声和执行器的不稳定性，机器人会接近到达目标，但其会继续移动，这不是我们想要的结果。
LatchedStopRotateController是一个不错的控制器，当机器人足够靠近目标时可以迅速启用。
然后，控制器将执行完全停止和原地旋转朝向目标方向的操作，无论在停止后的机器人位置是否超出目标位置公差范围。

6 bool dwaComputeVelocityCommands(geometry_msgs::PoseStamped& global_pose, geometry_msgs::Twist& cmd_vel);
* @brief Given the current position, orientation, and velocity of the robot,
* compute velocity commands to send to the base, using dynamic window approach
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise

7 bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
//computeVelocityCommands这个函数负责本次循环的下发速度的解算

二、dwa_planner

1.void DWAPlanner::reconfigure(DWAPlannerConfig &config)
* @brief Reconfigures the trajectory planner
path_distance_bias_ = resolution * config.path_distance_bias;
goal_distance_bias_ = resolution * config.goal_distance_bias;

2.DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util);
* @brief Constructor for the planner
* @param name The name of the planner

 if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
      sim_period_ = 0.05;
    } else {
      double controller_frequency = 0;
      private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
      if(controller_frequency > 0) {
        sim_period_ = 1.0 / controller_frequency;
      } else {
        ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
        sim_period_ = 0.05;
      }
    }


 // set up all the cost functions that will be applied in order
    // (any function returning negative values will abort scoring, so the order can improve performance)
    std::vector<base_local_planner::TrajectoryCostFunction*> critics;
    critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
    critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
    critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
    critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
    critics.push_back(&path_costs_); // prefers trajectories on global path
    critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
    critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin

3.bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
// used for visualization only, total_costs are not really total costs

 total_cost =
            path_distance_bias_ * path_cost +
            goal_distance_bias_ * goal_cost +
            occdist_scale_ * occ_cost;

4.bool setPlan(const std::vector& orig_global_plan);
* sets new plan and resets state

5 bool checkTrajectory(const Eigen::Vector3f pos,const Eigen::Vector3f vel, const Eigen::Vector3f vel_samples);
* @brief Check if a trajectory is legal for a position/velocity pair
* @param pos The robot's position
* @param vel The robot's velocity
* @param vel_samples The desired velocity
* @return True if the trajectory is valid, false otherwise

6void DWAPlanner::updatePlanAndLocalCosts(
const geometry_msgs::PoseStamped& global_pose,
const std::vector& new_plan,
const std::vector& footprint_spec)
* @brief Update the cost functions before planning
* @param global_pose The robot's current pose
* @param new_plan The new global plan(裁剪后的全局路径）
* @param footprint_spec The robot's footprint
* The obstacle cost function gets the footprint.
* The path and goal cost functions get the global_plan
* The alignment cost functions get a version of the global plan that is modified based on the global_pose

   在DWAPlanner::updatePlanAndLocalCosts函数（每个控制周期都会将映射到local costmap中的路径信息在这里更写到相关的打分项或者其他部件中）中，为了给前向打分点确定它的“goal”，采用的方法是先计算当前位置和局部路径末端点之间的连线朝向，然后在局部路径末端点的基础上沿该方向延伸forward_point_distance_距离，并将这个”goal“替换掉局部路径末端点。然后利用path_costs_.setTargetPoses函数将全局路径在局部地图中的映射传入到path_costs中，作为局部的打分参考路径，MapGridCostFunction::setTargetPoses函数将路径信息存储为target_poses_。

// 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);

7 base_local_planner::Trajectory DWAPlanner::findBestPath(
const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel,
geometry_msgs::PoseStamped& drive_velocities)

• @brief Given the current position and velocity of the robot, find the best trajectory to exectue
• @param global_pose The current position of the robot
• @param global_vel The current velocity of the robot
• @param drive_velocities The velocities to send to the robot base

• @return The highest scoring trajectory. A cost >= 0 means the trajectory is legal to execute.

  sensor_msgs::PointCloud2Iterator<float> iter_x(traj_cloud, "x");
  for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
  {
      if(t->cost_<0)
          continue;
      // Fill out the plan
      for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
          double p_x, p_y, p_th;
          t->getPoint(i, p_x, p_y, p_th);
          iter_x[0] = p_x;
          iter_x[1] = p_y;
          iter_x[2] = 0.0;
          iter_x[3] = p_th;
          iter_x[4] = t->cost_;
          ++iter_x;
      }
  }
  traj_cloud_pub_.publish(traj_cloud);
  

  ---

  //if we don't have a legal trajectory, we'll just command zero
  if (result_traj_.cost_ < 0) {
  drive_velocities.pose.position.x = 0;
  drive_velocities.pose.position.y = 0;
  drive_velocities.pose.position.z = 0;
  drive_velocities.pose.orientation.w = 1;
  drive_velocities.pose.orientation.x = 0;
  drive_velocities.pose.orientation.y = 0;
  drive_velocities.pose.orientation.z = 0;

  轨迹所有点得分的汇总方式有Last、Sum、Product三种方式，默认为Last（且没有对应的配置参数，除非自己修改源码实现），即只考虑轨迹最后一个点的打分。

总的来说，path_costs_打分项只考虑最后一个点的得分，该得分可能会取-4（如果是path_costs_和goal_costs_，还有可能返回-2，-3），或者最后一个点与局部地图路径之间的像素距离。