Initial commit

.gitignore

@@ -0,0 +1,2 @@
+# Project exclude paths
+/cmake-build-debug/

CMakeLists.txt

@@ -0,0 +1,84 @@
+## BEGIN_TUTORIAL
+## This CMakeLists.txt file for rviz_plugin_tutorials builds both the TeleopPanel tutorial and the ImuDisplay tutorial.
+##
+## First start with some standard catkin stuff.
+cmake_minimum_required(VERSION 2.8.3)
+project(navi_multi_goals_pub_rviz_plugin)
+find_package(catkin REQUIRED COMPONENTS rviz geometry_msgs std_msgs actionlib_msgs)
+catkin_package(CATKIN_DEPENDS message_runtime)
+include_directories(${catkin_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+## This setting causes Qt's "MOC" generation to happen automatically.
+set(CMAKE_AUTOMOC ON)
+
+## This plugin includes Qt widgets, so we must include Qt.
+## We'll use the version that rviz used so they are compatible.
+if(rviz_QT_VERSION VERSION_LESS "5")
+  message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
+  ## pull in all required include dirs, define QT_LIBRARIES, etc.
+  include(${QT_USE_FILE})
+else()
+  message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
+  ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
+  set(QT_LIBRARIES Qt5::Widgets)
+endif()
+## I prefer the Qt signals and slots to avoid defining "emit", "slots",
+## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here.
+add_definitions(-DQT_NO_KEYWORDS)
+
+## Here we specify which header files need to be run through "moc",
+## Qt's meta-object compiler.
+
+
+## Here we specify the list of source files, including the output of
+## the previous command which is stored in ``${MOC_FILES}``.
+set(SOURCE_FILES
+  src/teleop_pad.cpp 
+  ${MOC_FILES}
+)
+
+## An rviz plugin is just a shared library, so here we declare the
+## library to be called ``${PROJECT_NAME}`` (which is
+## "rviz_plugin_tutorials", or whatever your version of this project
+## is called) and specify the list of source files we collected above
+## in ``${SOURCE_FILES}``.
+add_library(${PROJECT_NAME} ${SOURCE_FILES})
+
+## Link the library with whatever Qt libraries have been defined by
+## the ``find_package(Qt4 ...)`` line above, and with whatever libraries
+## catkin has included.
+##
+## Although this puts "rviz_plugin_tutorials" (or whatever you have
+## called the project) as the name of the library, cmake knows it is a
+## library and names the actual file something like
+## "librviz_plugin_tutorials.so", or whatever is appropriate for your
+## particular OS.
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+## END_TUTORIAL
+
+
+## Install rules
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES 
+  plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+#install(DIRECTORY media/
+#  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)
+
+#install(DIRECTORY icons/
+#  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons)
+
+#install(PROGRAMS scripts/send_test_msgs.py
+#  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

package.xml

@@ -0,0 +1,48 @@
+<package>
+<name>navi_multi_goals_pub_rviz_plugin</name>
+<version>0.0.0</version>
+<description>The navi_multi_goals_pub_rviz_plugin package</description>
+<!--  One maintainer tag required, multiple allowed, one person per tag  -->
+<!--  Example:   -->
+<!--  <maintainer email="jane.doe@example.com">Jane Doe</maintainer>  -->
+<maintainer email="lentin@todo.todo">lentin</maintainer>
+<!--  One license tag required, multiple allowed, one license per tag  -->
+<!--  Commonly used license strings:  -->
+<!--    BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3  -->
+<license>TODO</license>
+<!--  Url tags are optional, but mutiple are allowed, one per tag  -->
+<!--  Optional attribute type can be: website, bugtracker, or repository  -->
+<!--  Example:  -->
+<!--  <url type="website">http://wiki.ros.org/navi_multi_goals_pub_rviz_plugin</url>  -->
+<!--  Author tags are optional, mutiple are allowed, one per tag  -->
+<!--  Authors do not have to be maintianers, but could be  -->
+<!--  Example:  -->
+<!--  <author email="jane.doe@example.com">Jane Doe</author>  -->
+<!--  The *_depend tags are used to specify dependencies  -->
+<!--  Dependencies can be catkin packages or system dependencies  -->
+<!--  Examples:  -->
+<!--  Use build_depend for packages you need at compile time:  -->
+<!--    <build_depend>message_generation</build_depend>  -->
+<!--  Use buildtool_depend for build tool packages:  -->
+<!--    <buildtool_depend>catkin</buildtool_depend>  -->
+<!--  Use run_depend for packages you need at runtime:  -->
+<!--    <run_depend>message_runtime</run_depend>  -->
+<!--  Use test_depend for packages you need only for testing:  -->
+<!--    <test_depend>gtest</test_depend>  -->
+<buildtool_depend>catkin</buildtool_depend>
+<build_depend>roscpp</build_depend>
+<build_depend>geometry_msgs</build_depend>
+<build_depend>actionlib_msgs</build_depend>
+<build_depend>rviz</build_depend>
+<build_depend>std_msgs</build_depend>
+<build_depend>message_generation</build_depend>
+<run_depend>roscpp</run_depend>
+<run_depend>rviz</run_depend>
+<run_depend>std_msgs</run_depend>
+<run_depend>geometry_msgs</run_depend>
+<run_depend>message_runtime</run_depend>
+<run_depend>actionlib_msgs</run_depend>
+<export>
+<rviz plugin="${prefix}/plugin_description.xml"/>
+</export>
+</package>

plugin_description.xml

@@ -0,0 +1,5 @@
+<library path="lib/libnavi_multi_goals_pub_rviz_plugin">
+<class name="navi_multi_goals_pub_rviz_plugin/TeleopPanel" type="navi_multi_goals_pub_rviz_plugin::TeleopPanel" base_class_type="rviz::Panel">
+<description> A panel widget allowing simple diff-drive style robot base control. </description>
+</class>
+</library>

src/teleop_pad.cpp

@@ -0,0 +1,283 @@
+#include <stdio.h>
+
+#include <QPainter>
+#include <QLineEdit>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QTimer>
+
+#include <math.h>
+#include <QDebug>
+#include <QtWidgets/QTableWidget>
+#include <QtWidgets/QTableWidgetItem>
+#include <tiff.h>
+#include <zconf.h>
+
+#include "teleop_pad.h"
+
+namespace navi_multi_goals_pub_rviz_plugin {
+
+// 构造函数，初始化变量
+    TeleopPanel::TeleopPanel(QWidget *parent)
+            : rviz::Panel(parent), nh_(), maxNumGoal_(1) {
+
+        goal_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal_temp", 1,
+                                                              boost::bind(&TeleopPanel::goalCntCB, this, _1));
+
+        status_sub_ = nh_.subscribe<actionlib_msgs::GoalStatusArray>("move_base/status", 1,
+                                                                     boost::bind(&TeleopPanel::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);
+        QVBoxLayout *root_layout = new QVBoxLayout;
+        // 创建一个输入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);
+
+        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 TeleopPanel::updateMaxNumGoal() {
+        setMaxNumGoal(output_maxNumGoal_editor_->text());
+    }
+
+// 设置maxNumGoal命名
+    void TeleopPanel::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();
+        }
+    }
+
+    void TeleopPanel::initPoseTable() {
+        ROS_INFO("Initialize");
+        curGoalIdx_ = 0, cycleCnt_ = 0;
+        permit_ = false, cycle_ = false;
+        poseArray_table_->clear();
+        pose_array_.poses.clear();
+        poseArray_table_->setRowCount(maxNumGoal_);
+        poseArray_table_->setColumnCount(3);
+        poseArray_table_->setEditTriggers(QAbstractItemView::NoEditTriggers);
+        QStringList pose_header;
+        pose_header << "x" << "y" << "yaw";
+        poseArray_table_->setHorizontalHeaderLabels(pose_header);
+        cycle_checkbox_->setCheckState(Qt::Unchecked);
+
+    }
+
+    void TeleopPanel::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();
+    }
+
+
+    void TeleopPanel::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);
+        } else {
+            ROS_ERROR("Beyond the maximum number of goals: %d", maxNumGoal_);
+        }
+    }
+
+
+    void TeleopPanel::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)));
+    }
+
+    void TeleopPanel::checkCycle() {
+        cycle_ = true;
+    }
+
+    void TeleopPanel::startNavi() {
+        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, 0, 0));
+            poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 0, 0));
+            poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 0, 0));
+            curGoalIdx_ += 1;
+            permit_ = true;
+        } else {
+            ROS_ERROR("Something Wrong");
+        }
+    }
+
+    void TeleopPanel::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, 0, 0));
+            poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 0, 0));
+            poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 0, 0));
+            curGoalIdx_ += 1;
+            permit_ = true;
+        } else {
+            ROS_ERROR("All goals are completed");
+            permit_ = false;
+        }
+    }
+
+    void TeleopPanel::cycleNavi() {
+        if (permit_) {
+            geometry_msgs::PoseStamped goal;
+            goal.header = pose_array_.header;
+            std::cout << curGoalIdx_ % pose_array_.poses.size() << std::endl;
+            goal.pose = pose_array_.poses.at(curGoalIdx_ % pose_array_.poses.size());
+            goal_pub_.publish(goal);
+            ROS_INFO("Navi to the Goal%d, 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, 0, 0); else color_table = QColor(0, 0, 255);
+            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);
+            std::cout << "check" << std::endl;
+            curGoalIdx_ += 1;
+            cycleCnt_ = curGoalIdx_ / pose_array_.poses.size();
+        }
+    }
+
+    void TeleopPanel::cancelNavi() {
+        if (!cur_goalid_.id.empty()) {
+            cancel_pub_.publish(cur_goalid_);
+            ROS_ERROR("Navigation have been canceled");
+        }
+    }
+
+    void TeleopPanel::statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses) {
+
+        if (checkGoal(statuses->status_list) && ros::ok() && permit_) {
+            if (cycle_) cycleNavi();
+            else completeNavi();
+        }
+    }
+
+    bool TeleopPanel::checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list) {
+        bool done;
+        if (!status_list.empty()) {
+            for (int i = 0; i < status_list.size(); ++i) {
+                if (status_list[i].status == 3) {
+                    done = true;
+                    ROS_INFO("completed Goal%d", curGoalIdx_);
+                } else if (status_list[i].status == 4) {
+                    ROS_ERROR("Goal%d is Invalid, Navi to Next Goal%d", curGoalIdx_, curGoalIdx_ + 1);
+                    return true;
+                } else if (status_list[i].status == 0) {
+                    done = true;
+                } else if (status_list[i].status == 1) {
+                    cur_goalid_ = status_list[i].goal_id;
+                    done = false;
+                } else done = false;
+            }
+
+        } else {
+            ROS_INFO("Please input the Navi Goal");
+            done = false;
+        }
+        return done;
+    }
+
+    void TeleopPanel::startSpin() {
+        if (ros::ok()) {
+            ros::spinOnce();
+        }
+    }
+
+
+
+//// 重载父类的功能
+//    void TeleopPanel::save(rviz::Config config) const {
+//        rviz::Panel::save(config);
+//        config.mapSetValue("Topic", output_topic_);
+//    }
+//
+//
+//// 重载父类的功能，加载配置数据
+//    void TeleopPanel::load(const rviz::Config &config) {
+//        rviz::Panel::load(config);
+//        QString topic;
+//        if (config.mapGetString("Topic", &topic)) {
+//            output_topic_editor_->setText(topic);
+//            updateTopic();
+//        }
+//    }
+
+} // end namespace navi-multi-goals-pub-rviz-plugin
+
+// 声明此类是一个rviz的插件
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::TeleopPanel, rviz::Panel
+)
+// END_TUTORIAL

src/teleop_pad.h

@@ -0,0 +1,90 @@
+#ifndef TELEOP_PAD_H
+#define TELEOP_PAD_H
+
+//所需要包含的头文件
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <rviz/panel.h>   //plugin基类的头文件
+
+#include <QPushButton>
+#include <QTableWidget>
+#include <QCheckBox>
+
+#include <geometry_msgs/PoseArray.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>
+
+class QLineEdit;
+
+namespace navi_multi_goals_pub_rviz_plugin {
+// 所有的plugin都必须是rviz::Panel的子类
+    class TeleopPanel : public rviz::Panel {
+// 后边需要用到Qt的信号和槽，都是QObject的子类，所以需要声明Q_OBJECT宏
+    Q_OBJECT
+    public:
+        // 构造函数，在类中会用到QWidget的实例来实现GUI界面，这里先初始化为0即可
+        TeleopPanel(QWidget *parent = 0);
+
+
+        // 公共槽.
+    public Q_SLOTS:
+
+        // 当用户输入topic的命名并按下回车后，回调用此槽来创建一个相应名称的topic publisher
+        void setMaxNumGoal(const QString &maxNumGoal);
+
+        void writePose(geometry_msgs::Pose pose);
+
+        // 内部槽.
+    protected Q_SLOTS:
+
+        void updateMaxNumGoal();             // update max number of goal
+        void initPoseTable();               // initialize the pose table
+
+        void updatePoseTable();             // update the pose table
+        void startNavi();                   // start navigate for the first pose
+        void cancelNavi();
+
+        void goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &pose);  //goal count sub callback function
+
+        void statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses); //status sub callback function
+
+        void checkCycle();
+
+        void completeNavi();               //after the first pose, continue to navigate the rest of poses
+        void cycleNavi();
+
+        bool checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list);  // check whether arrived the goal
+
+        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_;
+        ros::Subscriber goal_sub_, status_sub_;
+
+
+        int maxNumGoal_;
+        int curGoalIdx_ = 0, cycleCnt_ = 0;
+        bool permit_ = false, cycle_ = false;
+        geometry_msgs::PoseArray pose_array_;
+        actionlib_msgs::GoalID cur_goalid_;
+
+    };
+
+} // end namespace navi-multi-goals-pub-rviz-plugin
+
+#endif // TELEOP_PANEL_H