move_base

move_base 官方介绍：http://wiki.ros.org/move_base

如果在仿真环境下， sensor source、odometry source 和 sensor transforms 都已提供好，我们只需要完成以下部分：

一、编写导航程序

①创建 ROS 工作空间和 pkg 包

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_create_pkg nav_pkg roscpp rospy move_base_msgs actionlib

②在 pkg 中创建 nav.launch 文件，其中包含了上面介绍的3个节点。

<launch>
    <!--- Run move_base -->
    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    </node>

    <!-- Run map server -->
    <node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>

    <!--- Run AMCL -->
    <node pkg="amcl" type="amcl" name="amcl"/>

    <!--- Run rviz -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find wpr_simulation)/rviz/nav.rviz"/>
</launch>

③编译工作空间

二、运行导航程序

①运行仿真环境

roslaunch wpr_simulation wpb_stage_robocup.launch

②运行 nav.launch 文件

roslaunch nav_pkg nav.launch

③打开 rviz

④添加 Map（话题： /map）；Path（话题：/move_base/GlobalPlanner/plan）

其中运行 nav.launch 文件产生的话题如下：

1、map_server 产生的话题

/map

说明：发布静态地图数据，通常为 nav_msgs/OccupancyGrid 类型。
来源：由 map_server 节点生成，用于全局路径规划。

/map_metadata

说明：发布地图的元数据信息，如地图的分辨率、宽度、高度和原点位置。通常为 nav_msgs/MapMetaData 类型
来源：由 map_server 节点生成，辅助其他节点理解地图的结构和坐标系

2、amcl 产生的话题

/amcl_pose

说明：发布 amcl 计算出的机器人在地图中的位姿，通常为geometry_msgs/PoseWithCovarianceStamped 类型。
来源：由 amcl 节点生成，用于定位和导航。

/initialpose

说明：用于设置机器人的初始位姿，通常通过 rviz 手动发布该话题。通常为 geometry_msgs/PoseWithCovarianceStamped 类型
来源：由用户或 rviz 发布，用于 amcl 节点初始化机器人位置。

/diagnostics

说明：发布系统诊断信息，包括节点状态、传感器健康状况等，通常为 diagnostic_msgs/DiagnosticArray 类型。
来源：由 amcl 节点生成，用于监控和调试系统状态

/particlecloud

说明：发布粒子滤波器的粒子分布，可以在 rviz 中可视化。通常为 geometry_msgs/PoseArray 类型。
来源：由 amcl 节点生成，用于可视化粒子滤波器的状态。

/amcl/parameter_descriptions 和 /amcl/parameter_updates

说明：用于动态参数调整（dynamic reconfigure），允许在运行时修改 amcl 的参数。前者通常为 dynamic_reconfigure/ConfigDescription 类型；后者为 dynamic_reconfigure/Config 类型。
来源：由 amcl 节点生成，支持参数的动态管理。

3、move_base 产生的话题

3.1 move_base 的总体控制和状态

/move_base/current_goal

说明：发布当前的导航目标，通常为 geometry_msgs/PoseStamped 类型。
来源：由 move_base 节点生成，表示当前正在执行的目标。

/move_base/goal 和 /move_base_simple/goal

说明：
- /move_base/goal：用于接收复杂的导航目标，通常为 move_base_msgs/MoveBaseActionGoal 类型。
- /move_base_simple/goal：用于接收简化的导航目标，通常通过 rviz 发布，类型为 geometry_msgs/PoseStamped。
来源：由用户通过 rviz 或其他工具发布，用于设置导航目标。

/move_base/recovery_status

说明：发布导航恢复行为的状态信息，如避障操作的执行情况。通常为 move_base_msgs/RecoveryStatus 类型。
来源：由 move_base 节点生成，用于监控恢复行为。

/move_base/cancel

说明：用于取消当前的导航目标，通常为 actionlib_msgs/GoalID 类型。
来源：由用户或其他节点发布，用于终止当前导航任务。

/move_base/feedback

说明：发布导航过程中的实时反馈信息，通常为 move_base_msgs/MoveBaseActionFeedback 类型。
来源：由 move_base 节点生成，用于监控导航进展。

/move_base/result 和 /move_base/status

说明：
- /move_base/result：发布导航结果，通常为 move_base_msgs/MoveBaseActionResult 类型。
- /move_base/status：发布当前导航的状态，通常为 actionlib_msgs/GoalStatusArray 类型。
来源：由 move_base 节点生成，用于反馈导航任务的完成情况和状态。

/move_base/parameter_descriptions 和 /move_base/parameter_updates

说明：用于动态调整 move_base 节点的整体参数。
来源：由 move_base 节点生成，支持参数的动态管理。

3.2全局规划器（Global Planner）

/move_base/GlobalPlanner/parameter_descriptions 和 /move_base/GlobalPlanner/parameter_updates

说明：用于动态调整全局规划器的参数。
来源：由 move_base 的全局规划器（如 GlobalPlanner）生成，支持参数的动态管理。前者通常为 dynamic_reconfigure/ConfigDescription 类型；后者为 dynamic_reconfigure/Config 类型。

/move_base/GlobalPlanner/plan

说明：发布全局规划路径的坐标点，通常为 nav_msgs/Path 类型。
来源：由 GlobalPlanner 生成，用于展示全局路径。

/move_base/GlobalPlanner/potential

说明：发布潜在场（potential field）地图，用于全局路径规划的成本计算。通常为 nav_msgs/OccupancyGrid 类型。
来源：由 GlobalPlanner 生成，辅助路径规划。

3.3局部规划器（Local Planner）

/move_base/WpbhLocalPlanner/local_planner_target

说明：发布局部规划的目标点，通常为 geometry_msgs/PoseStamped 类型。
来源：由 WpbhLocalPlanner 生成，用于局部路径调整。

3.4全局代价地图（Global Costmap）

/move_base/global_costmap/costmap 和 /move_base/global_costmap/costmap_updates

说明：发布全局代价地图及其更新数据，通常为 nav_msgs/OccupancyGrid 和 map_msgs/OccupancyGridUpdate 类型。
来源：由 move_base 的全局代价地图生成，辅助全局路径规划。

/move_base/global_costmap/footprint

说明：发布机器人轮廓信息，通常为 geometry_msgs/PolygonStamped 类型。
来源：由 move_base 全局代价地图生成，用于计算安全代价区域。

/move_base/global_costmap/inflation_layer/parameter_descriptions 和 /move_base/global_costmap/inflation_layer/parameter_updates

说明：用于膨胀层的动态调参，用于障碍物周围的安全缓冲区设置。
来源：由 move_base 的膨胀层生成，增加障碍物周围的安全缓冲区。

/move_base/global_costmap/obstacle_layer/parameter_descriptions 和 /move_base/global_costmap/obstacle_layer/parameter_updates

说明：用于动态障碍物图层的动态调参。
来源：由 move_base 的障碍物层生成，处理动态障碍物信息。

/move_base/global_costmap/static_layer/parameter_descriptions 和 /move_base/global_costmap/static_layer/parameter_updates

说明：用于静态图层的动态调参。
来源：由 move_base 的静态层生成，处理静态障碍物和背景地图。

/move_base/global_costmap/parameter_descriptions 和 /move_base/global_costmap/parameter_updates

说明：用于调整全局代价地图的整体参数。
来源：由 move_base 全局代价地图生成，支持参数的动态管理。

3.5局部代价地图（Local Costmap）

/move_base/local_costmap/costmap 和 /move_base/local_costmap/costmap_updates

说明：发布局部代价地图及其更新数据，通常为 nav_msgs/OccupancyGrid 和 map_msgs/OccupancyGridUpdate 类型。
来源：由 move_base 的局部代价地图生成，辅助局部路径规划。

/move_base/local_costmap/footprint

说明：发布机器人轮廓信息，通常为 geometry_msgs/PolygonStamped 类型。
来源：由 move_base 局部代价地图生成，用于计算安全代价区域。

/move_base/local_costmap/inflation_layer/parameter_descriptions 和 /move_base/local_costmap/inflation_layer/parameter_updates

说明：用于膨胀层的动态调参，用于障碍物周围的安全缓冲区设置。
来源：由 move_base 的膨胀层生成，增加障碍物周围的安全缓冲区。

/move_base/local_costmap/obstacle_layer/parameter_descriptions 和 /move_base/local_costmap/obstacle_layer/parameter_updates

说明：用于动态障碍物图层的动态调参。
来源：由 move_base 的障碍物层生成，处理动态障碍物信息。

/move_base/local_costmap/parameter_descriptions 和 /move_base/local_costmap/parameter_updates

说明：用于调整局部代价地图的整体参数。
来源：由 move_base 局部代价地图生成，支持参数的动态管理。

三、move_bose 节点参数解析

    <!--- Run move_base -->
    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />

        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    </node>

1、全局规划器

官方介绍：global_planner - ROS Wiki

1.1 广度优先算法 BFS（Dijkstra算法）

1.2 深度优先算法 DFS（A*）

1.3 move_base 中的全局规划器

move_base 共有 3 个全局规划器，默认使用 Navfn规划器。其中前两个规划器中均包含 Dijkstra算法和 A*算法，都默认使用 Dijkstra算法，但 Navfn规划器中的 A*算法存在 Bug。

若想使用 Global_planner规划器中的 A*算法，需要加上如下代码：

<!-- 使用 GlobalPlanner规划器 中的 A*算法 -->
<param name="GlobalPlanner/use_dijkstra" value="false" />
<param name="GlobalPlanner/use_grid_path" value="true" />

Carrot_planner规划器：从起始点到目标点延伸一条路径，遇到障碍物就停止。代码简单，经常被用来作为自定义规划器的模版进行修改。

1.4 自定义规划器

move_base 支持自己编写自定义全局规划器，提供了一种 Plugin 插件接口，只要按照特定的格式，就能把自己的路径规划算法编写成新的规划器，加载到 move_base 节点中使用。

2、AMCL (Adaptive Mentcarto Localization)自适应蒙特卡罗定位算法

官方介绍：amcl - ROS Wiki

AMCL：使用粒子滤波在已知地图中进行定位的算法。同时使用了里程计和激光雷达数据，具有较强的自我纠错功能。rviz 中添加 PoseArray 订阅话题 /particlecloud 可查看 AMCL 产生的粒子。

AMCL参数：

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <!-- 机器人的运动模型为差动驱动模型，即机器人只能前后运动和原地旋转，无法侧向移动。 -->
  <param name="odom_model_type" value="diff"/>
  <!-- 机器人的运动模型为全向运动模型，允许机器人在平面上向任何方向移动。 -->
  <param name="odom_model_type" value="omni"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <!-- 粒子滤波的粒子数 -->
  <param name="min_particles" value="50"/>
  <param name="max_particles" value="500"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="odom_frame_id" value="odom"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

其中 AMCL 负责输出 map 到 odom 的 tf ；里程计负责输出 odom 到 base_frame 的 tf 。从而形成完整的 map 到 base_frame 的 tf。

注意：AMCL 切换本体和分身是在 map 到 odom 这段的 tf 上产生跳跃突变来实现的，所以在导航过程中会看到机器人位置跳变，这就是 AMCL 输出的这段 tf 突变产生的结果；而里程计输出的 odom 到 base_frame 这段 tf 通常是保持连续变化的，不会突然跳变（这个特征在生成代价地图时会用到）。

3、代价地图

官方介绍：costmap_2d - ROS Wiki

<!-- 代价地图参数 -->
<!-- 通过 命名空间 ns 实现了用一个文件给全局和局部两个代价地图设置一样的参数，参数是关于代价地图的形状的 -->
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<!-- 代价地图的计算范围和频率 -->
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />

3.1 代价地图的形状参数

robot_radius: 0.25              # 机器人底盘半径
inflation_radius: 0.5           # 膨胀区域的半径
obstacle_range: 6.0             # 激光雷达障碍物检测距离，单位 m
raytrace_range: 6.0             # 清除动态障碍物的残留影子
observation_sources: base_lidar # 障碍物的观测来源，与下文保持一致

# 观测来源的数据参数
base_lidar: {
    data_type: LaserScan, # 消息的类型
    topic: /scan,         # 话题名称
    marking: true,        # 是否将扫描到的障碍物添加到代价地图
    clearing: true        # 是否清除扫描范围内的障碍物残影
    }

3.2 全局代价地图的计算范围和频率参数

global_costmap:
  global_frame: map                 # 地图坐标系名称
  robot_base_frame: base_footprint  # 底盘坐标系名称
  static_map: true                  # 是否将 map_server 发来的地图数据作为初始代价地图
  update_frequency: 1.0             # 地图更新频率，单位 hz
  publish_frequency: 1.0            # 地图发布频率，单位 hz
  transform_tolerance: 1.0          # transform 延迟容忍值，单位 s。如出现 tf 的 timeout 错误，调大该值。

其中 transform 指的是传感器到 map 的 tf ，包含 3 段

3.3局部代价地图的计算范围和频率参数

local_costmap:
  global_frame: odom                # 地图坐标系名称
  robot_base_frame: base_footprint  # 底盘坐标系名称
  static_map: false                 # 是否将 map_server 发来的地图数据作为初始代价地图
  rolling_window: true              # 局部代价地图是否和底盘一起移动
  width: 3.0                        # 代价地图的宽度，单位 m
  height: 3.0
  update_frequency: 10.0            # 局部代价地图的更新频率，一般和激光雷达的扫描频率保持一致
  publish_frequency: 10.0           # 局部代价地图的发布频率
  transform_tolerance: 1.0

这里的 global_frame 设置为 odom 而不是 map，原因是 AMCL 是通过 map 到 odom 这段的 tf 的跳变来切换机器人和分身的位置的，如果以 map 为基准坐标系，当机器人的位置跳变时，传感器检测到的障碍物位置也会跳变，这对于全局路径规划来说问题不大，但对于局部路径规划来说，会使机器人运动变得不平稳。所以局部代价地图的 global_frame 通常会设置为 odom。