订阅rviz2的导航目标位置消息“/goal_pose”

系列文章目录

思岚激光雷达rplidar从ROS 1到ROS 2的移植
ROS 2下navigation 2 stack的构建
为Navigation 2创建自定义behavior tree plugin
打断behavior tree的异步动作节点，并执行其他节点动作

---

---

背景

ROS 2 Navigation 2相比ROS 1对处理与机器人当前目标点完全相反的导航目标点时有很大改进，通过适当调高dwb中max_vel_theta，能够使机器人获得相反目标点时，以很小的转弯半径掉头，但速度略快，安全性上有风险，而且在一些特殊场景下，会出现掉头碰撞。苇航智能的机器人NaviBOT A0延续ROS 1时版本，保留反向目标场景时的原地旋转功能，以调整面向目标点的航向角。因此，需要基于导航目标点以及所规划的全局路径计算与当前坐标的航向角偏差，实现方式是通过订阅导航目标点消息来获取用户设置的导航目标点，以此判断导航的开始

然而，在开发过程中发现rviz2的“Navigation2 Goal”目标点设置后，机器人驱动代码没有成功接收消息"/goal_pose"

---

一. ROS 2下导航目标消息的变化

ROS 2重构了ROS 1中的move_base，将其拆分为“nav2_simple_navigator”，“nav2_dijkstra_planner”，“nav2_controller_dwb”几个模块，再加上Behavior Tree的引入，使得ROS 2下navigation stack能够非常灵活的通过插件来配置其行为和功能，如下图：

具体的变化可以参考官网链接：
https://navigation.ros.org/about/ros1_comparison.html#ros1-comparison

move_base替代后，ROS 1中的导航目标消息“move_base/goal move_base_msgs::MoveBaseActionGoal”变为了“/goal_pose geometry_msgs::msg::PoseStamped”，订阅导航目标点消息需要做响应的替换

当导航stack运行后，可以在终端命令发布/goal_pose geometry_msgs/PoseStamped消息验证导航目标消息，在终端输入命令：

ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}"

消息发布后，可以看到机器人接收的该导航目标，并开始导航运行

NOTE：终端发送导航定位消息的内容需要遵循yaml格式

二. 未能成功接收"/goal_pose"的原因

原因是navigation 2 rviz的configuration中SetGoal的tool默认使用了navigation 2的插件“nav2_rviz_plugins/GoalTool”，如下图。而该插件是使用action client的方式发送导航目标点

通过查看该插件的代码可以发现：该插件直接和rviz交互取得导航定位点，然后通过action server “nav2_msgs::action::NavigateToPose”的client向“nav2_bt_navigator”的action server发送导航目标点的request，如下图：

由于nav2_bt_navigator自身就是action server “nav2_msgs::action::NavigateToPose”，所以能够通过action server的get_current_goal()函数获取当前的导航目标位置，如下图：

所以，基于上述原因，rviz的“Navigation2 Goal”不会发送“/goal_pose geometry_msgs/PoseStamped”消息

三. 回归rviz自身的默认插件

通过查看rviz2代码，可以看到其自身的插件“SetGoal”接收导航目标点后，确实是通过发布“/goal_pose geometry_msgs/PoseStamped”来触发导航，如下图：

因此，rviz中的导航目标点设置后，是可以通过发布消息来触发导航过程的，实现方式是用rviz2中自身的插件“rviz_default_plugins/SetGoal”，替换掉navigation 2中默认的插件“nav2_rviz_plugins/GoalTool”

替换过程可以通过rviz2的图形界面可以实现，点击tools的“+”，在弹出的对话框中选择添加 “SetGaol”，如下图：

添加“SetGoal”后，将在tools栏显示“2D Goal Pose”，与ROS 1中一致，如下图：

四. 验证

通过运行导航， 用命令echo /goal_pose来验证是否成功发布了导航目标点消息

打开终端，输入导航的运行命令：

cd ~/dev_ws
sudo su
. install/setup.bash
ros2 launch motion_ctrl_nav navigation_local_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml

NOTE：上述命令中以苇航智能机器人NaviBOT A0的导航运行命令为例，请根据实际情况做适应修改

rviz运行并显示地图后，通过“2D Pose Estimate”给定机器人的初始位置，如下图：

打开一个新终端，输入命令：

cd ~/dev_ws
. install/setup.bash
ros2 topic echo /goal_pose

等待消息/goal_pose的发布，如下图：

在rviz中，通过新添加的“2D Goal Pose”设置导航目标点，此时，NaviBOT A0将启动导航运动，并且在echo的终端中出现了导航目标点的消息，如下图：


至此，导航目标点消息可以通过消息/goal_pose发出，订阅了该消息的其他node也将能成功收到该消息，以处理自身基于该消息的后续逻辑

---

总结

本文是项目从ROS 1做ROS 2下移植过程中碰到问题后的一个记录，也算是对前面两篇移植相关内容的一个补充，希望能对大家有帮助

如果文中有错误和不明确的地方，欢迎指正，共同交流，[email protected]