源码解析 | SLAM 多点导航开发思路介绍

不久前，我们发布了一个多点SLAM的功能包，今天给大家介绍这个功能包的开发思路。

开发背景：

在使用 ROS Navigation & RViz 进行 2D Nav Goal 导航的时候，我们会遇到这些情况：

1. 给定导航的目标点只能设置一个，当有多点任务时需要等待一个个任务结束后，再次手动给目标
2. 无法暂停或取消任务
3. 任务不可循环

开发目的：

完成多目标点导航，可以对导航环节进行操控，如可循环、取消、重置任务等。

开发思路：

1. 2D Nav Goal 的单点导航是如何实现的？

我们可以知道导航目标是通过 RViz 工具栏中 2D Nav Goal发布出去的。

通过查看 RViz的配置文件或者 Panels->Add New Panel-> Tool Property ，可以了解当使用2D Nav Goal 在地图上拉了一个箭头（给定目标点时），其实是向话题 /move_base_simple/goal 发布了 geometry_msgs/PoseStamped 类型的消息，这个是目标点的位姿，包含坐标点与朝向。

根据 NodeGraph 可以看到话题 /move_base_simple/goal 被导航包的节点 /move_base 订阅了，进而发给 Navigation 中的各个话题，完成导航。

2. 如何基于单点实现多点导航？

要在单点基础上实现多目标点导航的话，就要设计一个关于多个导航目标点消息geometry_msgs/PoseStamped的数据结构，并对多个目标点进行处理，完成导航。

实现多点的方法有多种，在不打破 ROS Navigation 包的完整性的前提下，我选择在2D Nav Goal的 RViz节点和 /move_base 节点中间添加了一个话题 /move_base_simple/goal_temp，将原本发送给 /move_base_simple/goal 的消息，转发给/move_base_simple/goal_temp，通过此话题来积攒多个 2D Nav Goal（任务队列），并根据任务完成的状态反馈，按顺序将每个导航目标点消息 geometry_msgs/PoseStamped 再发送给话题/move_base_simple/goal，以完成多任务中的单次目标点的导航（如下图示）。

3. 如何来发布多点任务？

像 2D Nav Goal 一样，我们也可以在 RViz 中开发可视化的操作栏，这要使用到 RViz plugin ， ROS中的可视化工具绝大部分都是基于Qt进行开发的，此前古月居有过详细介绍，可参考这篇文章。

最终效果

首先，我们来看一下最终的实现效果。

MultiNaviGoalsPanel是多点SLAM导航任务的可视化操作区，包括任务点列表、循环、重置、取消、开始任务。

通过 RViz plugin 设计的Mark Display，能够显示的目标点的标号及箭头（朝向）。

源代码

项目地址：https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin

代码实现

1. 头文件 multi_navi_goal_panel.h

Qt说明：

• 文字编辑——QLineEdit
• 按键——QPushButton
• 列表——QTableWidget
• 复选框——QCheckBox
• 文字显示——QString

ROS说明:

Publisher：

• 发送每个目标点消息给/move_base_simple/goal的goal_pub_
• 发送取消指令消息给/move_base/cancel的cancel_pub_
• 发送文字和箭头标记的mark_pub_。

Subsrciber：

• 订阅来自rviz中2D Nav Goal的导航目标点消息的goal_sub_
• 订阅目前导航状态的status_sub_

#ifndef MULTI_NAVI_GOAL_PANEL_H
#define MULTI_NAVI_GOAL_PANEL_H


#include 

#include 
#include 

#include //plugin基类的头文件

#include //Qt按钮
#include //Qt表格
#include //Qt复选框

#include 
#include 
#include 
#include 
#include 
#include