move_base

        navigation 官方介绍：cn/navigation - ROS Wiki

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

        move_base 必须订阅 3 个话题：

• /map：map_server 发送的全局栅格地图
• /tf ：且 /tf 中必须包含 T_map_odom， T_odom_(base_footprint)
• /odom：/odom 为 T_odom_(base_footprint)， base_footprint 为 yaml 文件中自定义底盘坐标系
• /sensor_topics 可选：在 yaml 文件的 observation_sources 参数中定义，用于生成局部代价地图。如果不订阅任何话题，则局部代价地图会和全局代价地图的子地图保持一致，不会检测到临时障碍物。 

        如果在仿真环境下， 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">
        <!-- 代价地图参数 -->
        <!-- 通过 命名空间 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" />

        <!-- 设置 move_base 的全局路径规划器为 global_planner 规划器 -->
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <!-- 使用 GlobalPlanner规划器 中的 A*算法 -->
        <param name="GlobalPlanner/use_dijkstra" value="false" />
        <param name="GlobalPlanner/use_grid_path" value="true" />

        <!-- 设置 move_base 的局部路径规划器为 wpbh_local_planner 规划器-->
        <!-- <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" /> -->
        <!-- 设置 move_base 的局部路径规划器为 DWA 规划器 -->
        <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/dwa_local_planner_params.yaml" command="load" /> -->
        <!-- 设置 move_base 的局部路径规划器为 TEB 规划器 -->
        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/teb_local_planner_params.yaml" command="load" />
        
        <!-- 设置控制器的控制频率为 10 Hz -->
        <param name="controller_frequency" value="10" type="double" />

    </node>

    <!-- Run map server -->
    <!-- 全局静态地图，发布话题为 /map -->
    <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 nav_pkg)/rviz/nav.rviz"/>

    <!-- 航点导航插件 -->
    <!-- <node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen" />
    <node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen" /> -->
</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 节点生成，辅助其他节点理解地图的结构和坐标系

/map_updates

• 说明：描述了一个增量更新的地图，即只传递改变的部分（例如，新障碍物或移除的障碍物）。通常为 map_msgs/OccupancyGridUpdate 类型
• 来源：由 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。与 /move_base/goal 相比，没有连续反馈，无法追踪机器人执行状态)
• 来源：由用户通过 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）

        3.3.1 DWA 局部规划器

        3.3.2 TEB局部规划器

/move_base/TebLocalPlannerROS/teb_markers

• 说明：可视化 TEB 局部路径规划器的规划结果，用于在 RViz 中显示路径规划器生成的路径、障碍物、速度矢量等信息。通常为 visualization_msgs/Marker 类型。

        teb_markers 发布的不同命名空间的标记内容：

                ① PointObstacles 和 PolyObstacles 命名空间：用于可视化当前路径规划中被优化考虑的所有点状和多边形障碍物。

                ② TebContainer 命名空间：用于显示找到的所有路径及其在不同拓扑结构中的优化结果（仅在启用并行规划时）。

• 来源：由 TebLocalPlannerROS 生成，使开发人员可以清晰地看到路径、障碍物和优化后的轨迹。对于调整路径规划参数、理解算法行为和调试复杂环境中的局部路径规划器很有帮助。

        3.3.3 WpbhLocalPlanner 局部规划器

/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

        官方介绍：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*算法，需要加上如下代码：

<!-- 设置 move_base 的全局路径规划器为 global_planner 规划器 -->
<param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
<!-- 使用 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：使用粒子滤波在已知地图中进行重定位的算法。同时使用了 物理里程计输出的位姿 、 二维激光雷达数据和二维栅格地图匹配得到的位姿，具有较强的自我纠错功能。 AMCL 算法本质上是一种重定位算法，输出配准后的 T_map_odom，持续修正里程计的累计误差。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

        官方介绍：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 代价地图的形状参数

        参考：机器人footprint的研究

        参数模版：

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状
# footprint_padding: 0.01 #设置机器人轮廓的额外填充距离，主要用于导航和避障

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

        实际参数：

robot_radius: 0.25              # 机器人底盘半径
inflation_radius: 0.5           # 膨胀区域的半径
obstacle_range: 6.0             # 激光雷达障碍物检测距离，单位 m
raytrace_range: 6.0             # 清除动态障碍物的残留影子

plugins: 
    - {name: static_layer,     type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

static_layer:
    use_maximum: true          # 地图层与其他层合并时，选择最大值进行叠加。通常用于生成更为保守的代价地图
    map_topic: /map            # 将静态层使用来自外部的静态地图（如 /map 话题）构建代价地图

obstacles:
    observation_sources: base_lidar  # 障碍物的观测来源，与下文保持一致

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

         也可在 observation_sources 中添加深度相机和多线激光雷达作为额外观测，添加深度相机观测的示例如下：

robot_radius: 0.25              # 机器人底盘半径
inflation_radius: 0.5           # 膨胀区域的半径
obstacle_range: 6.0             # 激光雷达障碍物检测距离，单位 m
raytrace_range: 6.0             # 清除动态障碍物的残留影子

plugins: 
    - {name: static_layer,     type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

static_layer:
    use_maximum: true          # 地图层与其他层合并时，选择最大值进行叠加。通常用于生成更为保守的代价地图
    map_topic: /map            # 将静态层使用来自外部的静态地图（如 /map 话题）构建代价地图

obstacles:
    observation_sources: base_lidar head_kinect2 # 障碍物的观测来源，与下文保持一致

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

    head_kinect2: {
        sensor_frame: kinect2_ir_optical_frame, 
        data_type: PointCloud2, 
        topic: /kinect2/sd/points,         
        marking: true,        
        clearing: true,
        max_obstacle_height: 1.5, # 障碍物的高度，单位 m
        min_obstacle_height: 0.2  
    }

        可以看到，添加深度相机观测后，单线激光雷达检测不到的桌子也可以显示在代价地图中。 

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

        参数模版：

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

        实际参数： 

global_costmap:
  global_frame: map                 # 地图坐标系名称
  robot_base_frame: base_footprint  # 底盘坐标系名称
  static_map: false                 # 是否将 map_server 发来的地图数据作为初始代价地图
  rolling_window: true              # 滑动窗口，与 static_map 的值相反
  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 #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

        实际参数：

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。

        3.4 代价地图的初始化方式

        参考：move_base代码学习一

        有两种方法初始化costmap_2d::Costmap2DROS对象：

• static map(map_server)

  这种情况下，costmap 初始化的长度宽度和 static map 的一样，obstacle 的信息也来自 static map。
  一般用在 amcl 定位导航中，随着机器人的移动，使用传感器的信息更新 costmap

• rolling window

  自己给一个长，宽值，设置 rolling_window 参数为 true，这个参数设置机器人的位置在costmap 的中心，丢弃离机器人比较远的 obstacle 信息。
  一般用在在里程计坐标下的移动，机器人只关心在他周边的障碍物信息。

4、恢复行为 recovery_behaviors

        4.1 恢复行为流程

        恢复行为包含 重置 和 旋转清除。

        重置行为 官方介绍：clear_costmap_recovery - ROS Wiki

        旋转清除行为 官方介绍：rotate_recovery - ROS Wiki

[ERROR] [1731032109.633248194, 446.913000000]: Failed to get a plan.
[ WARN] [1731032114.653816024, 451.931000000]: Clearing both costmaps outside a square (2.00m) large centered on the robot.
[ERROR] [1731032114.736247928, 452.013000000]: Failed to get a plan.
[ WARN] [1731032119.757207178, 457.031000000]: Rotate recovery behavior started.
[ERROR] [1731032126.643172274, 463.913000000]: Failed to get a plan.
[ WARN] [1731032131.664204575, 468.931000000]: Clearing both costmaps outside a square (0.00m) large centered on the robot.
[ERROR] [1731032131.745834036, 469.013000000]: Failed to get a plan.
[ERROR] [1731032136.766966988, 474.031000000]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

        4.2 自定义恢复流程 

        自定义的恢复流程如下： 

        其中 reset_distance 的示意如下：

        特别注意：重置行为 clear_costmap_recovery::ClearCostmapRecovery 的 layer_names 参数的默认值为 obstacles

        但是在 costmap_2d 的代码中，默认的障碍物层是 obstacle_layer

        所以在全局代价地图的参数设置中，要把重置行为的 layer_names 修改成 obstacle_layer 。

        或者在 costmap_common_params.yaml 中 通过插件（plugins）设置代价地图参数，将障碍场的名字改为 obstacled。参考说明：Pre-Hydro Parameters 和 Configuring Layered Costmaps

Pre-Hydro Parameters
Hydro and later releases use plugins for all costmap_2d layers. 
If you don't provide a plugins parameter then the initialization code will assume that your configuration is pre-Hydro and will load a default set of plugins with default namespaces. 
Your parameters will be moved to the new namespaces automagically. 
The default namespaces are static_layer, obstacle_layer and inflation_layer. 
Some tutorials (and books) still refer to pre-Hydro parameters, so pay close attention. To be safe, be sure to provide a plugins parameter.

Hydro(2013) 版本前的参数配置
Hydro 及其之后的版本为所有 costmap_2d 层使用 plugins 。
如果你没有提供 plugins 参数，那么初始化代码将假设你的配置是 Hydro 版本之前的，并将加载一组具有默认命名空间的默认 plugins 。
您的参数将自动移动到新的命名空间。
默认名称空间是 static_layer、obstacle_layer 和 inflation_layer。
一些教程（和书籍）仍然提到 Hydro 之前的参数，因此请密切关注。为了安全起见，请务必提供插件参数。

        本博客自定义的恢复流程代码如下：

# 自定义恢复行为
recovery_behaviors:
  - name: 'conservative_reset'                          # 行为名称
    type: 'clear_costmap_recovery/ClearCostmapRecovery' # 行为类型
  - name: 'rotate_recovery'
    type: 'rotate_recovery/RotateRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

conservative_reset:                # 与上文行为名称保持一致
  reset_distance: 2.0              # 清除范围，单位 m
  layer_names: ["obstacles"]       # 要清除哪一层地图

aggressive_reset:
  reset_distance: 0.0
  layer_names: ["obstacles"]

 4.3 costmap_2d 中的 VoxelLayer 和 ObstacleLayer

         navigation/costmap_2d/src/costmap_2d_ros.cpp 源码

  ros::NodeHandle obstacles(nh, "obstacle_layer");
  if (nh.getParam("map_type", s) && s == "voxel")
  {
    map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
    map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
    super_map.setStruct(&map);
    plugins.push_back(super_map);

    // 使用 move_parameter 函数从全局参数服务器将相关参数移动到 obstacles 的命名空间下。
    move_parameter(nh, obstacles, "origin_z");
    move_parameter(nh, obstacles, "z_resolution");
    move_parameter(nh, obstacles, "z_voxels");
    move_parameter(nh, obstacles, "mark_threshold");
    move_parameter(nh, obstacles, "unknown_threshold");
    move_parameter(nh, obstacles, "publish_voxel_map");
  }
  else
  {
    map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
    map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
    super_map.setStruct(&map);
    plugins.push_back(super_map);
  }

        参考：what difference between voxel layer and obstacle layer ? 

        costmap_2d::VoxelLayer 和 costmap_2d::ObstacleLayer 都是 ROS 中用于导航的代价地图障碍物层，但它们各自有不同的应用场景和特点。

        4.3.1. VoxelLayer（体素层）

• 维度：三维。
• 数据结构：使用体素（Voxel）网格存储障碍物信息，即每个栅格单元内会有多个体素，用于记录不同高度的障碍物。
• 主要用途：适用于三维障碍物检测和导航环境中较复杂的场景，尤其适合处理高低不平、具有高度信息的障碍物。
• 参数：
  • origin_z：z 轴的起点位置。
  • z_resolution：z 轴的分辨率（体素大小）。
  • z_voxels：z 轴体素数量（栅格高度）。
  • mark_threshold：将体素标记为障碍物的阈值。
  • unknown_threshold：将体素标记为未知区域的阈值。
  • publish_voxel_map：是否发布体素地图信息。

        4.3.2 ObstacleLayer（障碍物层）

• 维度：二维。
• 数据结构：使用二维栅格网格存储障碍物信息，仅记录平面上的障碍物。
• 主要用途：适用于一般的室内、户外平面导航环境，不需要考虑高度差的场景。

 4.4 地图的分层结构

        4.4.1 静态地图

        4.4.2 障碍物地图 

        障碍物地图是由激光雷达扫描得到的动态障碍物，重置行为只清除障碍物地图，实际上就是清除激光雷达过去探测到的动态障碍物残影，而新检测到的障碍物消息，会立刻刷新进来。叠加上 map_server 发过来的静态地图，作为重新全局路径规划的地图。

        4.4.3 膨胀地图

        将 静态地图 和 障碍物地图 叠加后膨胀得到的地图。

        4.4.4 代价地图

        由以上 3 层合并得到的代价地图。 

5、局部规划器 local_planner

        全部的局部规划器：移动机器人常用ROS局部规划器简介（包含 Eband_local_planner）

        ROS 默认使用 Trajectory Planner 局部规划器，内部实现使用 DWA 算法，但是代码质量不高。相比于 Trajectory Planner 局部规划器 ，DWA Planner 局部规划器代码的可读性更好，运行效率更高。

        Eband Planner 和 TEB Planner 的实现思路相似，其中 TEB Planner 加入了时间因素的考虑，同时提供了代价地图的优化插件，运动平滑性和执行效率更高        

        5.1 DWA Planner 局部规划器

        DWA算法针对的机器人模型是差分机器人和全向机器人。

        DWA 官方介绍：dwa_local_planner - ROS Wiki

<!-- 设置 move_base 的局部路径规划器为 DWA 规划器 -->
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/dwa_local_planner_params.yaml" command="load" />

• 动态窗口算法(Dynamic Window Approaches, DWA) 是基于预测控制理论的一种次优方法，因其在未知环境下能够安全、有效的避开障碍物, 同时具有计算量小, 反应迅速、可操作性强等特点。
• DWA算法属于局部路径规划算法。
• DWA算法的核心思想：根据移动机器人当前的位置状态和速度状态在速度空间   中采样多种满足移动机器人硬件约束的速度，然后计算机器人在这些速度情况下一定时间内的运动轨迹， 并通过评价函数对该轨迹进行评价，最后选出评价最优的轨迹所对应的速度来作为移动机器人运动速度， 如此循环直至移动机器人到达目标点。
• 对于无人驾驶汽车而言，情况类似，将车辆的位置变化转化为线速度和角速度控制，避障问题转变成空间中的运动约束问题，这样可以通过运动约束条件选择局部最优的路径。

        DWA 包含速度采样、轨迹预测（推算）、轨迹评价。详细信息见 【路径规划】局部路径规划算法——DWA算法。

        ① 速度采样会综合考虑底盘加速度的限制，与障碍物保持有效的刹车距离，以及尽快运动到轨迹终点这 3 个因素。

        ② 轨迹评价有 3 个标准。分别是轨迹和全局路径的贴合程度；轨迹末端和导航目标点的距离；轨迹和障碍物之间的距离。综合考虑过程、目标和风险这三个因素挑选最符合标准的轨迹。

         在 rviz 中添加 Path 订阅话题 /move_base/DWAPlannerROS/local_plan；添加 PointCloud2 订阅话题 /move_base/DWAPlannerROS/trajectory_cloud。显示如下，图中白色的轨迹是 DWA 生成的备选轨迹，绿色的轨迹为最终挑选的轨迹。

        5.1.1 DWA 局部规划器的参数设置

DWAPlannerROS:
  # 速度参数，按照机器人的真实性能设置
  max_vel_x: 0.3      # 最大x方向速度
  min_vel_x: -0.05    # 最小x方向速度（设置负数将会允许倒车，适合激光雷达可以扫描到后方的机器人）
  max_vel_y: 0.0      # 差分驱动机器人的最大y方向速度为 0.0
  min_vel_y: 0.0      # 差分驱动机器人的最小y方向速度为 0.0
  max_vel_trans: 0.3  # 最大平移速度
  min_vel_trans: 0.01 # 最小平移速度（建议不要设置为 0.0 ）
  trans_stopped_vel: 0.1  # 当平移速度小于这个值，就让机器人停止
  acc_lim_trans: 2.5      # 最大平移加速度
  acc_lim_x: 2.5          # x方向的最大加速度上限
  acc_lim_y: 0.0          # y方向的加速度上限（差分驱动机器人应该设置为 0.0 ）
  
  max_vel_theta: 1.0      # 最大旋转速度，略小于基座的功能
  min_vel_theta: -0.01    # 当平移速度可以忽略时的最小角速度
  theta_stopped_vel: 0.1  # 当旋转速度小于这个值，就让机器人停止
  acc_lim_theta: 6.0      # 旋转的加速度上限

  # 目标容差参数，对机器人到达导航终点的判定条件
  yaw_goal_tolerance: 0.1         # 目标航向容差
  xy_goal_tolerance: 0.05         # 目标xy容差
  latch_xy_goal_tolerance: false  # 到达目标容差范围后，停止移动，只旋转调整航向

  # 向前模拟参数，决定了生成的轨迹的长度和数量
  sim_time: 1.7       # 模拟时间，默认值 1.7
  vx_samples: 3       # x方向速度采样数，默认值 3
  vy_samples: 1       # 差分驱动机器人y方向速度采样数，只有一个样本
  vtheta_samples: 20  # 旋转速度采样数，默认值 20

  # 轨迹评分参数，影响最终轨迹的挑选
  path_distance_bias: 32.0  # 贴合全局路径的权重，默认值 32.0
  goal_distance_bias: 24.0  # 接近导航目标点的权重，默认值 24.0
  occdist_scale: 0.01       # 控制器避障的权重，默认值 0.01
  forward_point_distance: 0.325 # 从机器人到评分点的位置，默认值 0.325
  stop_time_buffer: 0.2     # 在碰撞前机器人必须停止的时间长度，留出缓冲空间，默认值 0.2
  scaling_speed: 0.25       # 缩放机器人速度的绝对值，默认值 0.25
  max_scaling_factor: 0.2   # 机器人足迹在高速时能缩放的最大系数，默认值 0.2

  # 防振动参数
  oscillation_reset_dist: 1.05 # 重置振动标志前需要行进的距离，默认值 0.05

  # 辅助调试选项
  publish_traj_pc : true      # 是否在 RViz 里发布轨迹
  publish_cost_grid_pc: true  # 是否在 RViz 里发布代价网格
  global_frame_id: odom       # 基础坐标系

  # 差分驱动机器人配置
  holonomic_robot: false # 是否全向移动机器人

        5.1.2 对 DWA 参数进行动态调试

rosrun rqt_reconfigure rqt_reconfigure

        5.2  TEB Planner 局部规划器

        TEB 局部规划器通过以下参数区分 阿克曼转向机器人 和 差速驱动机器人。TEB 在调整机器人的朝向时，会倾向于使用走弧线倒车的方式，而不是直接原地旋转，所以 TEB 天生适合阿克曼底盘这种不能原地旋转的机器人。  

# ********************** 转弯半径相关 ********************
min_turning_radius: 1.7         # 最小转弯半径。差速驱动机器人设置为 0。！！！！！！
wheelbase: 0.68                 # 轴距。只有在 cmd_angle_instead_rotvel 为 true 时才有效
cmd_angle_instead_rotvel: False # 是否将收到的角速度消息转换为相应的转向角 [-pi/2,pi/2]。设置成 True 时，话题 vel_msg.angular.z 内的数据是转轴角度。
# ********************************************************************

# 用于执行最小转弯半径的优化权重（仅适用于阿克曼机器人）
weight_kinematics_turning_radius: 1.0

        TEB 官方介绍：teb_local_planner - ROS Wiki

<!-- 设置 move_base 的局部路径规划器为 TEB 规划器 -->
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/teb_local_planner_params.yaml" command="load" />

        时间弹力带（Timed Elastic Band, TEB），TEB 会在全局规划器发来的全局路径上选取一段路径进行优化，在选取的这一段路径的起点和终点之间拉一根弹力带。参考：TEB轨迹优化算法

• Elastic Band（弹力带）：连接起点和终点的路径，这个路径可以变形，变形的条件就是受到外力。将所有约束当做弹力带受到的外力，其中全局路径对弹力带产生吸引力，障碍物对弹力带产生排斥力。将这些外力叠加就得到了 TEB 生成的局部路径。

• TEB 会根据机器人的速度和加速度等运动信息，在弹力带上预测未来的连续几个时间单位的机器人的位姿，这里的每个位姿之间的时间间隔都是相等的，为 。

• 如果存在多条路线时，就会按照相同时间内机器人移动的距离远近来挑选距离最优的路线

• TEB 生成的局部轨迹由一系列带有时间信息的离散位姿(pose)组成，通过优化算法，使最终由这些离散位姿组成的轨迹能够达到时间最优、距离最优、远离障碍物等要求，同时限制速度与加速度使轨迹满足机器人的运动学约束。

        在 rviz 中添加 Path 订阅话题 /move_base/TebLocalPlannerROS/local_plan；添加 PoseArray 订阅话题 /move_base/TebLocalPlannerROS/teb_poses 。显示如下，图中绿色的轨迹是 TEB 规划的局部路径，红色的箭头为 TEB 预测的未来连续几个时间单位机器人将会到达的位姿。

        和 DWA 相比，TEB 规划器具有更强的脱困能力。TEB 在调整机器人的朝向时，会倾向于使用走弧线倒车的方式，而不是直接原地旋转，所以 TEB 天生适合阿克曼底盘这种不能原地旋转的机器人。  

         5.2.1 安装 TEB 局部规划器

sudo apt-get install ros-noetic-teb-local-planner

        5.2.2 TEB 局部规划器的参数设置

TebLocalPlannerROS:
 odom_topic: odom

 # 轨迹生成策略相关
 teb_autosize: True  # 是否允许改变轨迹的时域长度，也就是改变 dt_ref
 dt_ref: 0.5         # 路径上的两个相邻姿态的默认距离
 dt_hysteresis: 0.1  # 允许改变的时域解析度的浮动范围
 global_plan_overwrite_orientation: True # 是否修正全局路径中的临时局部路径点的朝向
 max_global_plan_lookahead_dist: 2.0     # 最大向前看距离
 feasibility_check_no_poses: 2           #在判断生成的轨迹是否冲突时使用，此时设置为2，即从轨迹起点开始逐个检查轨迹上的2个点，若2个点均不发生碰撞，则认为本次轨迹有效。
    
 # 运动性能相关     
 max_vel_x: 0.4           # 最大速度
 max_vel_x_backwards: 0.2 # 最大倒车速度，设置为0或者负数将导致错误。减少倒车应该修改倒车权重，不改这里。
 max_vel_theta: 1.0       # 最大转向角速度，跟 min_turning_radius 相关 (r = v / omega)
 acc_lim_x: 0.5           # 最大线加速度
 acc_lim_theta: 1.0       # 最大角加速度

 # ********************** 转弯半径相关 ********************
 min_turning_radius: 0.0         # 小转弯半径。如果设为 0，表示可以原地转弯。
 wheelbase: 0.31                 # 只有在 cmd_angle_instead_rotvel为true时才有效
 cmd_angle_instead_rotvel: False # 是否将收到的角速度消息转换为操作上的角度变化。设置成 True 时，话题 vel_msg.angular.z 内的数据是转轴角度。
 # ********************************************************************

# 车体轮廓描述
 footprint_model: # types可选项: "point", "circular", "two_circles", "line", "polygon"
   type: "circular"
   # 对 type "circular" 有效的参数：
   radius: 0.17
   # 对 type "line" 有效的参数：        
   line_start: [0.0, 0.0] 
   line_end: [0.35, 0.0]
   # 对 type "two_circles" 有效的参数：
   front_offset: 0.35
   front_radius: 0.35
   rear_offset: 0.35
   rear_radius: 0.35
   # 对 type "polygon" 有效的参数：
   vertices: [ [0.35, 0.0], [-0.2, -0.25], [0.2, -0.25]] 

 # 到达目标点的判断容差   
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
    
 # 障碍物避碰相关 
 min_obstacle_dist: 0.1  # 与障碍物的最小间距
 inflation_dist: 0.4     # 障碍物膨胀距离
 include_costmap_obstacles: True          # 是否检测动态障碍物
 costmap_obstacles_behind_robot_dist: 1.0 # 身后多远距离内障碍物加入检测范围
 obstacle_poses_affected: 25              # 障碍物对附近多少个关键点产生影响
 costmap_converter_plugin: ""             # costmap_converter 插件名称，这里不使用

 # 路径优化相关
 no_inner_iterations: 3     # 图优化optimizer的迭代次数
 no_outer_iterations: 3     # 外循环迭代次数
 penalty_epsilon: 0.1       # 为所有的惩罚项增加一个小的安全余量
 weight_max_vel_x: 2        # 平移速度的优化权重
 weight_max_vel_theta: 1    # 角速度的优化权重
 weight_acc_lim_x: 1        # 平移加速度的优化权重
 weight_acc_lim_theta: 1    # 角加速度的优化重量
 weight_kinematics_nh: 1000 # 非完整运动学的优化权重
 weight_kinematics_forward_drive: 1 # 往前移动的权重
 weight_optimaltime: 1      # 耗时权重
 weight_obstacle: 50        # 与障碍物保持距离的权重

 # 多线规划
 enable_homotopy_class_planning: True # 激活多线规划
 enable_multithreading: True          # 多线程计算
 max_number_classes: 2                # 规划的路径线数上限
 selection_cost_hysteresis: 1.0       # 路径轨迹入选的评价上限
 selection_obst_cost_scale: 1.0       # 障碍物评价在入选标准中的缩放倍率
 selection_alternative_time_cost: False # 时间成本是否要进行平方计算
 roadmap_graph_no_samples: 15         # 为创建 roadmap graph 而生成的样本数
 roadmap_graph_area_width: 5          # 关键点采样的宽度，单位为米。

        5.2.3 对 TEB 参数进行动态调试

rosrun rqt_reconfigure rqt_reconfigure

        5.2.4 costmap_converter 插件

        启用 costmap_converter_plugin 插件可以获得更好的规划性能和避障效果

四、导航的 Action 编程接口

        1 C++ 实现

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "demo_simple_goal");

  // tell the action client that we want to spin a thread by default
  // 创建 MoveBaseClient 的实例 ac，并连接到名为 "move_base" 的 action server。
  // true 表示 表示 spin a thread，即让 action client 使用自己的线程来管理内部事件循环。
  // 这意味着 client 将在后台管理与 server 的通信，不会阻塞主线程。这样主线程可以继续执行其他操作，而不需要等待 action server 就绪。
  MoveBaseClient ac("move_base", true);

  // wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0)))
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal goal;

  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();

  goal.target_pose.pose.position.x = -3.0;
  goal.target_pose.pose.position.y = 2.0;
  goal.target_pose.pose.orientation.w = 1.0;

  ROS_INFO("Sending goal");
  ac.sendGoal(goal);

  ac.waitForResult();

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Mission complete!");
  else
    ROS_INFO("Mission failed ...");

  return 0;
}

        2 Python实现 

#!/usr/bin/env python3
# coding=utf-8

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

if __name__ == "__main__":
    rospy.init_node("simple_goal")
    # 生成一个导航请求客户端
    ac = actionlib.SimpleActionClient('move_base',MoveBaseAction)
    # 等待服务器端启动
    ac.wait_for_server()

    # 构建目标航点消息
    goal = MoveBaseGoal()
    # 目标航点的参考坐标系
    goal.target_pose.header.frame_id="map"
    # 目标航点在参考坐标系里的三维数值
    goal.target_pose.pose.position.x = -3.0
    goal.target_pose.pose.position.y = 2.0
    goal.target_pose.pose.position.z = 0.0
    # 目标航点在参考坐标系里的朝向信息
    goal.target_pose.pose.orientation.x = 0.0
    goal.target_pose.pose.orientation.y = 0.0
    goal.target_pose.pose.orientation.z = 0.0
    goal.target_pose.pose.orientation.w = 1.0

    # 发送目标航点去执行
    ac.send_goal(goal)
    rospy.loginfo("开始导航……")
    ac.wait_for_result()

    if ac.get_state() == actionlib.GoalStatus.SUCCEEDED:
        rospy.loginfo("导航成功！")
    else:
        rospy.loginfo("导航失败……")

五、航点导航插件  

        ① 下载插件

git clone https://github.com/6-robot/waterplus_map_tools.git

        ② 运行插件 

roslaunch waterplus_map_tools add_waypoint_simulation.launch

        该 launch 的具体代码如下：

<launch>

  <!-- Map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
  
  <!-- RViz and TF  -->
  <arg name="rvizconfig" default="$(find waterplus_map_tools)/rviz/editwaypoints.rviz" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /map /base_link 100"/>

  <!-- Map tools  -->
  <node pkg="waterplus_map_tools" type="wp_edit_node" name="wp_edit_node" output="screen" />

</launch>

        ③ 点击 Add Waypoint，按住鼠标左键画圈 ，添加航点。（鼠标左键点击航点上的箭头可以调整航点）

        ④  保持航点信息至主文件夹，文件为 waypoints.xml 。

rosrun waterplus_map_tools wp_saver

        ⑤ 在 nav.launch 中添加如下代码，集成航点导航插件

<!-- 航点导航插件 -->
<node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen" />
<node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen" />

        在 rviz 中添加 Marker 订阅话题 /waypoints_marker。航点信息可视化如下：

        注意： wp_manager.cpp 中存储了 waypoints.xml 文件的路径，如下所示：

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wp_waypoint_manager");

    // waypoints.xml所在的文件夹
    std::string strLoadFile;
    char const* home = getenv("HOME");
    strLoadFile = home;
    strLoadFile += "/test/waypoints.xml";

    // 其他代码
}

        ⑥  Python实现

#!/usr/bin/env python3
# coding=utf-8

import rospy
from std_msgs.msg import String

# 导航结果回调函数
def resultNavi(msg):
    rospy.loginfo("导航结果 = %s",msg.data)

if __name__ == "__main__":
    rospy.init_node("demo_map_tools")
    # 发布航点名称话题
    navi_pub = rospy.Publisher("/waterplus/navi_waypoint",String,queue_size=10)
    # 订阅导航结果话题
    result_sub = rospy.Subscriber("/waterplus/navi_result",String,resultNavi,queue_size=10)
    # 延时1秒钟，让后台的话题发布操作能够完成
    rospy.sleep(1)
    # 构建航点名称消息包
    msg = String()
    msg.data = '1'
    # 发送航点名称消息包
    navi_pub.publish(msg)
    # 构建循环让程序别退出，等待导航结果
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        rate.sleep()

参考：

         7.2.4 导航实现04_路径规划

        ROS Melodic中costmap2D详解

        ros 导航中出现的costmap_ros costmap-CSDN博客

        机器人工匠阿杰的个人空间-机器人工匠阿杰个人主页-哔哩哔哩视频

        ROS局部运动规划器Teb/DWA