diff --git a/develop.md b/develop.md new file mode 100644 index 0000000..f6c205d --- /dev/null +++ b/develop.md @@ -0,0 +1,492 @@ +# 源码解析 | 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](http://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) 类型的消息，这个是目标点的位姿，包含坐标点与朝向。 + +根据 NodeGraph 可以看到话题 /move_base_simple/goal 被导航包的节点 /move_base 订阅了，进而发给 Navigation 中的各个话题，完成导航。 + +![](images/dev1.jpg) + +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，以完成多任务中的单次目标点的导航（如下图示）。 + +![](images/dev2.jpg) + +3. 如何来发布多点任务？ + +像 2D Nav Goal 一样，我们也可以在 RViz 中开发可视化的操作栏，这要使用到 RViz plugin ， ROS中的可视化工具绝大部分都是基于Qt进行开发的，此前古月居有过详细介绍，可参考[这篇文章](https://zhuanlan.zhihu.com/p/39390512)。 + +### 最终效果 + +首先，我们来看一下最终的实现效果。 + +MultiNaviGoalsPanel是多点SLAM导航任务的可视化操作区，包括任务点列表、循环、重置、取消、开始任务。 + +![](images/dev3.jpg) + +通过 RViz plugin 设计的Mark Display，能够显示的目标点的标号及箭头（朝向）。 + +![](images/intro.png) + + +### 源代码 + +项目地址：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 +#include +#include + +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 status_list); // 检查是否到达目标点 + + static void startSpin(); // spin for sub + 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 + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#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("move_base_simple/goal_temp", 1, + boost::bind(&MultiNaviGoalsPanel::goalCntCB, this, _1)); + + status_sub_ = nh_.subscribe("move_base/status", 1, + boost::bind(&MultiNaviGoalsPanel::statusCB, this, + _1)); + + goal_pub_ = nh_.advertise("move_base_simple/goal", 1); + + cancel_pub_ = nh_.advertise("move_base/cancel", 1); + + marker_pub_ = nh_.advertise("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(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 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_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel) + +``` + + +### 使用 + +使用请参考[使用说明](README.md)。 \ No newline at end of file diff --git a/images/dev1.jpg b/images/dev1.jpg new file mode 100644 index 0000000..29ce107 Binary files /dev/null and b/images/dev1.jpg differ diff --git a/images/dev2.jpg b/images/dev2.jpg new file mode 100644 index 0000000..78fcf24 Binary files /dev/null and b/images/dev2.jpg differ diff --git a/images/dev3.jpg b/images/dev3.jpg new file mode 100644 index 0000000..d463ba2 Binary files /dev/null and b/images/dev3.jpg differ