11. move_base详解
11.1. move_base配置参数解读
参考链接：https://blog.csdn.net/banzhuan133/article/details/90239252
https://wenku.baidu.com/view/1ae6d9a56729647d27284b73f242336c1fb9304d.html
https://blog.csdn.net/qq_29313679/article/details/106237063
在仿真中，查看turtlebot3_navigation/launch/move_base.launch，发现局部规划器用的是DWAPlannerROS，同时加载了下面几个yaml文件：
costmap_common_params_$(arg model).yaml
local_costmap_params.yaml
global_costmap_params.yaml
move_base_params.yaml
dwa_local_planner_params_$(arg model).yaml
这里的(arg model)用的burger模型。
在rviz下，直接用鼠标选定2D navigation goal就可以指定目标了，但是，你可能会发现效果不太好，比如，疯狂原地打转，走S型，绕大圈等现象。这些都可以通过调参搞定。
重点看后两个move_base_params.yaml、dwa_local_planner_params_$(arg model).yaml文件，地图相关参数放到Costmap2DROS分析章节中。
move_base_params.yaml:
shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
recovery_behavior_enabled: true
clearing_rotation_allowed: true
dwa_local_planner_params.yaml：
DWAPlannerROS:
max_vel_x: 0.22
min_vel_x: -0.22
max_vel_y: 0.0
min_vel_y: 0.0
max_vel_trans: 0.22
min_vel_trans: 0.11
max_vel_theta: 2.75
min_vel_theta: 1.37
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
latch_xy_goal_tolerance: true
trans_stopped_vel: 0
sim_time: 1.5
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
oscillation_reset_dist: 0.05
publish_traj_pc : true
publish_cost_grid_pc: true
11.2. 启动流程
move_base节点源文件位于/navigation-melodic-devel/move_base/move_base_node.cpp，其调用了move_base.cpp中的MoveBase的构造。主要的初始化过程也在这个构造中。main函数只有下面几句话：
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
move_base::MoveBase move_base( buffer );
由节点关系图可以看到，这里监听的"/tf"来自定位模块，可以是amcl或其他定位模块，传输的是各个坐标系间的转换关系。
构造流程下：
- 构建MoveBaseActionServer，基于ROS的actionlib机制启动了一个action服务，主要作用是接收客户端发送的目标点，在回调中处理目标点并驱动AGV行走，回调函数是
executeCb； - 各个参数的初始化，在上面的仿真中，很多默认参数已经被配置文件修改了，如上面的仿真中局部规划器被改为了DWA；
- 创建全局规划器线程；
- 创建全局地图和局部地图；
- 加载全局规划器、局部规划器的插件；
- 初始化状态机状态等等。
11.3. executeCb回调
该回调函数在收到目标点时会触发调用，收到新的目标点的话会通过ROS的actionlib机制进行处理，可以调用一些接口来判断，actionlib我们不关心，大概的流程如下：
- 通过一个信号量
planner_cond_激发了全局规划器线程进行了规划， - 定义变量
ros::Rate r(controller_frequency_)来控制下面while循环的频率，这个频率比较关键，最终体现在发送cmd_vel指令的频率上，我们称之为“控制频率”，程序中好多地方用到； - 中间过程不关心，调用了
executeCycle()接口，来控制AGV行走，该接口会返回AGV是否到达目标点。
11.4. executeCycle接口
本函数的核心在状态CONTROLLING处理逻辑中：
- 通过调用
LatchedStopRotateController类的isGoalReached()接口判断是否到达目标点，这个类参考后面的<目标点达到控制>章节； - 通过调用
dwa_planner_ros.cpp中DWAPlannerROS类的computeVelocityCommands()接口来计算下发给AGV底盘的指令。这里AGV清除恢复CLEARING及流程我们不关心。
11.5. DWAPlannerROS::computeVelocityCommands
DWA参考链接： http://gaoyichao.com/Xiaotu/?book=turtlebot&title=dwa_local_planner
本接口会进行一个是否到达目标点的判断（通过LatchedStopRotateController）：
- 最开始部分会调用
planner_util_.getLocalPlan()接口，将原/map坐标系下的全局路径转换到/odom坐标系下； - 调用
LatchedStopRotateController::isPositionReached只判断XY是否到达，如果到达那么会调用latchedStopRotateController_.computeVelocityCommandsStopRotate()来判断角度是否到达，角度没到达进行旋转控制，否则发送停止；如果没到达xy，那么会调用DWAPlannerROS::dwaComputeVelocityCommands()来把最佳轨迹对应的速度作为下发底盘的速度cmd_vel。xy到达后的旋转参考后面的<目标点到达控制>章节； - 未到达目标点，调用
DWAPlanner类的findBestPath()接口，它又调用了SimpleScoredSamplingPlanner接口中的findBestTrajectory()，后者会通过SimpleTrajectoryGenerator类创建出轨迹，并调用代价算法来计算轨迹得分。
DWAPlanner类中存储了7种代价计算方法，分别是：
OscillationCostFunction oscillation_costs_
ObstacleCostFunction obstacle_costs_
MapGridCostFunction goal_front_costs_
MapGridCostFunction alignment_costs_
MapGridCostFunction path_costs_
MapGridCostFunction goal_costs_
TwirlingCostFunction twirling_costs_
通过设定的比例系数，来对各个代价进行加权得出最后的代价数值。当计算出最优轨迹后，将轨迹对应的速度下发到底盘。
11.5.1. SimpleTrajectoryGenerator轨迹创建
该类中主要的接口有两个：initialise和generateTrajectory。
- initialise接口首先根据<x,y,th>三个加速度，根据当前速度和“控制频率”时间，计算一个时间片后的最大和最小速度；然后根据设定的采样数量对三个速度方向进行<min_vec,max_vec>范围采样，得到一堆速度（turtlebot3仿真中的参数得到了800个速度）；
- generateTrajectory接口，一个速度计算一段轨迹，轨迹中点的个数计算公式：
线速度仿真距离 = 线速度 * 采样时间；
角速度速度仿真距离 = 角速度速度 * 采样时间；
线速度方向个数 = 线速度仿真距离 / 线速度仿真间隔；
角速度方向个数 = 角速度仿真距离 / 角速度仿真间隔；
总数量 = max(线速度方向个数，角速度方向个数)；
11.6. MapGridCostFunction代价计算
有四种代价都是通过本类实现的，这里只关注了两个主要的：贴近全局轨迹和贴近全局轨迹与局部地图的交点。
这个类主要的接口有两个：prepare()和scoreTrajectory()。
- prepare():其中调用的逻辑是由
MapGrid类实现的，需要提前调用，生成代价数据表来加速代价查询。该函数有两个作用:
一个是提前算好局部网格地图（注意是网格地图）中每个网格点距离全局路径在局部地图中的单位长度，如图，其中0是全局路径占用的网格，其他网格数据通过一个扩散的算法实现的(不是欧式距离)，这里1 2 3代表离0的一个相对距离：
3 2 1 0 1 2 3
3 2 1 0 1 2 3
2 1 0 1 1 2 3
1 0 1 2 3 4 5
1 0 1 2 4 4 5
另一个是计算每个网格到全局路径和局部地图交点的单位长度，也是通过同样的扩散算法实现的，这里0是目标点：
- 2 1 0 1 2 -
- 3 2 1 2 3 -
- 4 3 2 3 4 -
- scoreTrajectory():这个函数很简单，就是直接查询代价表，来通过三种策略计算轨迹的代价值：
Last:默认是这个，只保留最后一个轨迹点距离全局轨迹的相对距离，来作为最终的代价；
Sum:所有点相对距离的累加；
Product:所有点相对距离的累乘。
11.7. OscillationCostFunction代价计算
本代价是为了减少车身的晃动。逻辑非常简单，总体来说就是AGV往前行驶一段距离的过程中，每次创建轨迹并进行打分时，都会考虑这些轨迹的速度和初始速度的正负关系，保证<x,y,th>都和初始速度方向一致。行驶一段距离后会重置初始的速度方向，重新开始统计。
关键的接口有：
- updateOscillationFlags() : 每次计算出最优轨迹时调用，如果超出了设定的距离，会重置检测标记；
- scoreTrajectory() : 对某个轨迹进行打分，就是看看轨迹的<x, y, th>速度方向是否和初始设置的一致。
11.8. 目标点到达控制(LatchedStopRotateController)
isPositionReached(): 只判断XY是否到达，不判断角度是否到达。isGoalReached(): 判断xy和朝向角都在误差范围、且会通过里程计信息判断<x,y,th>速度是满足到达误差。此函数会在CONTROLLING状态下每次控制频率到后都会调用，以此来切换控制状态。其中有个标志latch_xy_goal_tolerance_可以对xy判断进行锁存，意思是说如果如果该标记为true且曾经满足当前位置和目标位置的xy在误差范围内，那么后续再调用本接口时，不再判断xy是否满足误差范围，只判断角度和速度是否满足误差范围，这样到达目标点(xy到达)后调整角度过程中，如果xy偏离了也不关心。核心代码如下：
if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
xy_tolerance_latch_ = true;
}
if (fabs(angle) <= limits.yaw_goal_tolerance) {
if (base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)) {
return true;
}
}
return false;
computeVelocityCommandsStopRotate() : 该函数的进入条件为xy已经到了，然后判断角度是否到了，如果到了那么直接发送0速度来强行停止（如果第一次到达xy并且角度也满足，这样做太粗暴了；如果角度不满足，这里就是旋转过程中控制停止的条件，是可以的）。如果没到那么会检查是否速度<x,y,th>已经满足停止条件：
- 满足：此时可以认为机器人已经停下了，则进行旋转操作rotateToGoal()，这里会根据最大最小限速及和目标角度的夹角计算角速度，
这里注意当离目标角度越近的时候，速度计算会直接采用这个距离，也就是旋转过程离目标点越近旋转速度越慢，并下发下去，进行下次循环，继续在computeVelocityCommandsStopRotate中判断是否角度到了发送0速度； - 不满足：则进行减速操作，通过加速度计算下个仿真周期减速后的<x,y,th>方向的速度，这里会调用轨迹检查接口，不用关心该调用。发送的速度计算：v-at,如果为负的，那么就是0。
这个目标点到达控制如果在到达目标点且角度也一样时，会强行停止，比如在行驶速度特别大的时候，可能会出现急停滑行的问题等等，实际应用的话还需要改进。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
