源码解析 | SLAM 多点导航开发思路介绍

原创

已于 2024-03-01 14:29:42 修改 · 4.1k 阅读

109 ·

CC 4.0 BY-SA版权

文章标签：

#ROS #机器人 #教程 #学习

于 2020-12-11 11:08:16 首次发布

本文介绍了一种基于ROS的多点导航解决方案，通过在2DNavGoal与move_base间添加自定义话题，实现了多目标点的连续导航，并支持任务循环、取消等功能。

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