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             # 地图更新频率，单位 hzpublish_frequency: 1.0            # 地图发布频率，单位 hztransform_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                        # 代价地图的宽度，单位 mheight: 3.0update_frequency: 10.0            # 局部代价地图的更新频率，一般和激光雷达的扫描频率保持一致publish_frequency: 10.0           # 局部代价地图的发布频率transform_tolerance: 1.0

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