还在目测航点坐标口算四元数？试试这款开源的地图导航插件吧！

没有机器人，如何学习ROS

众所周知，在ROS的Navigation子系统中，对导航目标的描述是由XYZ三维坐标值和一个四元数描述的朝向角组成的。比如，在上一个实验中，我们为了让机器人从客厅门口导航到厨房的一个目标航点，总共要做下面这些工作：

这才走了一个航点就这么麻烦，若是多设几个航点在屋子里晃悠一圈，代码和工作量更是成倍增加。最关键的，所有航点都是数字组成，需要时刻脑补才能分清楚哪个坐标对应哪个航点。在服务机器人比赛中，还需要在导航任务的间隙再插入一些识别抓取这类常用操作，最终的程序结构，恐怕已经臃肿到一个人无法维护的程度。 

有没有什么方法能够简化这类航点导航操作呢？如果你也有这样的烦恼，下面介绍的这款开源的地图导航插件正是你需要的。它使用简单，操作直观，小鼠标一点，加上寥寥几行代码，让你轻轻松松完成导航任务。

这个插件的源码下载网址：

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

还是用上面的那个导航任务做例子，有了这款插件，最后只需要做这些工作：

是不是清爽了很多？然后再看看调用插件导航服务的代码：

#include <ros/ros.h> #include <std_msgs/String.h> void NavResultCallback(const std_msgs::String::ConstPtr &msg) {  ROS_WARN("[NavResultCallback] %s",msg->data.c_str());} int main(int argc, char** argv)     ros::init(argc, argv, "demo_map_tool");     ros::NodeHandle n;     ros::Publisher nav_pub = n.advertise<std_msgs::String>("/waterplus/navi_waypoint", 10);     ros::Subscriber res_sub = n.subscribe("/waterplus/navi_result", 10, NavResultCallback);     sleep(1);     std_msgs::String nav_msg;     nav_msg.data = "1";     nav_pub.publish(nav_msg);     ros::spin();     return 0; }

(1) 代码的开头include了两个头文件：ros.h是ROS的系统头文件；String.h是字符串类型头文件，程序中发送航点名称和输出反馈信息需要用到字符串； 

(2) 程序定义一个回调函数void NavResultCallback()，用来获取导航插件服务反馈回来的执行结果。

(3) 回调函数void NavResultCallback()的参数msg是一个std_msgs::String格式指针，其指向的内存区域就是存放导航任务结果的内存空间。其中data就是存放结果的字符串，这个字符串是string格式的，我们需要调用c_str()函数将其转换成char数组才能交给ROS_WARN进行显示。

(4) 在主函数main()中，按照惯例，先调用ros::init()对这个节点进行初始化。

(5) 接下来定义一个ros::NodeHandle节点句柄n，并使用这个句柄发布一个主题“/waterplus/navi_waypoint”，用于向导航插件发送目标航点的名称。我们的地图导航插件会从这个主题中获取航点名称，激活导航行为。

(6) 继续使用节点句柄n向ROS核心节点订阅主题“/waterplus/navi_result”，回调函数设置为前面定义的NavResultCallback ()。这个“/waterplus/navi_result”是导航插件发布导航任务执行结果的主题名，回调函数NavResultCallback ()会将这个主题里的信息通过ROS_WARN显示在终端程序里。

(7) 接下来调用sleep(1)延时一秒，等待前面的主题发布和订阅能够初始化完成。

(8) 延时结束后，定义一个std_msgs::String格式的消息包nav_msg。向这个消息包的成员变量data赋值目标航点的名称（这里是数字“1”，后面我们会在地图中设置这个航点）。

(9) 使用发布器对象nav_pub将消息包nav_msg发布到主题“/waterplus/navi_waypoint”里。导航插件会从这个主题获取我们发送的消息包，激活导航行为。

(10) 调用ros::spin()对main()函数进行阻塞，保持这个节点程序不会马上结束退出。剩下的就是静静的等待回调函数void NavResultCallback()获取导航任务的结果信息，整个程序到此结束。

这么短的代码能管用吗？我们找个机器人运行一下看看。对于购买了六部工坊机器人的上帝们，在附带的开发手册和实验指导书中就有这个插件的实验章节，可以直接在实物机器人上进行实验操作。而没有实物机器人的同学，咱们继续使用之前用到的虚拟机器人仿真包wpr_simulation，网址是：

https://github.com/6-robot/wpr_simulation

文中的demo_map_tool程序源码也在/wpr_simulation/src/里，文件名为demo_map_tool.cpp。

实验步骤：

1. 下载源码

如果在之前的实验下载过wpr_simulation，需要更新到最新版本。

在Ubuntu桌面打开一个终端程序，输入如下指令：

cd ~/catkin_ws/src git clone https://github.com/6-robot/waterplus_map_tools.git git clone https://github.com/6-robot/wpr_simulation.git git clone https://github.com/6-robot/wpb_home.git

其中catkin_ws是ROS的工作空间目录，请根据实际情况就行修改，接下来安装一些依赖项。

使用Kinetic/Ubuntu 16.04的同学，执行如下指令:

cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts ./install_for_kinetic.sh

使用Melodic/Ubuntu 18.04的同学，执行如下指令：

cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts ./install_for_melodic.sh

 

2. 编译源代码

在终端程序中，继续输入如下指令：

cd ~/catkin_ws catkin_make

同样的，其中catkin_ws是ROS的工作空间目录，请根据实际情况就行修改。

3. 使用GMapping建立环境地图

务必将最后生成的地图文件复制到wpr_simulation的maps文件夹里。

4. 在地图中设置航点

在终端程序中，继续输入如下指令：

roslaunch waterplus_map_tools add_waypoint_simulation.launch

回车执行会启动Rviz，界面上可以看到我们刚才建好的地图。（如果出现错误，务必检查一下地图文件是否已经按照上一节实验生成，并复制到wpr_simulation的maps文件夹里）

在Rviz工具栏的右边，有一个“Add Waypoint”按钮：

点击“Add Waypoint”按钮，就可以在地图上设置航点：先用鼠标左键点击要设置航点的位置，按住不放拖动鼠标可以选择航点的朝向。

确定好航点朝向后，松开鼠标左键，便完成一个航点的设置。

设置完的航点位置会显示一个三维箭头标记，标记上的数字“1”就是航点名称，这个名称后面会介绍如何修改。航点的位置和朝向还可以在Rviz里继续调整：

·用鼠标点击航点标记旁边的红色箭头并拖动，可以在前后方向上调整航点位置：

·用鼠标点击航点标记旁边的绿色箭头并拖动，可以在左右方向上调整航点位置：

·用鼠标点击航点标记周围的蓝色圆环并拖动，可以改变航点的朝向：

使用上述方法，在地图上设置更多的航点：

航点设置完毕后，需要将这些信息保存成文件。保持Rviz界面别关闭，启动一个新的终端程序，执行如下指令：

rosrun waterplus_map_tools wp_saver

执行完毕后，在Ubuntu系统的主文件夹下会生成一个名为“waypoints.xml”的文件，这个文件里保存的就是我们刚刚设置的航点信息。这时可以关闭Rviz程序，准备下一步实验。

5. 运行例程

首先，我们把机器人仿真环境和带插件的Rviz启动起来。在Ubuntu的终端程序里输入如下指令：

roslaunch wpr_simulation wpb_map_tool.launch

按下回车键后，就出现了熟悉的三维仿真界面：

同时启动的还有Rviz窗口：

在Rviz中，机器人的默认位置是地图的中心，对比一下仿真环境，机器人正确的位置应该在客厅入口处。所以需要给机器人重新设置一下初始位置：点击Rviz界面上方工具栏条里的“2D Pos Estimate”按钮，然后再点击Rviz地图里客厅的入口处，会出现一个绿色大箭头，代表机器人初始位置的朝向。按住鼠标左键不放，在屏幕上拖动画圈，可以控制绿色箭头的朝向。

在Rviz中拖动绿色箭头，指向房间入口，松开鼠标左键，机器人模型的位置就会定位到这个位置。

这时可以看到红色的激光雷达数据点和静态障碍物的轮廓大致贴合，说明初始位置设置正确了。接下来就要运行这一节实验的主角——拥有超短代码的demo_map_tool例程。

在终端程序输入以下指令：

rosrun wpr_simulation demo_map_tool

按下回车键后，可以看到一条紫红色的线条从机器人脚下延伸到航点“1”的位置，这就是导航插件规划出来的全局路径：

机器人会沿着这条路径自主移动，到达目标地点后停止。

此时在运行demo_map_tool节点终端里，可以看到回调函数NavResultCallback()显示的插件服务反馈的信息“done”，提示导航任务完成。看，是不是简洁又高效？

6. 航点信息的修改

前面提到了，航点信息保存在主文件夹的waypoints.xml文件里，我们可以使用文本编辑工具gedit来打开它。在终端程序里输入：

gedit waypoints.xml

回车执行，会弹出gedit的窗口界面：

可以看到文件里记录了航点的名称、三维坐标以及朝向角的四元数描述。我们只需要对航点名称“Name”进行修改即可：

修改后，按组合键Ctrl+S保存。然后再次打开Rviz地图插件：

roslaunch waterplus_map_tools add_waypoint_simulation.launch

在Rviz窗口里，就可以看到我们把航点名称从1、2、3、4这样的数字改成kitchen、living room等这类具有实际意义的单词。

这时候再把demo_map_tool节点里的航点名称改成修改后的房间名称，就可以跟继续进行导航任务了。使用房间名称作为航点标记，可以让我们的程序具备更好的可读性，后面再插入更多的识别抓取操作，也不会把自己绕晕了~

更多产品了解

欢迎扫码加入云巴巴企业数字化交流服务群

产品交流、问题咨询、专业测评

都在这里！