不久前，我们发布了一个多点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

2. cpp文件 multi_navi_goal_panel.cpp

#include <cstdio>
#include <ros/console.h>
#include <fstream>#include <sstream>
#include <QPainter>#include <QLineEdit>#include <QVBoxLayout>#include <QLabel>#include <QTimer>#include <QDebug>#include <QtWidgets/QTableWidget>#include <QtWidgets/qheaderview.h>

#include "multi_navi_goal_panel.h"
namespace navi_multi_goals_pub_rviz_plugin {

    MultiNaviGoalsPanel::MultiNaviGoalsPanel(QWidget *parent)            : rviz::Panel(parent), nh_(), maxNumGoal_(1) {
        goal_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal_temp", 1,                                                              boost::bind(&MultiNaviGoalsPanel::goalCntCB, this, _1));
        status_sub_ = nh_.subscribe<actionlib_msgs::GoalStatusArray>("move_base/status", 1,                                                                     boost::bind(&MultiNaviGoalsPanel::statusCB, this,                                                                                 _1));
        goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 1);
        cancel_pub_ = nh_.advertise<actionlib_msgs::GoalID>("move_base/cancel", 1);
        marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
        QVBoxLayout *root_layout = new QVBoxLayout;        // create a panel about "maxNumGoal"        QHBoxLayout *maxNumGoal_layout = new QHBoxLayout;        maxNumGoal_layout->addWidget(new QLabel("目标最大数量"));        output_maxNumGoal_editor_ = new QLineEdit;        maxNumGoal_layout->addWidget(output_maxNumGoal_editor_);        output_maxNumGoal_button_ = new QPushButton("确定");        maxNumGoal_layout->addWidget(output_maxNumGoal_button_);        root_layout->addLayout(maxNumGoal_layout);
        cycle_checkbox_ = new QCheckBox("循环");        root_layout->addWidget(cycle_checkbox_);        // creat a QTable to contain the poseArray        poseArray_table_ = new QTableWidget;        initPoseTable();        root_layout->addWidget(poseArray_table_);        //creat a manipulate layout        QHBoxLayout *manipulate_layout = new QHBoxLayout;        output_reset_button_ = new QPushButton("重置");        manipulate_layout->addWidget(output_reset_button_);        output_cancel_button_ = new QPushButton("取消");        manipulate_layout->addWidget(output_cancel_button_);        output_startNavi_button_ = new QPushButton("开始导航!");        manipulate_layout->addWidget(output_startNavi_button_);        root_layout->addLayout(manipulate_layout);
        setLayout(root_layout);        // set a Qtimer to start a spin for subscriptions        QTimer *output_timer = new QTimer(this);        output_timer->start(200);
        // 设置信号与槽的连接        connect(output_maxNumGoal_button_, SIGNAL(clicked()), this,                SLOT(updateMaxNumGoal()));        connect(output_maxNumGoal_button_, SIGNAL(clicked()), this,                SLOT(updatePoseTable()));        connect(output_reset_button_, SIGNAL(clicked()), this, SLOT(initPoseTable()));        connect(output_cancel_button_, SIGNAL(clicked()), this, SLOT(cancelNavi()));        connect(output_startNavi_button_, SIGNAL(clicked()), this, SLOT(startNavi()));        connect(cycle_checkbox_, SIGNAL(clicked(bool)), this, SLOT(checkCycle()));        connect(output_timer, SIGNAL(timeout()), this, SLOT(startSpin()));

    }
// 更新maxNumGoal命名    void MultiNaviGoalsPanel::updateMaxNumGoal() {        setMaxNumGoal(output_maxNumGoal_editor_->text());    }
// set up the maximum number of goals    void MultiNaviGoalsPanel::setMaxNumGoal(const QString &new_maxNumGoal) {        // 检查maxNumGoal是否发生改变.        if (new_maxNumGoal != output_maxNumGoal_) {            output_maxNumGoal_ = new_maxNumGoal;
            // 如果命名为空，不发布任何信息            if (output_maxNumGoal_ == "") {                nh_.setParam("maxNumGoal_", 1);                maxNumGoal_ = 1;            } else {//                velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>(output_maxNumGoal_.toStdString(), 1);                nh_.setParam("maxNumGoal_", output_maxNumGoal_.toInt());                maxNumGoal_ = output_maxNumGoal_.toInt();            }            Q_EMIT configChanged();        }    }
    // initialize the table of pose    void MultiNaviGoalsPanel::initPoseTable() {        ROS_INFO("Initialize");        curGoalIdx_ = 0, cycleCnt_ = 0;        permit_ = false, cycle_ = false;        poseArray_table_->clear();        pose_array_.poses.clear();        deleteMark();        poseArray_table_->setRowCount(maxNumGoal_);        poseArray_table_->setColumnCount(3);        poseArray_table_->setEditTriggers(QAbstractItemView::NoEditTriggers);        poseArray_table_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);        QStringList pose_header;        pose_header << "x" << "y" << "yaw";        poseArray_table_->setHorizontalHeaderLabels(pose_header);        cycle_checkbox_->setCheckState(Qt::Unchecked);
    }
    // delete marks in the map    void MultiNaviGoalsPanel::deleteMark() {        visualization_msgs::Marker marker_delete;        marker_delete.action = visualization_msgs::Marker::DELETEALL;        marker_pub_.publish(marker_delete);    }
    //update the table of pose    void MultiNaviGoalsPanel::updatePoseTable() {        poseArray_table_->setRowCount(maxNumGoal_);//        pose_array_.poses.resize(maxNumGoal_);        QStringList pose_header;        pose_header << "x" << "y" << "yaw";        poseArray_table_->setHorizontalHeaderLabels(pose_header);        poseArray_table_->show();    }
    // call back function for counting goals    void MultiNaviGoalsPanel::goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &pose) {        if (pose_array_.poses.size() < maxNumGoal_) {            pose_array_.poses.push_back(pose->pose);            pose_array_.header.frame_id = pose->header.frame_id;            writePose(pose->pose);            markPose(pose);        } else {            ROS_ERROR("Beyond the maximum number of goals: %d", maxNumGoal_);        }    }
    // write the poses into the table    void MultiNaviGoalsPanel::writePose(geometry_msgs::Pose pose) {
        poseArray_table_->setItem(pose_array_.poses.size() - 1, 0,                                  new QTableWidgetItem(QString::number(pose.position.x, 'f', 2)));        poseArray_table_->setItem(pose_array_.poses.size() - 1, 1,                                  new QTableWidgetItem(QString::number(pose.position.y, 'f', 2)));        poseArray_table_->setItem(pose_array_.poses.size() - 1, 2,                                  new QTableWidgetItem(                                          QString::number(tf::getYaw(pose.orientation) * 180.0 / 3.14, 'f', 2)));
    }
    // when setting a Navi Goal, it will set a mark on the map    void MultiNaviGoalsPanel::markPose(const geometry_msgs::PoseStamped::ConstPtr &pose) {        if (ros::ok()) {            visualization_msgs::Marker arrow;            visualization_msgs::Marker number;            arrow.header.frame_id = number.header.frame_id = pose->header.frame_id;            arrow.ns = "navi_point_arrow";            number.ns = "navi_point_number";            arrow.action = number.action = visualization_msgs::Marker::ADD;            arrow.type = visualization_msgs::Marker::ARROW;            number.type = visualization_msgs::Marker::TEXT_VIEW_FACING;            arrow.pose = number.pose = pose->pose;            number.pose.position.z += 1.0;            arrow.scale.x = 1.0;            arrow.scale.y = 0.2;            number.scale.z = 1.0;            arrow.color.r = number.color.r = 1.0f;            arrow.color.g = number.color.g = 0.98f;            arrow.color.b = number.color.b = 0.80f;            arrow.color.a = number.color.a = 1.0;            arrow.id = number.id = pose_array_.poses.size();            number.text = std::to_string(pose_array_.poses.size());            marker_pub_.publish(arrow);            marker_pub_.publish(number);        }    }
    // check whether it is in the cycling situation    void MultiNaviGoalsPanel::checkCycle() {        cycle_ = cycle_checkbox_->isChecked();    }
    // start to navigate, and only command the first goal    void MultiNaviGoalsPanel::startNavi() {        curGoalIdx_ = curGoalIdx_ % pose_array_.poses.size();        if (!pose_array_.poses.empty() && curGoalIdx_ < maxNumGoal_) {            geometry_msgs::PoseStamped goal;            goal.header = pose_array_.header;            goal.pose = pose_array_.poses.at(curGoalIdx_);            goal_pub_.publish(goal);            ROS_INFO("Navi to the Goal%d", curGoalIdx_ + 1);            poseArray_table_->item(curGoalIdx_, 0)->setBackgroundColor(QColor(255, 69, 0));            poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 69, 0));            poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 69, 0));            curGoalIdx_ += 1;            permit_ = true;        } else {            ROS_ERROR("Something Wrong");        }    }
    // complete the remaining goals    void MultiNaviGoalsPanel::completeNavi() {        if (curGoalIdx_ < pose_array_.poses.size()) {            geometry_msgs::PoseStamped goal;            goal.header = pose_array_.header;            goal.pose = pose_array_.poses.at(curGoalIdx_);            goal_pub_.publish(goal);            ROS_INFO("Navi to the Goal%d", curGoalIdx_ + 1);            poseArray_table_->item(curGoalIdx_, 0)->setBackgroundColor(QColor(255, 69, 0));            poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 69, 0));            poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 69, 0));            curGoalIdx_ += 1;            permit_ = true;        } else {            ROS_ERROR("All goals are completed");            permit_ = false;        }    }
    // command the goals cyclically    void MultiNaviGoalsPanel::cycleNavi() {        if (permit_) {            geometry_msgs::PoseStamped goal;            goal.header = pose_array_.header;            goal.pose = pose_array_.poses.at(curGoalIdx_ % pose_array_.poses.size());            goal_pub_.publish(goal);            ROS_INFO("Navi to the Goal%lu, in the %dth cycle", curGoalIdx_ % pose_array_.poses.size() + 1,                     cycleCnt_ + 1);            bool even = ((cycleCnt_ + 1) % 2 != 0);            QColor color_table;            if (even) color_table = QColor(255, 69, 0); else color_table = QColor(100, 149, 237);            poseArray_table_->item(curGoalIdx_ % pose_array_.poses.size(), 0)->setBackgroundColor(color_table);            poseArray_table_->item(curGoalIdx_ % pose_array_.poses.size(), 1)->setBackgroundColor(color_table);            poseArray_table_->item(curGoalIdx_ % pose_array_.poses.size(), 2)->setBackgroundColor(color_table);            curGoalIdx_ += 1;            cycleCnt_ = curGoalIdx_ / pose_array_.poses.size();        }    }
    // cancel the current command    void MultiNaviGoalsPanel::cancelNavi() {        if (!cur_goalid_.id.empty()) {            cancel_pub_.publish(cur_goalid_);            ROS_ERROR("Navigation have been canceled");        }    }
    // call back for listening current state    void MultiNaviGoalsPanel::statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses) {        bool arrived_pre = arrived_;        arrived_ = checkGoal(statuses->status_list);        if (arrived_) { ROS_ERROR("%d,%d", int(arrived_), int(arrived_pre)); }        if (arrived_ && arrived_ != arrived_pre && ros::ok() && permit_) {            if (cycle_) cycleNavi();            else completeNavi();        }    }
    //check the current state of goal    bool MultiNaviGoalsPanel::checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list) {        bool done;        if (!status_list.empty()) {            for (auto &i : status_list) {                if (i.status == 3) {                    done = true;                    ROS_INFO("completed Goal%d", curGoalIdx_);                } else if (i.status == 4) {                    ROS_ERROR("Goal%d is Invalid, Navi to Next Goal%d", curGoalIdx_, curGoalIdx_ + 1);                    return true;                } else if (i.status == 0) {                    done = true;                } else if (i.status == 1) {                    cur_goalid_ = i.goal_id;                    done = false;                } else done = false;            }        } else {            ROS_INFO("Please input the Navi Goal");            done = false;        }        return done;    }
// spin for subscribing    void MultiNaviGoalsPanel::startSpin() {        if (ros::ok()) {            ros::spinOnce();        }    }
} // end namespace navi-multi-goals-pub-rviz-plugin
// 声明此类是一个rviz的插件
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel)