【ROS】航点导航功能
前言
在实际的 ROS 导航应用中，我们不可能始终依赖 RViz 手动设置导航目标点。RViz 设置目标点仅适用于测试或调试阶段。在复杂任务或自动化流程中，导航目标点的发布应由专门的程序节点负责。
因此，本节将实现一个自定义的 Action 接口，用于发布导航目标点，并介绍一个实用的插件工具，帮助我们更高效地开发航点导航功能。
我的环境：
本教程使用的环境是：实体 ROS 小车，Ubuntu 18.04，ROS1 Melodic
参考资料：
航点导航中的 Action 编程接口
推荐阅读以下视频以更深入了解：
在智能车导航的测试阶段，我们常用 RViz 手动发送导航点 来进行路径测试。但在实际应用中，不可能每次都手动发点来控制机器人。为了实现自主导航，就需要使用 ROS 官方推荐的 Action 编程接口 来通过代码控制导航过程。
什么是 Action 通信？
Action 是 ROS 中的一种通信机制，和话题（Topic）不同：
- 话题通信：是单向通信，一个节点发布，另一个节点订阅，不能实现实时反馈。
- Action 通信：是双向通信，包含**客户端（Client）和服务端（Server）**两部分。
在 ROS 导航系统中的应用
以 ROS 导航系统为例：
move_base是 Action 的服务端（Server），负责实际的导航控制；- 我们自己写的程序是客户端（Client），向
move_base发送导航目标点（包括位置和朝向）；
Action 通信的优势在于：
- 支持反馈：客户端可以实时接收到服务端的反馈，比如“导航进度 10%、50%、到达目标”等；
- 支持取消任务：如果中途想停止导航，可以发送取消请求；
- 支持超时和失败返回：如果导航失败，客户端也能及时收到失败的消息。
Action 通信与 Service 通信的区别
虽然 Action 和 Service 都属于“请求-响应”机制，都包含客户端和服务端，但两者的适用场景和功能不同：
| 特性 | Service（服务） | Action（动作） |
|---|---|---|
| 适用场景 | 短时间、快速完成的任务 | 长时间执行、需要过程反馈的任务 |
| 是否支持反馈 | 否 | 是 |
| 是否支持取消任务 | 否 | 是 |
| 通信模式 | 一次请求、一次响应，完成后即断开 | 请求后可持续反馈，直到完成或被取消 |
类比理解：
- Service 就像你去便利店买东西，提出请求后立即完成，一次性交互，不能中途更改。
- Action 更像是点外卖，下单后骑手配送过程中你可以查看实时进度，也可以取消订单，并在
最后收到完成或失败的结果。
航点导航的 Action 节点的实现
该部分内容建议参考以下视频：
- ROS 坐标导航的 Python 编程实现
action借点的实现分为三部，
1.编写借点文件
2.给借点文件添加可执行权限
3.运行借点文件
导航目标点发布节点
该部分内容建议参考以下视频：
编写节点文件
我们先在功能包的 scripts 文件夹下新建一个用于发送航点导航目标的客户端节点，文件名为 nav_client.py，内容如下：
#!/usr/bin/env python3
# coding=utf-8
import rospy
import actionlib
# 导入 move_base action 需要的消息类型
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
if __name__ == "__main__":
# 初始化 ROS 节点，节点名为 "nav_client"
rospy.init_node("nav_client")
# 创建一个 SimpleActionClient，用于与名为 'move_base' 的 action 服务器通信
# Action 类型是 MoveBaseAction
ac = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# 等待 action 服务器启动并准备好
rospy.loginfo("等待 move_base action 服务器...")
ac.wait_for_server()
rospy.loginfo("move_base action 服务器已连接！")
# 创建一个 MoveBaseGoal 消息，用于设置导航目标
goal = MoveBaseGoal()
# 设置目标姿态 (pose) 的 header 信息
# 设置 frame_id 为 "map"，表示目标坐标是基于 map 坐标系的
goal.target_pose.header.frame_id = "map"
# 设置 timestamp 为当前 ROS 时间，这是一个好的实践，确保消息的新鲜度
goal.target_pose.header.stamp = rospy.Time.now()
# 设置目标位置 (position)
goal.target_pose.pose.position.x = -3.0
goal.target_pose.pose.position.y = 2.0
goal.target_pose.pose.position.z = 0.0 # 通常在2D导航中 z 设为 0
# 设置目标方向 (orientation)，使用四元数表示
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
# 设置 w 为 1.0，当 x, y, z 都为 0 时，(0,0,0,1) 代表没有旋转，即朝向 map 坐标系的 +X 方向
goal.target_pose.pose.orientation.w = 1.0
rospy.loginfo("开始导航，发送目标到 (-3.0, 2.0) ...")
# 将目标发送给 action 服务器
ac.send_goal(goal)
# 等待 action 服务器执行完目标，直到返回结果
rospy.loginfo("等待导航结果...")
ac.wait_for_result()
# 获取最终的导航状态
# GoalStatus.SUCCEEDED 表示导航成功到达目标
if ac.get_state() == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("导航成功！机器人已到达目标点。")
else:
# 如果状态不是 SUCCEEDED，则认为导航失败
# 可能的原因包括：目标不可达、机器人被困、操作被取消/抢占等
rospy.loginfo("导航失败。机器人未能到达目标点。")
测试节点
1. 添加可执行权限
进入脚本所在目录，使用如下命令赋予执行权限：
chmod +x nav_client.py
执行 ls 时如果看到 nav_client.py 显示为绿色，则说明权限设置成功。
2. 测试节点功能
步骤一：启动导航功能
roslaunch nav_pkg nav.launch
步骤二：运行自定义发布节点
rosrun nav_pkg nav_client.py
观察终端输出及 RViz 中的导航过程，即可验证功能是否正常。
航点导航插件：waterplus_map_tools
该部分建议参考以下视频讲解：
在上一节中，我们已经实现了基础的单点导航功能。
但存在一个问题：导航目标点需要手动通过坐标设置，无法直接在地图上选点，操作繁琐且不直观。
为了解决这个问题，我们可以使用一款开源工具插件：waterplus_map_tools。
该工具可以直接在 RViz 中可视化选点，并实现航点的保存与自动巡航等功能，非常适合用于测试或比赛中的航点导航。
GitHub 项目地址：
- Github - waterplus_map_tools
详细使用说明见其项目下的README文档。
安装方法
-
在工作空间的
src目录下克隆项目：git clone https://github.com/6-robot/waterplus_map_tools.git -
根据你的 ROS 版本安装对应的依赖：
~/lby_ws/src/waterplus_map_tools/scripts/install_for_melodic.sh -
回到工作空间根目录进行编译：
cd ~/lby_ws catkin_make
航点设置、保存与测试
1. 设置航点
首先启动航点设置工具：
roslaunch waterplus_map_tools add_waypoint.launch
注意：maps 文件夹下需要提前放置你已经建好的地图文件。
启动后，在 RViz 中点击上方工具栏中的 Add Waypoint（添加航点） 按钮：
然后在地图上点击你想要添加航点的位置，按照顺序添加多个航点。
2. 保存航点
设置好航点后，使用以下命令将航点信息保存到本地：
rosrun waterplus_map_tools wp_saver
这样就完成了航点保存。如果你仅仅想通过该工具来“取点”，而不使用它自带的自动导航功能，到此为止即可。
3. 航点自动导航测试
使用以下命令测试机器人自动遍历所有航点：
rosrun waterplus_map_tools wp_nav_test
机器人将按照你设置的顺序依次前往各个航点，实现自动巡航。
集成航点导航插件到导航 launch 文件中
该部分建议参考以下视频讲解：
waterplus_map_tools 功能包中提供了两个主要节点：wp_navi_server 和 wp_manager。
wp_manager节点：用于读取waypoints.xml文件中保存的航点信息（包括每个航点的坐标及朝向，即姿态信息）；wp_navi_server节点：订阅一个“目标航点”的话题，并将目标姿态信息发送给move_base进行导航，同时发布导航结果话题。
我们可以自己编写一个节点，将目标航点编号发布到“目标航点”话题，wp_navi_server 订阅该话题，并根据其编号从 wp_manager 中读取相应的航点姿态，然后给 move_base发送导航目标点。
此外，该功能包中还包含一个测试节点 demo_map_tool，可用于快速测试航点导航功能。
修改 launch 文件，集成航点导航插件
我们在原有导航的 launch 文件中，添加以下两行启动节点：
<!-- 启动 wp_navi_server 节点 -->
<node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen" />
<!-- 启动 wp_manager 节点 -->
<node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen" />
同时，将 rviz 的配置文件路径修改为 map_tools.rviz，这样便于在 RViz 中使用航点标记功能。
完整示例：
<launch>
<node pkg="move_base" type="move_base" name="move_base">
<rosparam file="$(find wpb_home_tutorials)/nav_lider/costmap_common_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find wpb_home_tutorials)/nav_lider/costmap_common_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find wpb_home_tutorials)/nav_lider/global_costmap_params.yaml" command="load"/>
<rosparam file="$(find wpb_home_tutorials)/nav_lider/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" />
<param name="controller_frequency" value="10" type="double"/>
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/m" />
<node pkg="amcl" type="amcl" name="amcl"/>
<!-- 启动 RViz，加载专用于 waterplus_map_tools 的配置 -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_pkg)/rviz/map_tools.rviz"/>
<!-- 集成 waterplus_map_tools 的两个核心节点 -->
<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>
测试流程
- 启动导航 launch 文件：
roslaunch nav_pkg nav.launch
- 启动测试节点发送航点：
rosrun wpr_simulation demo_map_tool
测试过程中，demo_map_tool 会向目标话题发布航点编号，wp_navi_server 会调用 move_base 导航到目标点。
自定义 wp_navi_server 的航点发布接口节点
该部分建议参考以下视频讲解：
本节目标是实现一个自定义节点，向 wp_navi_server 所监听的航点话题发布目标航点编号，同时订阅导航结果，从而实现完整的航点导航控制。你可以将其看作是 demo_map_tool 的简化版或替代版本，后续也可以在此基础上扩展功能。
如下图所示，我们即将实现红框标注的节点： 它既要发布目标航点编号，又要订阅导航结果。
创建节点文件
在你的导航功能包的 scripts 文件夹下，创建一个新文件：
wp_node.py，并写入以下代码：
#!/usr/bin/env python3
# coding=utf-8
import rospy
from std_msgs.msg import String
# 定义导航结果的回调函数
def NavResultCallback(msg):
rospy.logwarn("导航结果 = %s", msg.data)
if __name__ == "__main__":
# 初始化 ROS 节点
rospy.init_node("wp_node")
# 创建发布者，发布航点编号到指定话题
navi_pub = rospy.Publisher("/waterplus/navi_waypoint", String, queue_size=10)
# 创建订阅者，接收导航结果
res_sub = rospy.Subscriber("/waterplus/navi_result", String, NavResultCallback, queue_size=10)
# 等待连接建立
rospy.sleep(1)
# 创建并发布航点编号 '1'
navi_msg = String()
navi_msg.data = '1'
rospy.loginfo("发布导航目标：航点 %s", navi_msg.data)
navi_pub.publish(navi_msg)
# 进入回调监听循环
rospy.spin()
测试节点
1. 添加可执行权限
进入脚本所在目录，使用如下命令赋予执行权限：
chmod +x wp_node.py
执行 ls 时如果看到 wp_node.py 显示为绿色，则说明权限设置成功。
2. 测试节点功能
步骤一：启动导航功能（包括 wp_navi_server 和 wp_manager）
roslaunch nav_pkg nav.launch
步骤二：运行自定义发布节点
rosrun nav_pkg wp_node.py
观察终端输出及 RViz 中的导航过程，即可验证功能是否正常。
扫一扫
1517
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
