不久前，我们发布了一个多点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类型的消息，这个是目标点的位姿，包含坐标点与朝向。

注：geometry_msgs/PoseStamped 

http://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
根据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进行开发的，此前古月居有过详细介绍。

注：古月居文章

https://zhuanlan.zhihu.com/p/39390512

最终效果

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

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：

• 发送每个目标点消息给
• /movebase_simple/goal的goal_pub_
• 发送取消指令消息给 /movebase/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 <string> #include <ros/ros.h>#include <ros/console.h> #include <rviz/panel.h>//plugin基类的头文件 #include <QPushButton>//Qt按钮#include <QTableWidget>//Qt表格#include <QCheckBox>//Qt复选框 #include <visualization_msgs/Marker.h>#include <geometry_msgs/PoseArray.h>#include <geometry_msgs/Point.h>#include <geometry_msgs/PoseStamped.h>#include <std_msgs/String.h>#include <actionlib_msgs/GoalStatus.h>#include <actionlib_msgs/GoalStatusArray.h>#include <tf/transform_datatypes.h> namespace navi_multi_goals_pub_rviz_plugin {     class MultiNaviGoalsPanel : public rviz::Panel {    Q_OBJECT    public:        explicit MultiNaviGoalsPanel(QWidget *parent = 0);     public Q_SLOTS:        void setMaxNumGoal(const QString &maxNumGoal);//设置最大可设置的目标点数量        void writePose(geometry_msgs::Pose pose);//将目标点位姿写入任务列表        void markPose(const geometry_msgs::PoseStamped::ConstPtr &pose);//在地图中标记目标位姿        void deleteMark();//删除标记    protected Q_SLOTS:         void updateMaxNumGoal();             // 更新最大可设置的目标点数量        void initPoseTable();               // 初始化目标点表格         void updatePoseTable();             // 更新目标点表格        void startNavi();                   // 开始第一个目标点任务导航        void cancelNavi();                  // 取消现在进行中的导航         void goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &pose);  // 目标数量子回调函数         void statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses); // 状态子回调函数         void checkCycle();//确认循环         void completeNavi();               // 第一个任务到达后，继续进行剩下任务点的导航任务        void cycleNavi();         bool checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list);  // 检查是否到达目标点         static void startSpin(); // 启用ROS订阅    protected:        QLineEdit *output_maxNumGoal_editor_;        QPushButton *output_maxNumGoal_button_, *output_reset_button_, *output_startNavi_button_, *output_cancel_button_;        QTableWidget *poseArray_table_;        QCheckBox *cycle_checkbox_;         QString output_maxNumGoal_;         // The ROS node handle.        ros::NodeHandle nh_;        ros::Publisher goal_pub_, cancel_pub_, marker_pub_;        ros::Subscriber goal_sub_, status_sub_;        // 多目标点任务栏定义        int maxNumGoal_;        int curGoalIdx_ = 0, cycleCnt_ = 0;        bool permit_ = false, cycle_ = false, arrived_ = false;        geometry_msgs::PoseArray pose_array_;         actionlib_msgs::GoalID cur_goalid_;    }; } // end namespace navi-multi-goals-pub-rviz-plugin #endif // MULTI_NAVI_GOAL_PANEL_H