不久前，我们发布了一个多点SLAM的功能包，今天给大家介绍这个功能包的开发思路。
开发背景：
在使用 ROS Navigation & RViz 进行 2D Nav Goal 导航的时候，我们会遇到这些情况：
- 给定导航的目标点只能设置一个，当有多点任务时需要等待一个个任务结束后，再次手动给目标
- 无法暂停或取消任务
- 任务不可循环
开发目的：
完成多目标点导航，可以对导航环节进行操控，如可循环、取消、重置任务等。
开发思路：
- 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
