diff --git a/source/version_two/development/OSintro.md b/source/version_two/development/OSintro.md index c79a61b..8fd3002 100644 --- a/source/version_two/development/OSintro.md +++ b/source/version_two/development/OSintro.md @@ -12,7 +12,6 @@ AutolaborOS 是由 Autolabor 推出的免费开源机器人操作系统，基于 OS在AP1上已经实现了机器人自主导航（2D/3D SLAM 单目标/多目标）、自动循迹、远程遥控等功能。 -Autolabor Pro1 机器人出厂都已预装AutolaborOS。 ## 源码说明 diff --git a/source/version_two/development/imgs/multi1.png b/source/version_two/development/imgs/multi1.png new file mode 100644 index 0000000..2f827ad Binary files /dev/null and b/source/version_two/development/imgs/multi1.png differ diff --git a/source/version_two/development/imgs/multi2.jpeg b/source/version_two/development/imgs/multi2.jpeg new file mode 100644 index 0000000..bb0a97a Binary files /dev/null and b/source/version_two/development/imgs/multi2.jpeg differ diff --git a/source/version_two/development/imgs/multi3.png b/source/version_two/development/imgs/multi3.png new file mode 100644 index 0000000..5cd393f Binary files /dev/null and b/source/version_two/development/imgs/multi3.png differ diff --git a/source/version_two/development/imgs/vslam1.gif b/source/version_two/development/imgs/vslam1.gif new file mode 100644 index 0000000..d4ac3b9 Binary files /dev/null and b/source/version_two/development/imgs/vslam1.gif differ diff --git a/source/version_two/development/imgs/vslam2.png b/source/version_two/development/imgs/vslam2.png new file mode 100644 index 0000000..42931c3 Binary files /dev/null and b/source/version_two/development/imgs/vslam2.png differ diff --git a/source/version_two/development/imgs/vslam3.png b/source/version_two/development/imgs/vslam3.png new file mode 100644 index 0000000..d681ef0 Binary files /dev/null and b/source/version_two/development/imgs/vslam3.png differ diff --git a/source/version_two/development/imgs/vslam4.jpg b/source/version_two/development/imgs/vslam4.jpg new file mode 100644 index 0000000..9b84789 Binary files /dev/null and b/source/version_two/development/imgs/vslam4.jpg differ diff --git a/source/version_two/development/imgs/vslam5.jpg b/source/version_two/development/imgs/vslam5.jpg new file mode 100644 index 0000000..6d9b803 Binary files /dev/null and b/source/version_two/development/imgs/vslam5.jpg differ diff --git a/source/version_two/development/imgs/vslam6.jpg b/source/version_two/development/imgs/vslam6.jpg new file mode 100644 index 0000000..1a5c2a8 Binary files /dev/null and b/source/version_two/development/imgs/vslam6.jpg differ diff --git a/source/version_two/development/multiGoalintro.md b/source/version_two/development/multiGoalintro.md index 3a15674..2cce534 100644 --- a/source/version_two/development/multiGoalintro.md +++ b/source/version_two/development/multiGoalintro.md @@ -1,4 +1,602 @@ # 激光SLAM-多点导航 -## 目录 +本文介绍SLAM导航中[多点导航功能包](/usedoc/navigationKit2/version_two/user_guide/quick_start/multi_slam_doc)的开发思路。 + + +### 开发背景： + +在使用 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 中的各个话题，完成导航。 + +![](./imgs/multi1.png) + + +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，以完成多任务中的单次目标点的导航（如下图示）。 + +![](./imgs/multi2.jpeg) + + +3. 如何来发布多点任务？ + +像 2D Nav Goal 一样，我们也可以在 RViz 中开发可视化的操作栏，这要使用到 RViz plugin ， ROS中的可视化工具绝大部分都是基于Qt进行开发的，此前古月居有过详细介绍，可参考[这篇文章](https://zhuanlan.zhihu.com/p/39390512)。 + +### 最终效果 + +首先，我们来看一下最终的实现效果。 + +MultiNaviGoalsPanel是多点SLAM导航任务的可视化操作区，包括任务点列表、循环、重置、取消、开始任务。 + +通过 RViz plugin 设计的Mark Display，能够显示的目标点的标号及箭头（朝向）。 + +![](./imgs/multi3.png) + + +#### 代码实现 + +#### 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 +#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); + + virtual void load(const rviz::Config &config); + + virtual void save(rviz::Config config) const; + + 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 addPose(); + + 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(); // 启用ROS订阅 + protected: + QLineEdit *output_maxNumGoal_editor_; + QPushButton *output_maxNumGoal_button_, *output_reset_button_, *output_startNavi_button_, *output_cancel_button_, *output_addPoint_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_, init_goal_pub_; + ros::Subscriber goal_sub_, status_sub_; + + tf2_ros::Buffer tfBuffer_; + tf2_ros::TransformListener tfListener_; + // 多目标点任务栏定义 + 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), tfListener_(tfBuffer_) { + + goal_sub_ = nh_.subscribe("move_base_simple/goal_temp", 100, + 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); + + init_goal_pub_ = nh_.advertise("move_base_simple/goal_temp", 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); + + QHBoxLayout *second_row_layout = new QHBoxLayout; + cycle_checkbox_ = new QCheckBox("循环"); + second_row_layout->addWidget(cycle_checkbox_); + output_addPoint_button_ = new QPushButton("添加机器人当前位置"); + second_row_layout->addWidget(output_addPoint_button_); + root_layout->addLayout(second_row_layout); + + // 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(output_addPoint_button_, SIGNAL(clicked()), this, SLOT(addPose())); + 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(); + } + + void MultiNaviGoalsPanel::addPose() { + std::string target_frame = "map"; + std::string child_frame = "base_link"; + if (tfBuffer_.canTransform(target_frame, child_frame, ros::Time(0), ros::Duration(4.0))) { + geometry_msgs::TransformStamped transformStamped = tfBuffer_.lookupTransform(target_frame, child_frame, + ros::Time(0), + ros::Duration(4.0)); + geometry_msgs::PoseStamped pose; + pose.header.frame_id = target_frame; + pose.header.stamp = transformStamped.header.stamp; + pose.pose.position.x = transformStamped.transform.translation.x; + pose.pose.position.y = transformStamped.transform.translation.y; + pose.pose.position.z = transformStamped.transform.translation.z; + pose.pose.orientation = transformStamped.transform.rotation; + + init_goal_pub_.publish(pose); + } + + } + + // 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 false; + } 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(); + } + } + //读取目标点 + void MultiNaviGoalsPanel::load(const rviz::Config &config) { + Panel::load(config); + QString goal_number; + if (config.mapGetString("multi_goal_panel_number", &goal_number)) { + output_maxNumGoal_editor_->setText(goal_number); + updateMaxNumGoal(); + updatePoseTable(); + + QString goal_var; + if (config.mapGetString("multi_goal_panel_data", &goal_var)) { + QStringList goal_list = goal_var.split("|"); + for (int i = 0; i < goal_list.size(); i++) { + QStringList goal = goal_list.at(i).split(","); + + geometry_msgs::PoseStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = ros::Time::now(); + pose.pose.position.x = goal.at(0).toDouble(); + pose.pose.position.y = goal.at(1).toDouble(); + pose.pose.position.z = 0.0; + + pose.pose.orientation = tf::createQuaternionMsgFromYaw(goal.at(2).toDouble() / 180 * 3.14); + init_goal_pub_.publish(pose); + } + } + + bool cycle_flag; + if (config.mapGetBool("multi_goal_panel_cycle", &cycle_flag)) { + if (cycle_flag) { + cycle_checkbox_->setCheckState(Qt::CheckState::Checked); + } else { + cycle_checkbox_->setCheckState(Qt::CheckState::Unchecked); + } + checkCycle(); + } + + } + } + + //保存目标点 + void MultiNaviGoalsPanel::save(rviz::Config config) const { + Panel::save(config); + config.mapSetValue("multi_goal_panel_cycle", cycle_checkbox_->isChecked()); + if (!output_maxNumGoal_editor_->text().isEmpty()) { + int goal_number = output_maxNumGoal_editor_->text().toInt(); + if (goal_number > 0) { + config.mapSetValue("multi_goal_panel_number", goal_number); + + QString *goal_list = new QString(); + for (int i = 0; i < pose_array_.poses.size(); i++) { + for (int j = 0; j < 3; j++) { + goal_list->append(poseArray_table_->item(i, j)->text()); + if (j != 2) { + goal_list->append(","); + } + } + if (i != goal_number - 1) { + goal_list->append("|"); + } + } + + if (!goal_list->isEmpty()) { + config.mapSetValue("multi_goal_panel_data", *goal_list); + } + + } + } + } + +} // end namespace navi-multi-goals-pub-rviz-plugin + +// 声明此类是一个rviz的插件 + +#include + +PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel) + + +``` + diff --git a/source/version_two/development/vslamintro.md b/source/version_two/development/vslamintro.md index 762cca8..fc8bdbf 100644 --- a/source/version_two/development/vslamintro.md +++ b/source/version_two/development/vslamintro.md @@ -1,4 +1,106 @@ # 视觉SLAM +>本文主要对SLAM技术进行介绍，叙述了VSLAM的框架及关键技术和方法，并总结了目前已有的VSLAM系统和相关资料。 + ## 目录 +1. SLAM技术介绍 +2. VSLAM系统及相关资料介绍 +3. SLAM框架 + 1. SLAM的基本过程 + 2. SLAM的主要模块 + + +## 1.SLAM技术介绍 + +**SLAM**，全称是Simultaneous Localization and Mapping，即**同时定位与建图**，指机器人在自身位置不确定的条件下,在完全未知环境中创建地图，同时利用地图进行自主定位和导航。因此可知SLAM的主要工作是**定位**以及**建图**。 + + +![](./imgs/slamintro1.jpg) + +目前SLAM有很多实现方法，根据使用传感器不同，主要分为： + +**（1）激光雷达传感器；** +**（2）视觉传感器；** + +**视觉SLAM**（Visual SLAM 简称VSLAM）包括使用**单目SLAM**、**双目SLAM**和以**Kinect**为代表的深度摄像头的**RGB-D SLAM**。相比使用激光雷达传感器，视觉传感器的成本更低，因此也越来越受青睐。 + +目前SLAM被广泛应用于无人驾驶汽车、无人机、VR和AR等领域。 + +下面是宾大的教授kumar做的特别有名的一个demo，是在无人机上利用二维激光雷达做的SLAM。 + +![](./imgs/vslam1.gif) + +## 2.VSLAM系统及相关资料介绍 + +目前在VSLAM领域实现较好的SLAM系统有： + +- 单目、双目SLAM: + +(1).**PTAM**(Parallel Tracking And Mapping):2007年很流行的SLAM项目，是**第一个**使用BA完成**实时SLAM**的系统。但其缺乏**回环检测和重定位等功能**，只能作用于小规模的场景，且稳定性也不是很高。（http://www.robots.ox.ac.uk/~gk/PTAM/） + +(2).**DTAM**(Dense tracking and mapping in real-time):2011年，Direct SLAM方法的**鼻祖**; + +(3).**LSD-SLAM**(Large-Scale Direct Monocular SLAM):2014年一个半稠密SLAM系统。（http://vision.in.tum.de/research/vslam/lsdslam） + +(4).**ORB-SLAM**:2015年一个比较完整的基于特征点的SLAM系统。 +（http://webdiis.unizar.es/~raulmur/orbslam/） + +- RGB-D SLAM: + +(1).**KinectFusion** 2011 +（http://www.microsoft.com/en-us/research/project/kinectfusion-project-page/） + +(2).**RGBD-SLAM2** 2014 （http://felixendres.github.io/rgbdslam_v2/） + +(3).**ElasticFusion** 2015 （http://www.imperial.ac.uk/dyson-robotics-lab/downloads/elastic-fusion/） + +当然除了上述项目的学习外，对于slam的学习入门，有一些经典教材如： +**Multiple View Geometry in Computer Vision (Second Edition)** +（http://www.robots.ox.ac.uk/~vgg/hzbook/）也有中文版为《计算机视觉中的多视图几何》。 + +**Robotics Vision and Control**（http://www.petercorke.com/RVC/） 本书是面向实践的，详细介绍了机器人和机器视觉。也有了中文版，叫做《机器人学、机器视觉与控制》。 +此外，对于slam入门学习推荐高博的**一起做RGB-D SLAM**其博客为半闲居士（http://www.cnblogs.com/gaoxiang12/p/4633316.html） + +## 3.SLAM框架 + +### 3.1 SLAM的基本过程 + +描述为:机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和传感器数据进行自身定位,同时建造增量式地图。 + +（1）**定位(localization)**：机器人必须知道自己在环境中位置。 + +（2）**建图(mapping)**：机器人必须记录环境中特征的位置（如果知道自己的位置） + +（3）**SLAM**：机器人在定位的同时建立环境地图。其基本原理是运过概率统计的方法，通过多特征匹配来达到定位和减少定位误差的。 + +![](./imgs/vslam2.png) + +### 3.2 VSLAM的主要模块 + + 视觉SLAM主要分为几个模块：**数据采集、视觉里程计(Visual Odometry)、后端优化、建图(Mapping)、闭环检测(Loop closure detection)**。如下图所示： + +![](./imgs/vslam3.png) + +#### 3.2.1 视觉里程计 + +视觉里程计是利用一个图像序列或者一个视频流，计算摄像机的方向和位置的过程。一般包括图像获取后、畸变校正、特征检测匹配或者直接匹配对应像素、通过对极几何原理估计相机的旋转矩阵和平移向量。 + + +![](./imgs/vslam4.jpg) + +#### 3.2.2 后端优化 + +理论上来说，如果视觉里程计模块估计的相机的旋转矩阵R和平移向量t都正确，就能得到完美的定位和建图了。但实际试验中，我们得到的数据往往有很多噪声，且由于传感器的精度、错误的匹配等，都对造成结果有误差。并且由于我们是只把新的一帧与前一个关键帧进行比较，当某一帧的结果有误差时，就会对后面的结果产生**累计误差**，导致误差越来越大。**为了解决这个问题，引入后端优化**。 + +后端优化一般采用捆集调整(BA)、卡尔曼滤波(EKF)、图优化等方式来解决。其中**基于图优化的后端优化**，效果最好。Graph-based SLAM一般使用g2o求解器，进行图优化计算。 + +![](./imgs/vslam5.jpg) + +#### 3.2.3 闭环检测 + +后端优化可能得到一个比较优的解，但当运动回到某一个之前去过的地方，如果能认出这个地方，找到并与当时的关键帧进行比较，就可以得到比单用后端优化更准确更高效的结果。闭环检测就是要解决这个问题。 + +闭环检测有两种方式，一是根据估计出来的相机的位置，看是否与之前否个位置邻近；**另外一种是根据图像，自动识别出来这个场景之前到过，并找到当时的关键帧**。目前常用的后一种方法，其实是一个非监督的模式识别问题。比较常用的方法是采用Bag-of-Words(BOW)，**ORB-SLAM**就是使用这个方法来进行**闭环检测**。 + +![](./imgs/vslam6.jpg)