ROS-导航系统-move_base全局规划器-AMCL-代价地图_参数设置-恢复行为_参数设置-局部规划器-DWA-TEB-导航Action编程接口-坐标导航实现-导航插件-导航插件集成

最新推荐文章于 2025-03-18 22:15:05 发布

看星猩的柴狗

最新推荐文章于 2025-03-18 22:15:05 发布

阅读量832

点赞数 28

文章标签： ros

本文链接：https://blog.csdn.net/llovewuzhengzi/article/details/145595623

版权

文章目录

navigation
move_base
全局规划器
AMCL 定位算法
代价地图
代价地图参数设置
恢复行为
恢复行为参数设置
局部规划器
DWA
TEB
导航Action 编程接口
坐标导航实现
导航插件
导航插件集成

navigation

在这里插入图片描述

move_base

在这里插入图片描述
主要组件

全局规划器（Global Planner）：
负责生成从起点到目标点的全局路径。
常用实现包括 navfn 和 global_planner。
局部规划器（Local Planner）：
负责根据全局路径和局部环境信息（如障碍物）生成具体的运动指令。
常用实现包括 base_local_planner 和 dwa_local_planner。
代价地图（Costmap）：
用于表示机器人周围环境的代价信息，帮助规划器避开障碍物。
包括全局代价地图和局部代价地图。
恢复行为（Recovery Behaviors）
当机器人在导航过程中遇到问题（如被困住）时，move_base 会执行恢复行为。
默认的恢复行为包括：
清除代价地图中的障碍物。
原地旋转以重新规划路径。

下载一些配置文件

git clone https://github.com/6-robot/wpb_home.git
cd wpb_home/wpb_home_bringup/scripts/
./install_for_noetic.sh
cd ~/catkin_ws/
catkin_make

catkin_create_pkg nav_pkg roscpp rospy move_base_msgs actionlib

map文件先保持到wpr_simulation下的maps文件夹

roslaunch wpr_simulation wpb_stage_robocup.launch
roslaunch nav_pkg nav.launch

rviz

添加map和机器人
这个是选择导航的目的地和到达对应位置后的朝向
在这里插入图片描述
还可以添加path显示路线

全局规划器

变量名称对应使用哪种全局规划器

在这里插入图片描述
参数配置

关键参数：
~base_global_planner：指定全局规划器插件（默认为 navfn/NavfnROS）。
~base_local_planner：指定局部规划器插件（默认为 base_local_planner/TrajectoryPlannerROS）。
~recovery_behaviors：指定恢复行为列表。
~controller_frequency：控制循环的频率。
~planner_patience 和 ~controller_patience：规划和控制的超时时间。
其他参数：包括代价地图的配置、振荡检测等。

AMCL 定位算法

rviz里分裂各种粒子，即不同位置不同朝向的可能机器人位置
然后实际移动一段距离并雷达扫描得到里程计距离+雷达扫描特征
然后再通过将里程计距离叠加到各个粒子里，通过雷达特征匹配进行末尾淘汰
重复上述三步

功能：amcl 是一个用于机器人 2D 定位的概率系统，实现了自适应（或 KLD采样）蒙特卡洛定位方法，使用粒子滤波器跟踪机器人在已知地图中的位姿。
算法：使用多种算法，包括 sample_motion_model_odometry、beam_range_finder_model、likelihood_field_range_finder_model、Augmented_MCL 和 KLD_Sampling_MCL。
传感器支持：目前仅支持激光扫描和激光地图，但可以扩展到其他传感器数据。
节点：amcl 节点接收激光地图、激光扫描和变换消息，输出位姿估计。
订阅主题：
scan (sensor_msgs/LaserScan)：激光扫描数据。
tf (tf/tfMessage)：变换消息。
initialpose (geometry_msgs/PoseWithCovarianceStamped)：用于初始化粒子滤波器的均值和协方差。
map (nav_msgs/OccupancyGrid)：当 use_map_topic 参数设置时，订阅该主题以获取用于激光定位的地图。
发布主题：
amcl_pose (geometry_msgs/PoseWithCovarianceStamped)：机器人在地图中的估计位姿及协方差。
particlecloud (geometry_msgs/PoseArray)：滤波器维护的位姿估计集合。
tf (tf/tfMessage)：发布从 odom 到 map 的变换。

本体和分身切换就是map->odom
在这里插入图片描述
rviz里看到的是最匹配的，但每次分身都会变化到最匹配的那个分身，所以显示的跳跃。但里程计输出是平滑的

roslaunch wpr_simulation wpb_stage_robocup.launch

roslaunch nav_pkg nav.launch
rviz

在这里插入图片描述

代价地图

代价地图（Costmap）是机器人操作系统（ROS，Robot Operating System）中用于路径规划和导航的重要数据结构。它通过为地图上的每个单元格分配一个“代价”值来表示机器人在该位置移动的难易程度或风险程度。

roslaunch wpr_simulation wpb_stage_robocup.launch 
roslaunch nav_pkg nav.launch
rviz

在这里插入图片描述

<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/mymap.yaml"/>

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

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

代价地图的不同层次
Static Map Layer：静态地图层，基本上不变的地图层，通常都是SLAM建立完成的静态地图。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。
Master Layer：组合层，将上述各层的信息进行综合处理，生成最终的代价地图。

代价地图参数设置

功能：costmap_2d 包提供了一个 2D 代价地图的实现，用于机器人导航。它接收传感器数据，构建 2D 或 3D 占用网格（取决于是否使用体素实现），并根据占用网格和用户指定的膨胀半径在 2D 代价地图中膨胀代价。
标记和清除（Marking and Clearing）：
代价地图会自动订阅传感器主题，并根据传感器数据更新自身。
每个传感器用于标记（插入障碍物信息）或清除（移除障碍物信息）代价地图，或两者兼有。
占用、自由和未知空间（Occupied, Free, and Unknown Space）：
代价地图中的每个单元格可以有 255 种不同的代价值，但底层结构只能表示三种状态：自由、占用或未知。
每种状态在投影到代价地图时都有一个特殊的代价值。
地图更新（Map Updates）：
代价地图以指定的更新频率（update_frequency）执行地图更新周期。
在每个周期中，传感器数据被处理，标记和清除操作在底层占用结构中执行，然后投影到代价地图中。
变换（tf）：
代价地图使用 tf（变换）来插入传感器数据。假设全局坐标系（global_frame）、机器人基座坐标系（robot_base_frame）和传感器源之间的所有变换都是连接且最新的。
transform_tolerance 参数设置这些变换之间的最大延迟时间。
膨胀（Inflation）：
膨胀是将代价值从占用单元格向外传播的过程，代价值随着距离的增加而减小。
定义了 5 种特定的代价值符号，用于表示机器人与障碍物的关系。
静态地图（Static Map）：
使用用户生成的静态地图初始化代价地图（如通过 map_server 包构建的地图）。
代价地图的宽度、高度和障碍物信息与静态地图匹配。
滚动窗口（Rolling Window）：
设置 rolling_window 参数为 true，使机器人在移动时保持在代价地图的中心，随着机器人远离某个区域，障碍物信息从地图中移除。
这种配置通常用于机器人只关心局部区域的障碍物的情况。
坐标系和变换参数：
global_frame：代价地图的全局坐标系（默认为 /map）。
robot_base_frame：机器人基座坐标系（默认为 base_link）。
transform_tolerance：变换数据的延迟容忍时间（默认为 0.2 秒）。
频率参数：
update_frequency：地图更新频率（默认为 5.0 Hz）。
publish_frequency：地图发布频率（默认为 0.0 Hz，表示仅发布更新部分）。
地图管理参数：
rolling_window：是否使用滚动窗口（默认为 false）。
always_send_full_costmap：是否每次更新都发布完整代价地图（默认为 false）。
width、height、resolution、origin_x、origin_y：地图的宽度、高度、分辨率和原点坐标。

在这里插入图片描述

恢复行为

在这里插入图片描述

Conservative Reset（保守重置）：
描述：机器人尝试清除地图中超出指定区域的障碍物信息。
转换：
如果成功清除障碍物并继续导航，则进入 Navigating（正常导航）状态。
如果清除失败或仍然被困住，则进入 Clearing Rotation（旋转清除）状态。
Clearing Rotation（旋转清除）：
描述：机器人执行原地旋转，以尝试清除局部障碍物信息。
转换：
如果旋转后成功清除障碍物并继续导航，则进入 Navigating（正常导航）状态。
如果旋转后仍然被困住，则进入 Aggressive Reset（激进重置）状态。
Aggressive Reset（激进重置）：
描述：机器人更激进地清除地图中的障碍物信息，包括所有超出指定区域的障碍物。
转换：
如果成功清除障碍物并继续导航，则进入 Navigating（正常导航）状态。
如果清除失败或仍然被困住，则进入 Clearing Rotation（旋转清除）状态。
Navigating（正常导航）：
描述：机器人在正常导航状态，按照规划的路径移动。
转换：
如果遇到障碍物或被困住，则进入 Conservative Reset（保守重置）状态。
Aborted（放弃任务）：
描述：如果所有恢复行为都失败，机器人认为目标不可达，放弃导航任务。
转换：
无进一步转换，任务结束。

恢复行为参数设置

recovery_behaviors:
  - name: 'rotate_recovery'
    type: 'rotate_recovery/RotateRecovery'
  - name: 'reset_recovery'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

clear_costmap_recovery 包提供了一种恢复行为（Recovery Behavior），用于在机器人导航过程中清除代价地图（costmap）中的障碍物信息，以帮助机器人摆脱困境并继续导航。

该恢复行为通过将代价地图中超出指定区域的障碍物信息清除，使机器人能够重新规划路径。
清除的区域是一个以机器人为中心的正方形区域，区域的边长由 reset_distance 参数决定。

rotate_recovery:
  reset_distance: 2.0
  layer_names: ["obstacle_layer"]

~/reset_distance (double, default: 3.0)：
指定清除障碍物的区域边长（以机器人为中心的正方形区域），单位为米。
例如，reset_distance: 3.0 表示清除机器人周围 3 米范围内的障碍物信息。
~/invert_area_to_clear (bool, default: false)：
如果设置为 false，则清除指定区域外的障碍物信息；如果设置为 true，则清除指定区域内的障碍物信息。
~/layer_names (list, default: [“obstacles”])：
指定要清除的图层名称。默认情况下，只清除 obstacles 图层。

rotate_recovery 包提供了一种恢复行为（Recovery Behavior），用于在机器人导航过程中通过执行 360 度旋转来清除代价地图（costmap）中的障碍物信息，从而帮助机器人摆脱困境并继续导航。
执行 360 度旋转：
该恢复行为通过让机器人在原地旋转 360 度，更新传感器数据，从而清除代价地图中的障碍物信息。
旋转过程中，机器人会重新扫描周围环境，更新代价地图中的障碍物信息。

reset_recovery:
  reset_distance: 0.0
  layer_names: ["obstacle_layer"]

~/sim_granularity (double, default: 0.017)：
检查原地旋转是否安全时，每次检查的角度间隔（以弧度为单位）。默认值为 0.017 弧度（约 1 度）。
~/frequency (double, default: 20.0)：
发送速度指令给移动基座的频率（以 Hz 为单位）。默认值为 20.0 Hz。
~TrajectoryPlannerROS/yaw_goal_tolerance (double, default: 0.05)：
控制器在实现目标时，偏航角（yaw）的容差（以弧度为单位）。默认值为 0.05 弧度。
~TrajectoryPlannerROS/acc_lim_th (double, default: 3.2)：
机器人的旋转加速度限制（以弧度/秒² 为单位）。默认值为 3.2 弧度/秒²。
~TrajectoryPlannerROS/max_rotational_vel (double, default: 1.0)：
基座允许的最大旋转速度（以弧度/秒为单位）。默认值为 1.0 弧度/秒。
~TrajectoryPlannerROS/min_in_place_rotational_vel (double, default: 0.4)：
原地旋转时基座允许的最小旋转速度（以弧度/秒为单位）。默认值为 0.4 弧度/秒。

局部规划器

在这里插入图片描述
wpbbh_local_planner/WpbbhLocalPlanner就是根据激光雷达采集的信息，把代价地图和广度优先搜索算法在代局部规划期里又实现了一遍。采用了人工势场的方法，绕过障碍物又回到全局路线里

DWA

动态窗口法（Dynamic Window Approach, DWA）

状态空间采样：在当前时刻，基于机器人的速度和加速度限制，计算出可能的速度组合（线速度 v 和角速度 ω），形成动态速度窗口。
动态窗口可以表示为一个速度范围，其中包含机器人在当前时刻可以安全地采取的速度组合（线速度 v 和角速度 ω）
轨迹生成：对于每一对 (v,ω)，通过运动模型预测未来的轨迹，通常在一定时间跨度内进行预测。
评估轨迹：根据轨迹的质量评估函数（通常包括距离目标点的距离、与障碍物的安全距离等），给每条轨迹打分。
选择最佳速度：选择得分最高的速度命令，使得机器人在运动中既能达到目标，又能有效避开障碍物。

动态调参

rosrun rqt_reconfigure  rqt_reconfigure

参数选择

速度参数
max_vel_x: 最大 x 方向速度，单位为米/秒。机器人能够达到的最大线速度。
min_vel_x: 最小 x 方向速度，单位为米/秒。设置负数会允许倒车。
max_vel_y: 差分驱动机器人的最大 y 方向速度为 0.0。
min_vel_y: 差分驱动机器人的最小 y 方向速度为 0.0。
max_vel_trans: 最大平移速度，单位为米/秒。机器人能够达到的最大综合线速度。
min_vel_trans: 最小平移速度，单位为米/秒。建议不要设置为 0.0，避免机器人无法移动。
trans_stopped_vel: 当平移速度小于这个值时，机器人会停止。
acc_lim_trans: 最大平移加速度，单位为米/秒²。
acc_lim_x: x 方向的最大加速度上限，单位为米/秒²。
acc_lim_y: y 方向的加速度上限，差分驱动机器人应设置为 0.0。
max_vel_theta: 最大旋转速度，单位为弧度/秒。应略小于基座的功能极限。
min_vel_theta: 最小角速度，单位为弧度/秒。当平移速度可以忽略时的最小角速度。
theta_stopped_vel: 当旋转速度小于这个值时，机器人会停止。
acc_lim_theta: 旋转的加速度上限，单位为弧度/秒²。
目标容差参数
yaw_goal_tolerance: 目标航向容差，单位为弧度。机器人达到目标点时允许的偏航角误差。
xy_goal_tolerance: 目标 xy 容差，单位为米。机器人达到目标点时允许的 xy 位置误差。
latch_xy_goal_tolerance: 如果设置为 true，在机器人进入目标点的 xy 容差范围内后，即使之后机器人稍微偏离容差范围，也会继续认为目标已达成。
向前模拟参数
sim_time: 模拟时间，单位为秒。表示算法模拟机器人运动的时间跨度。
vx_samples: x 方向速度采样数。表示在 x 方向上采样多少个不同的速度。
vy_samples: 差分驱动机器人 y 方向速度采样数，只有一个样本。
vtheta_samples: 旋转速度采样数。表示在角速度方向上采样多少个不同的速度。
轨迹评分参数
path_distance_bias: 靠近全局路径的权重。较高的值会使机器人更倾向于保持在全局路径附近。
goal_distance_bias: 接近导航目标点的权重。较高的值会使机器人更倾向于尽快到达目标点。
occdist_scale: 控制器避障的权重。较高的值会使机器人更倾向于避开障碍物。
forward_point_distance: 从机器人到评分点的位置，单位为米。表示在机器人的前方多远的位置进行障碍物检测。
stop_time_buffer: 在碰撞前机器人必须停止的时间长度，留出缓冲空间，单位为秒。
scaling_speed: 缩放机器人速度的绝对值。当机器人速度超过这个值时，足迹会开始缩放。
max_scaling_factor: 机器人足迹在高速时能缩放的最大系数。
防振动参数
oscillation_reset_dist: 重置振动标志前需要行进的距离，单位为米。如果机器人在移动过程中，距离变化小于这个值，可能会被视为振动。
辅助调试选项
publish_traj_pc: 是否在 RViz 中发布轨迹。如果设置为 true，可以在 RViz 中看到机器人生成的轨迹。
publish_cost_grid_pc: 是否在 RViz 中发布代价网格。如果设置为 true，可以在 RViz 中看到代价网格。
global_frame_id: 基础坐标系。表示机器人在世界坐标系中的位置。
差分驱动机器人配置
holonomic_robot: 是否为全向移动机器人。如果设置为 true，表示机器人具有全向移动能力。

rviz显示可能轨迹
在这里插入图片描述

TEB

teb_local_planner 是 ROS 导航堆栈中的一个局部轨迹规划器插件，基于 Timed-Elastic-Bands 方法。它能够在运行时优化机器人的轨迹，同时考虑轨迹执行时间、与障碍物的距离以及运动学和动力学约束。

teb_local_planner 基于非线性优化，实时地为机器人生成平滑、安全且高效的局部轨迹。其核心方法是「基于时间的弹性带（Timed-Elastic-Bands）」，该方法通过引入虚拟力来拉伸或压缩轨迹，以适应环境约束和机器人动力学。

TEB算法首先从全局规划器提供的全局路线中选择一段作为初始轨迹。
根据这段轨迹的起点和终点，生成一条连线（时间弹性带）。这条连线由多个离散的时间点组成，每个时间点对应机器人的一个位姿（位置和姿态）
全局路线对TEB连线具有吸引力，即算法会尽量使连线靠近全局路线。这通过在优化过程中施加一个目标函数来实现，该目标函数使得连线上的点尽量接近全局路线上的点
障碍物对TEB连线具有排斥力，即算法会尽量使连线远离障碍物。这通过在优化过程中施加另一个目标函数来实现，该目标函数使得连线上的点尽量远离障碍物。

TEB算法通过多目标优化来调整连线的形状。优化的目标包括：
路径长度：尽量缩短路径长度。
轨迹运行时间：尽量减少轨迹的执行时间。
与障碍物的距离：保持与障碍物的安全距离。
通过中间路径点：尽量通过全局路线上的关键点。
机器人动力学和运动学约束：确保轨迹符合机器人的运动能力。

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

rviz显示选择路径和每个时间段的位置
在这里插入图片描述

速度相关参数
max_vel_x：机器人最大的平移速度，单位为米/秒。表示机器人在平移方向上能够达到的最快速度。
max_vel_x_backwards：机器人向后行驶时的最大绝对平移速度，单位为米/秒。如果设置为0或负数，会导致错误。可以通过减少倒车权重来调整倒车速度，而不是直接修改此参数。
max_vel_theta：机器人最大的旋转速度，单位为弧度/秒。与最小转弯半径相关，计算公式为r=v/ω ，其中 r 为转弯半径，v 为线速度，ω 为角速度。
加速度相关参数
acc_lim_x：机器人在x方向上的最大加速度，单位为米/秒²。限制了机器人在平移方向上加速或减速的速率。
acc_lim_theta：机器人最大的旋转加速度，单位为弧度/秒²。限制了机器人在旋转方向上加速或减速的速率。
转弯半径相关参数
min_turning_radius：最小转弯半径。如果设置为0，表示机器人可以原地转弯。
wheelbase：只有在 cmd_angle_instead_rotvel 为 True 时才有效。表示机器人后轴与前轴之间的距离。
cmd_angle_instead_rotvel：是否将收到的角速度消息转换为操作上的角度变化。如果设置为 True，则话题 vel_msg.angular.z 内的数据是转轴角度。
车体轮廓相关参数
footprint_model：定义机器人的轮廓模型，用于优化过程中的碰撞检测。支持的模型类型包括：“point”（点模型）、“circular”（圆形模型）、“two_circles”（双圆模型）、“line”（线模型）、“polygon”（多边形模型）。
type：指定轮廓模型的类型，例如 “circular”。
radius：仅对类型 “circular” 有效。表示圆形轮廓的半径。
line_start 和 line_end：仅对类型 “line” 有效。分别表示线段的起点和终点坐标。
front_offset、front_radius、rear_offset、rear_radius：仅对类型 “two_circles” 有效。分别表示前圆和后圆的偏移量和半径。
vertices：仅对类型 “polygon” 有效。表示多边形的顶点坐标。
到达目标点的判断容差
xy_goal_tolerance：机器人到达目标位置时允许的欧几里得距离偏差，单位为米。表示在二维平面上，机器人与目标位置之间的最大允许距离。
yaw_goal_tolerance：机器人到达目标方向时允许的偏差，单位为弧度。表示机器人在旋转方向上的最大允许偏差。
障碍物相关参数
min_obstacle_dist：机器人与障碍物之间的最小允许距离，单位为米。机器人会尽量保持在这个距离之外，以避免与障碍物发生碰撞。
inflation_dist：障碍物的膨胀距离，单位为米。在计算路径时，机器人会将障碍物的范围扩大这个距离，以确保更安全的路径规划。
include_costmap_obstacles：是否考虑局部成本图中的障碍物。如果设置为 True，则会将成本图中的障碍物纳入路径规划的考虑范围。
costmap_obstacles_behind_robot_dist：机器人身后多远距离内的障碍物会被纳入检测范围，单位为米。如果设置为较远的距离，机器人可以更早地感知到身后的障碍物，从而做出相应的避障动作。
obstacle_poses_affected：障碍物对附近多少个关键点产生影响。较大的数值会导致更严格的避障行为，但也会增加计算量。
路径优化相关参数
no_inner_iterations：图优化算法中的内部迭代次数。较大的数值可以提高轨迹的优化效果，但也会增加计算时间。
no_outer_iterations：外部循环的迭代次数。与内部迭代次数相结合，可以进一步提高轨迹的优化效果。
penalty_epsilon：为所有的惩罚项增加一个小的安全余量，以避免数值不稳定问题。
weight_max_vel_x、weight_max_vel_theta、weight_acc_lim_x、weight_acc_lim_theta：分别为平移速度、旋转速度、平移加速度和旋转加速度的优化权重。这些权重决定了在轨迹优化过程中，各个因素的相对重要性。
weight_kinematics_nh：非完整运动学的优化权重。较高的权重可以更严格地满足机器人的运动学约束。
weight_kinematics_forward_drive：往前移动的权重。较高的权重可以促使机器人优先选择向前行驶的路径。
weight_optimaltime：耗时权重。较高的权重会促使机器人选择更快速的路径。
weight_obstacle：与障碍物保持距离的权重。较高的权重可以更严格地避免与障碍物发生碰撞。
多线规划相关参数
enable_homotopy_class_planning：是否激活多线规划。如果设置为 True，机器人会同时规划多条不同拓扑结构的路径，并选择最优的一条。
enable_multithreading：是否启用多线程计算。如果设置为 True，可以提高规划效率。
max_number_classes：最多考虑的路径线数上限。限制了同时规划的路径数量，避免计算量过大。
selection_cost_hysteresis：路径轨迹入选的评价上限。只有当候选路径的评价低于当前选定路径的评价乘以这个倍数时，才会被选中，从而避免频繁切换路径。
selection_obst_cost_scale：障碍物评价在入选标准中的缩放倍率。较高的倍率会更严格地考虑障碍物的影响。
selection_alternative_time_cost：时间成本是否进行平方计算。如果设置为 True，时间成本将按平方计算，从而更重视时间因素。
roadmap_graph_no_samples：为创建 roadmap graph 而生成的样本数。较多的样本可以提高路径规划的准确性，但也会增加计算时间。
roadmap_graph_area_width：关键点采样的宽度，单位为米。较大的宽度会增加路径规划的灵活性，但也会增加计算时间。

导航Action 编程接口

Action 通信是一种双向的消息传递机制，与传统的Topic通信不同，Action可以提供连续的反馈信息。
导航Action通信包括三个主要部分：

目标请求（Goal Request）：客户端向服务器发送的目标请求，包含导航目标点的坐标和姿态。
结果（Result）：服务器在完成导航任务后返回的结果，表示导航是否成功。
反馈（Feedback）：服务器在导航过程中定期向客户端发送的反馈信息，用于实时监控导航进程。

坐标导航实现

http://docs.ros.org/en/fuerte/api/move_base_msgs/html/msg/MoveBaseAction.html
在这里插入图片描述


#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, "nav_client");
    MoveBaseClient ac("move_base", true);

//   它的作用是等待动作服务器启动，并返回一个布尔值。
// 如果服务器在指定的时间内（由 ros::Duration 设置）启动，函数返回 true。
// 如果超时未启动，函数返回 false。
    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 = 1.0;

    goal.target_pose.pose.orientation.w = 1;

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);
    ac.waitForResult();
    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        ROS_INFO("complete mission");
    else
        ROS_INFO("failed mission");

}

创建动作客户端
等待动作服务器启动
定义目标点位置和姿态
发送目标点到服务器
等待返回结果
检查结果状态

roslaunch wpr_simulation wpb_stage_robocup.launch
roslaunch nav_pkg nav.launch 
rosrun nav_pkg nav_client

#! /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('nav_client')
    ac = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    ac.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = -3.0
    goal.target_pose.pose.position.y = 1.0
    goal.target_pose.pose.orientation.w = 1.0

    
    ac.send_goal(goal)
    rospy.loginfo("send goal")
    ac.wait_for_result()
    
    if ac.get_state()==actionlib.GoalStatus.SUCCEEDED:
        rospy.loginfo("goal reached")
    else:
        rospy.loginfo("goal failed")

导航插件

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

设置导航目标点

roslaunch waterplus_map_tools add_waypoint_simulation.launch

在这里插入图片描述
直接在地图上画出导航点

保存导航点

rosrun waterplus_map_tools  wp_saver

启动环境

roslaunch wpr_simulation  wpb_map_tool.launch

开始导航到设置的目标位置（会导航到设置的第一个目标点）

rosrun wpr_simulation  demo_map_tool

如果要修改导航点再重新导航，重复上述步骤，其中在打开导航地图里移动导航点就行

导航插件集成

roslaunch wpr_simulation wpb_stage_robocup.launch 
roslaunch nav_pkg nav.launch 
rosrun wpr_simulation demo_map_tool

wp_manager：
功能：wp_manager 节点负责管理航点（waypoints），它从 waypoints.xml 文件中读取航点数据，然后发给wp_navi_server，wp_navi_server存储好航点信息，但没有开始导航，要等目标导航话题出现航点信息然后和wp_manager 发送给wp_navi_server后wp_navi_server存储好的航点信息比对来进行导航
wp_navi_server：
功能：wp_navi_server 节点负责处理导航任务，它订阅“目标航点话题”，接收目标航点信息，然后给move_base来导航，并通过“导航结果话题”发布导航结果。

在这里插入图片描述

<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="teb_local_planner/TebLocalPlannerROS" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/teb_local_planner_params.yaml" command="load" />
        
    </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"/>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_pkg)/rviz/map_tool.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 
roslaunch nav_pkg nav.launch 
rosrun nav_pkg  wp_node