源码解析 | 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 类型的消息，这个是目标点的位姿，包含坐标点与朝向。
+根据 NodeGraph 可以看到话题 /move_base_simple/goal 被导航包的节点 /move_base 订阅了，进而发给 Navigation 中的各个话题，完成导航。
+
-
+
- 如何基于单点实现多点导航？ +
要在单点基础上实现多目标点导航的话，就要设计一个关于多个导航目标点消息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，以完成多任务中的单次目标点的导航（如下图示）。
+
-
+
- 如何来发布多点任务？ +
像 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 <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)
+使用
+使用请参考使用说明。