DWA 简介：
该包使用DWA（Dynamic Window Approach）算法实现了平面上移动机器人局部导航功能。 输入全局规划和代价地图后，局部规划器将会生成发送给移动底座的命令。该包适用于其footprint可以表示为凸多边形或者圆形的机器人，它导出配置参数为ROS参数形式，可以在启动文件进行配置，当然也可以动态配置。 这个包的ROS封装接口继承了BaseLocalPlanner接口。
1 Overview
dwa_local_planner包实现了一个驱动底座移动的控制器，该控制器将路径规划器和机器人底座连在了一起。该规划器使用地图创建运动轨迹让机器人从起点到达目标点。移动过程中规划器会在机器人周围创建可以表示为珊格地图的评价函数。这里的控制器的主要任务就是利用评价函数确定发送给底座的速度（dx,dy,dtheta）。
DWA算法基本思路如下:
- 在机器人控制空间进行速度离散采样(dx,dy,dtheta)
- 对每一个采样速度执行前向模拟，看看使用该采样速度移动一小段段时间后会发生什么
- 评价前向模拟中每个轨迹，评价准则如: 靠近障碍物，靠近目标，贴近全局路径和速度；丢弃非法轨迹（如哪些靠近障碍物的轨迹）
- 挑出得分最高的轨迹并发送相应速度给移动底座
- 重复上面步骤.
一些有价值的参考:
-
D. Fox, W. Burgard, and S. Thrun. “The dynamic window approach to collision avoidance”. The Dynamic Window Approach to local control.
-
Alonzo Kelly. “An Intelligent Predictive Controller for Autonomous Vehicles”. An previous system that takes a similar approach to control.
-
Brian P. Gerkey and Kurt Konolige. “Planning and Control in Unstructured Terrain “. Discussion of the Trajectory Rollout algorithm in use on the LAGR robot.
2 DWAPlannerROS
dwa_local_planner::DWAPlannerROS对象是dwa_local_planner::DWAPlanner对象的ROS封装，在初始化时指定的ROS命名空间使用，继承了nav_core::BaseLocalPlanner接口。
如下是一个简单的dwa_local_planner::DWAPlannerROS对象使用示例:
-
#include <tf/transform_listener.h> #include <costmap_2d/costmap_2d_ros.h> #include <dwa_local_planner/dwa_planner_ros.h> ... tf::TransformListener tf(ros::Duration(10)); costmap_2d::Costmap2DROS costmap("my_costmap", tf); dwa_local_planner::DWAPlannerROS dp; dp.initialize("my_dwa_planner", &tf, &costmap);
2.1 API Stability
- The C++ API is stable
- The ROS API is stable
2.1.1Published Topics
~<name>/global_plan (nav_msgs/Path)
- 局部规划器当前正在跟踪的全局规划的一部分。Used primarily for visualization purposes.
~<name>/local_plan (nav_msgs/Path)
- 上一个周期的局部规划或者得分最高的轨迹。Used primarily for visualization purposes.
2.1.2 Subscribed Topics
odom (nav_msgs/Odometry)
- Odometry信息用于给局部规划器提供机器人当前移动速度。 本消息中的速度信息被假定使用了与TrajectoryPlannerROS对象的代价地图参数robot_base_frame设定值坐标系一致的坐标系。 关于参数robot_base_frame用法详见costmap_2d。
2.2 Parameters
有很多ROS参数可以用来配置dwa_local_planner::DWAPlannerROS的行为。 这些参数被分成了几组：: robot configuration, goal tolerance, forward simulation, trajectory scoring, oscillation prevention, and global plan.
这些参数多数可以用 dynamic_reconfigure 进行动态配置，这样便于在系统运行时tune局部规划器。
2.2.1 Robot Configuration Parameters
-
~<name>/acc_lim_x (double, default: 2.5) The x acceleration limit of the robot in meters/sec^2 ~<name>/acc_lim_y (double, default: 2.5) The y acceleration limit of the robot in meters/sec^2 ~<name>/acc_lim_th (double, default: 3.2) The rotational acceleration limit of the robot in radians/sec^2 ~<name>/max_trans_vel (double, default: 0.55) The absolute value of the maximum translational velocity for the robot in m/s ~<name>/min_trans_vel (double, default: 0.1) The absolute value of the minimum translational velocity for the robot in m/s ~<name>/max_vel_x (double, default: 0.55) The maximum x velocity for the robot in m/s. ~<name>/min_vel_x (double, default: 0.0) The minimum x velocity for the robot in m/s, negative for backwards motion. ~<name>/max_vel_y (double, default: 0.1) The maximum y velocity for the robot in m/s ~<name>/min_vel_y (double, default: -0.1) The minimum y velocity for the robot in m/s ~<name>/max_rot_vel (double, default: 1.0) The absolute value of the maximum rotational velocity for the robot in rad/s ~<name>/min_rot_vel (double, default: 0.4) The absolute value of the minimum rotational velocity for the robot in rad/s
-
2.2.2 Goal Tolerance Parameters
-
~<name>/yaw_goal_tolerance (double, default: 0.05) The tolerance in radians for the controller in yaw/rotation when achieving its goal ~<name>/xy_goal_tolerance (double, default: 0.10) The tolerance in meters for the controller in the x & y distance when achieving a goal ~<name>/latch_xy_goal_tolerance (bool, default: false) If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.
-
2.2.3 Forward Simulation Parameters
-
~<name>/sim_time (double, default: 1.7) The amount of time to forward-simulate trajectories in seconds ~<name>/sim_granularity (double, default: 0.025) The step size, in meters, to take between points on a given trajectory ~<name>/vx_samples (integer, default: 3) The number of samples to use when exploring the x velocity space ~<name>/vy_samples (integer, default: 10) The number of samples to use when exploring the y velocity space ~<name>/vtheta_samples (integer, default: 20) The number of samples to use when exploring the theta velocity space ~<name>/controller_frequency (double, default: 20.0) The frequency at which this controller will be called in Hz. Uses searchParam to read the parameter from parent namespaces if not set in the namespace of the controller. For use withmove_base, this means that you only need to set its "controller_frequency" parameter and can safely leave this one unset.
-
2.2.4 Trajectory Scoring Parameters
The cost function used to score each trajectory is in the following form:
-
cost = path_distance_bias * (distance to path from the endpoint of the trajectory in meters) + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))
-
-
~<name>/path_distance_bias (double, default: 32.0) The weighting for how much the controller should stay close to the path it was given ~<name>/goal_distance_bias (double, default: 24.0) The weighting for how much the controller should attempt to reach its local goal, also controls speed ~<name>/occdist_scale (double, default: 0.01) The weighting for how much the controller should attempt to avoid obstacles ~<name>/forward_point_distance (double, default: 0.325) The distance from the center point of the robot to place an additional scoring point, in meters ~<name>/stop_time_buffer (double, default: 0.2) The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds ~<name>/scaling_speed (double, default: 0.25) The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s ~<name>/max_scaling_factor (double, default: 0.2) The maximum factor to scale the robot's footprint by
-
2.2.5 Oscillation Prevention Parameters
-
~<name>/oscillation_reset_dist (double, default: 0.05) How far the robot must travel in meters before oscillation flags are reset
-
2.2.6 Global Plan Parameters
~<name>/prune_plan (bool, default: true)
- Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.
2.3 C++ API
For C++ level API documentation on the base_local_planner::TrajectoryPlannerROS class, please see the following page:DWAPlannerROS C++ API
3 DWAPlanner
dwa_local_planner::DWAPlanner实现了DWA和Trajectory Rollout的实现。要在ROS中使用dwa_local_planner::DWAPlanner，请使用进行了ROS封装的格式，不推荐直接使用它。
3.1 API Stability
-
The C++ API is stable. However, it is recommended that you use theDWAPlannerROS wrapper instead of using thedwa_local_planner::TrajectoryPlanner on its own.
3.2 C++ API
For C++ level API documentation on thedwa_local_planner::DWAPlanner class, please see the following page:DWAPlanner C++ API
参考:http://blog.csdn.net/x_r_su/article/details/53393872
DWA Local Planner这部分是属于Local planner，在Navigation中有两个包： dwa_local_planner 和base_local_planner 查看了dwa_local_planner ，发现其实际是调用的base_local_planner 中的函数，而base_local_planner 中包含了两种planner ：DWA 或者Trajectory Rollout approach 。所以结论就是，只需要搞清楚base_local_planner 的执行就OK。 base_local_planner package 实际是继承于BaseLocalPlanner ：class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner
对于基类接口定义：
-
namespace nav_core { class BaseLocalPlanner{ public: virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0; virtual bool isGoalReached() = 0; virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0; virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0; virtual ~BaseLocalPlanner(){} protected: BaseLocalPlanner(){} }; };
-
因此，派生类必须实现基类的接口。类TrajectoryPlannerROS 的方法：
在成员变量中，重要的几个变量如下：
-
WorldModel* world_model_; ///< @brief The world model that the controller will use TrajectoryPlanner* tc_; ///< @brief The trajectory controller costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will use std::vector<geometry_msgs::PoseStamped> global_plan_;
-
其实最重要的就是去找到代价最小代价的velocity。通过在三个维度（x，y，theta）的速度，加速度的采样，得到候选velocity，然后对这些velocity做cost 评判，评判的标准是：
-
if (!heading_scoring_) { cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost; } else { cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost + 0.3 * heading_diff; } traj.cost_ = cost;
-
那么，这里面的path_dist，goal_dist，occ_cost，heading_diff 是怎么计算的呢？
occ_cost的计算很简单，直接通过costmap就能得到这个值
-
double footprint_cost = footprintCost(x_i, y_i, theta_i); occ_cost = std::max( std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
-
计算path_dist，goal_dist：
-
path_dist = path_map_(cell_x, cell_y).target_dist; goal_dist = goal_map_(cell_x, cell_y).target_dist;
-
那么如何更新path_dist，goal_dist: 需要costmap_ , global_plan_
-
path_map_.setTargetCells(costmap_, global_plan_); goal_map_.setLocalGoal(costmap_, global_plan_);
-
首先看 成员函数setTargetCells：
-
//update what map cells are considered path based on the global_plan void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,const std::vector<geometry_msgs::PoseStamped>& global_plan)
-
这个函数会根据global_plan更新costmap，如何做到的呢？
于是重点来了，最后一行computeTargetDistance(path_dist_queue, costmap);，这个函数的实际算法实现：
-
void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){ MapCell* current_cell; MapCell* check_cell; unsigned int last_col = size_x_ - 1; unsigned int last_row = size_y_ - 1; while(!dist_queue.empty()){ current_cell = dist_queue.front(); dist_queue.pop(); if(current_cell->cx > 0){ check_cell = current_cell - 1; if(!check_cell->target_mark){ //mark the cell as visisted check_cell->target_mark = true; if(updatePathCell(current_cell, check_cell, costmap)) { dist_queue.push(check_cell); } } } if(current_cell->cx < last_col){ check_cell = current_cell + 1; if(!check_cell->target_mark){ check_cell->target_mark = true; if(updatePathCell(current_cell, check_cell, costmap)) { dist_queue.push(check_cell); } } } if(current_cell->cy > 0){ check_cell = current_cell - size_x_; if(!check_cell->target_mark){ check_cell->target_mark = true; if(updatePathCell(current_cell, check_cell, costmap)) { dist_queue.push(check_cell); } } } if(current_cell->cy < last_row){ check_cell = current_cell + size_x_; if(!check_cell->target_mark){ check_cell->target_mark = true; if(updatePathCell(current_cell, check_cell, costmap)) { dist_queue.push(check_cell); } } } } }
-
上述代码检查current_cell 的四个邻接cell， 然后不断的扩散，每个cell相对于之前的cell，更新target_dist :
-
double new_target_dist = current_cell->target_dist + 1; if (new_target_dist < check_cell->target_dist) { check_cell->target_dist = new_target_dist; }
-
-
inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell, const costmap_2d::Costmap2D& costmap){ //if the cell is an obstacle set the max path distance unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy); if(! getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){ check_cell->target_dist = obstacleCosts(); return false; } double new_target_dist = current_cell->target_dist + 1; if (new_target_dist < check_cell->target_dist) { check_cell->target_dist = new_target_dist; } return true; }
-
参考：http://blog.csdn.net/u013158492/article/details/50512900
详解：
1 体系结构
（1）主要成员
-
base_local_planner::LocalPlannerUtil planner_util_; 用来存储运动控制参数以及costmap2d、tf等，会被传入dp_ costmap_2d::Costmap2DROS* costmap_ros_; base_local_planner::OdometryHelperRos odom_helper_; 用来辅助获取odom信息，会被传入dp_ boost::shared_ptr dp_; 正常的dwa运动控制类 base_local_planner::LatchedStopRotateController latchedStopRotateController_; 到达目标点后的停止旋转运动控制类
-
（2）主要接口
-
void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)—初始化 bool setPlan(const std::vector& orig_global_plan)—设置全局路径 bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)—计算控制速度 bool isGoalReached()—判断是否达到目标点
-
2 总体流程
- 在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlan将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。
- 在运动规划之前，move_base先利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity），如果是则控制结束，即发送0速，且复位move_base的相关控制标记，并返回action成功的结果。否则，利用DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)计算局部规划速度。
- 在DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)中，会先将全局路径映射到局部地图中，并更新对应的打分项（参考DWAPlanner::updatePlanAndLocalCosts函数，和具体的打分项更新）。然后，利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令（取决于是否机器人已经停稳，进而决定实现减速停止或者旋转至目标朝向），否则利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。
- 对于停止时的处理逻辑，即LatchedStopRotateController的所有逻辑，参考“停止”章节。
- 对于DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)函数直接利用DWAPlanner::findBestPath( tf::Stamped global_pose, tf::Stamped global_vel, tf::Stamped& drive_velocities, std::vector footprint_spec)获取最优局部路径对应的速度指令。
- 在DWAPlanner::findBestPath函数中，先利用SimpleTrajectoryGenerator::initialise( const Eigen::Vector3f& pos, const Eigen::Vector3f& vel, const Eigen::Vector3f& goal, base_local_planner::LocalPlannerLimits_ limits, const Eigen::Vector3f& vsamples, bool discretize_by_time = false)初始化轨迹产生器，即产生速度空间。
- 然后，利用SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector_ all_explored = 0)查找最优的局部路径。在该函数中，先调用每个打分项的prepare函数进行准备，参考“打分”章节。然后，利用SimpleScoredSamplingPlanner里面存储的TrajectorySampleGenerator轨迹产生器列表（目前，只有SimpleTrajectoryGenerator一个产生器）分别进行轨迹产生和打分，并根据打分选出最优轨迹。对于每个轨迹的打分，调用SimpleScoredSamplingPlanner::scoreTrajectory执行，在该函数中，遍历各个打分项进行打分并进行求和，如果某个打分项打分小于0，则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前，先获取对应的scale参数，如果scale为0证明该打分项无效，则跳过。如果获取到正常的打分项打分后（利用打分项的scoreTrajectory函数），将打分项乘以对应的scale作为最终的打分项打分。
- 具体的速度采样和轨迹规划参考“运动规划”章节。
- 接着在DWAPlanner::findBestPath中，根据使能参数，选择发布轨迹相关的可视化数据。
然后，如果最优轨迹有效，则利用最优轨迹更新摆动打分项的标志位。
最后，在返回最优规划轨迹之前，判断最优轨迹代价，如果代价小于0，则代表最优轨迹无效，即没有有效轨迹，则将返回速度指令，设置为0。 - 最后，如果DWAPlannerROS::dwaComputeVelocityCommands会判断得到的最优轨迹代价值，如果小于0，则该函数会返回false，从而引起move_base进行进一步的处理，如重新规划、recovery等。
- 至此，完成了整个dwa local planner的流程。
3 运动规划
在每个控制周期内，DWAPlanner::findBestPath会查找最优局部轨迹。
在DWAPlanner::findBestPath中，首先调用SimpleTrajectoryGenerator::initialise进行速度采样，然后利用SimpleScoredSamplingPlanner::findBestTrajectory根据速度采样空间进行轨迹产生、打分、筛选，从而得到最优局部轨迹。
（1）采样
速度采样在SimpleTrajectoryGenerator::initialise中进行，总体思路是先获取速度空间边界，然后根据采样个数参数在空间内进行采样。
在获取空间边界时，根据use_dwa参数，采用两套策略。
如果use_dwa==false，首先用当前位置与目标位置的距离处理仿真时间sim_time（模拟仿真时间内匀减速到0，刚好到达目标点的情景）得到max_vel_x和max_vel_y边界，然后基于acc_lim _ sim_time得到一种边界（上边界），还有设置的速度极限参数（max_vel_xxx，min_vel_xxx）作为一种边界，然后选取三种边界中空间较小的边界。这种策略，能够获得较大的采样空间（因为用了sim_time）。
如果use_dwa==true，则直接用acc_lim _ sim_period得到边界，然后还有设置的速度极限参数作为边界，然后选取两种边界中空间较小的边界。
得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间。
（2）获取最优轨迹
获取最优轨迹在SimpleScoredSamplingPlanner::findBestTrajectory中进行，在该函数中，首先调用各个打分项的prepare函数进行准备。然后遍历std::vector
4 路径
（1）存储全局路径
路径类型为std::vector。
在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlann将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。
在DWAPlanner::setPlann中，会先将OscillationCostFunction中的摆动标志位复位，然后利用planner_util_->setPlan将路径传入planner_util_，直接保存为global_plan_。
此时的路径时相对于全局地图的全局坐标系的（通常为”map”)。
（2）局部路径映射及存储
在computeVelocityCommands计算速度前，会先将全局路径映射到局部地图坐标系下（通常为“odom”），通过LocalPlannerUtil::getLocalPlan(tf::Stamped& global_pose, std::vector& transformed_plan)实现。
在getLocalPlan中，先将较长的全局路径映射并截断到局部地图内（即坐标系转换为局部地图，且范围完全在局部地图内，超出地图的则抛弃）。然后，裁减全局路径和局部路径（与机器人当前位置距离超过1m的旧的路径会被裁减掉）。
裁减过的全局路径还保存在planner_util_中，映射并经过裁减后的局部路径会在computeVelocityCommands函数中传入dp_中（利用DWAPlanner::updatePlanAndLocalCosts函数）。
在updatePlanAndLocalCosts中，会将局部路径保存到dp_的global_plan_中（对于dp_来说，局部地图中映射的全局路径就是全局的，而dp_自己规划的路径是局部的）。然后，将该路径信息传入各个打分对象，具体参考打分章节。
5 打分
打分对象一共6个，分别为：
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_（对齐打分）
其中，摆动打分和避障打分项是独立的类型，后四个打分项是同一类型。
打分计算过程中出现的负的值可以认为是错误代码（用于指示具体的出错原因），而不是得分，该说明对下面所有的打分描述有效。
如果轨迹为空（在产生轨迹时出错，例如不满足最大最小速度要去），则各个打分项对应的得分都为0。
（1）打分对象初始化及更新
oscillation_costs_
在DWAPlanner的构造函数中，利用oscillation_costs_.resetOscillationFlags()复位摆动标志位（侧移、旋转、前进方向的相关参数strafe_pos_only_，strafe_neg_only_，strafing_pos_，strafing_neg_，rot_pos_only_，rot_neg_only_，rotating_pos_，rotating_neg_，forward_pos_only_，forward_neg_only_，forward_pos_，forward_neg_）。
然后将oscillation_costs传入打分项列表（也会加入其它打分项），并将打分项列表std::vector
6 停止
对于DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。
（1）停止判断
在move_base的每个控制周期，都会利用利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity）。
对于LatchedStopRotateController::isGoalReached函数，分为锁存目标和不锁存目标两种处理逻辑。锁存目标，即如果机器人在行驶过程中出现过位置满足xy_goal_tolerance的条件时，则设置xy_tolerance_latch_标志位，代表已经达到过目标，不用考虑最终是否超出目标点，直接进行停止、旋转至目标朝向即可。
如果不锁存，则必须始终满足机器人当前位置是否满足xy_goal_tolerance的条件，满足则代表到达了目标位置。
在到达目标位置的前提下，还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即x和y的速度小于trans_stopped_velocity，theta速度小于rot_stopped_velocity。
只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回true。
（2）加速停止
在DWAPlannerROS::computeVelocityCommands的每个计算周期内，都会先利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令，否则才会使用DWAPlanner计算最优轨迹对应的速度命令。
对于isPostionReached的判断，和isGoalReached中判断达到位置的逻辑一样，即如果启动了锁存，且xy_tolerance_latch_标志位被标记（即满足过xy_goal_tolerance条件），则认为已经到达；如果未启用锁存，则需要基于当前位置是否满足xy_goal_tolerance确定是否到达了位置。
在LatchedStopRotateController::computeVelocityCommandsStopRotate函数中，会假设用户想要进行旋转停止了，即已经到达位置了，因此会先判断当前朝向是否满足yaw_goal_tolerance要求，如果满足，则将速度指令设置为0（这种做法太过粗略）。
如果朝向没有满足要求，则会判断是要进行加速停止还是旋转至目标点。
如果机器人还未处于旋转停止状态（通过rotating_to_goal标记），并且速度没有满足停止要求，则会调用stopWithAccLimits进行加速停止处理，否则代表应进入了旋转停止状态，则会调用rotateToGoal进行旋转至目标朝向处理，同时设置rotating_to_goal标志位true。
对于stopWithAccLimits函数，对于利用acc_lim参数进行最快的减速命令计算，然后利用DWAPlanner::checkTrajectory函数计算出轨迹，并进行轨迹打分。在计算轨迹时利用了SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj)，如果速度不满足最小速度要求（min_rot_vel、min_trans_vel）则轨迹为空。
在对轨迹进行打分时，利用了SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)，如果轨迹为空，则打分为0，此时虽然checkTrajectory返回true，但对应的速度可能过小（目前判断没有太大影响）。
如果checkTrajecotry返回true（轨迹打分大于等于0）则代表路径有效，stopWithAccLimits计算出的加速停止速度命令有效，否则设置速度命令为0。
（3）旋转至目标朝向
旋转至目标朝向有rotateToGoal函数计算，首先将x和y速度设置为0，对于theta速度，利用当前朝向与目标朝向的差的比例控制（系数为1）产生，然后用加速能力（acc_lim*sim_period）、速度限幅（max_rot_vel，min_rot_vel）、减速能力（v^2=2as公式）进行速度限幅。接着利用DWAPlanner::checkTrajectory进行轨迹打分检测，逻辑同前。
评论（0）
您还未登录，请登录后发表或查看评论