add pose marker

.gitignore

@@ -1,2 +1,3 @@
 # Project exclude paths
-/cmake-build-debug/
+/cmake-build-debug/
+/.idea/

CMakeLists.txt

@@ -72,6 +72,11 @@ install(TARGETS
 install(FILES 
   plugin_description.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY icons/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons)
+
+install(DIRECTORY media/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)
 
 #install(DIRECTORY media/
 #  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)

package.xml

@@ -5,7 +5,7 @@
 <!--  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>
+<maintainer email="ryoo@todo.todo">ryoo</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  -->

plugin_description.xml

@@ -1,5 +1,7 @@
 <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>
+    <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

@@ -1,18 +1,15 @@
-#include <stdio.h>
+#include <cstdio>
+
+#include <ros/console.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"
 
@@ -31,6 +28,9 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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;
         // 创建一个输入maxNumGoal命名的窗口
         QHBoxLayout *maxNumGoal_layout = new QHBoxLayout;
@@ -73,8 +73,8 @@ namespace navi_multi_goals_pub_rviz_plugin {
         connect(cycle_checkbox_, SIGNAL(clicked(bool)), this, SLOT(checkCycle()));
         connect(output_timer, SIGNAL(timeout()), this, SLOT(startSpin()));
 
-    }
 
+    }
 
 // 更新maxNumGoal命名
     void TeleopPanel::updateMaxNumGoal() {
@@ -106,6 +106,7 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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);
@@ -116,6 +117,12 @@ namespace navi_multi_goals_pub_rviz_plugin {
 
     }
 
+    void TeleopPanel::deleteMark() {
+        visualization_msgs::Marker marker_delete;
+        marker_delete.action = visualization_msgs::Marker::DELETEALL;
+        marker_pub_.publish(marker_delete);
+    }
+
     void TeleopPanel::updatePoseTable() {
         poseArray_table_->setRowCount(maxNumGoal_);
 //        pose_array_.poses.resize(maxNumGoal_);
@@ -125,18 +132,17 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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);
+            markPose(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,
@@ -146,6 +152,26 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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::markPose(const geometry_msgs::PoseStamped::ConstPtr &pose) {
+        if (ros::ok()) {
+            visualization_msgs::Marker arrow;
+            arrow.header.frame_id = pose->header.frame_id;
+            arrow.ns = "navi_point_arrow";
+            arrow.action = visualization_msgs::Marker::ADD;
+            arrow.type = visualization_msgs::Marker::ARROW;
+            arrow.pose = pose->pose;
+            arrow.scale.x = 1.0;
+            arrow.scale.y = 0.2;
+            arrow.color.r = 1.0f;
+            arrow.color.g = 250;
+            arrow.color.b = 205;
+            arrow.color.a = 1.0;
+            arrow.id = pose_array_.poses.size();
+            marker_pub_.publish(arrow);
+        }
     }
 
     void TeleopPanel::checkCycle() {
@@ -191,10 +217,9 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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,
+            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;
@@ -202,7 +227,6 @@ namespace navi_multi_goals_pub_rviz_plugin {
             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();
         }
@@ -226,17 +250,17 @@ namespace navi_multi_goals_pub_rviz_plugin {
     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) {
+            for (auto &i : status_list) {
+                if (i.status == 3) {
                     done = true;
                     ROS_INFO("completed Goal%d", curGoalIdx_);
-                } else if (status_list[i].status == 4) {
+                } else if (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) {
+                } else if (i.status == 0) {
                     done = true;
-                } else if (status_list[i].status == 1) {
-                    cur_goalid_ = status_list[i].goal_id;
+                } else if (i.status == 1) {
+                    cur_goalid_ = i.goal_id;
                     done = false;
                 } else done = false;
             }
@@ -256,6 +280,8 @@ namespace navi_multi_goals_pub_rviz_plugin {
 
 
 
+
+
 //// 重载父类的功能
 //    void TeleopPanel::save(rviz::Config config) const {
 //        rviz::Panel::save(config);
@@ -276,8 +302,8 @@ namespace navi_multi_goals_pub_rviz_plugin {
 } // 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
-)
+PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::TeleopPanel, rviz::Panel)
 // END_TUTORIAL

src/teleop_pad.h

@@ -6,29 +6,35 @@
 
 #include <ros/ros.h>
 #include <ros/console.h>
+
 #include <rviz/panel.h>   //plugin基类的头文件
 
 #include <QPushButton>
 #include <QTableWidget>
 #include <QCheckBox>
 
+#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>
 
-class QLineEdit;
 
+//class QLineEdit;
 namespace navi_multi_goals_pub_rviz_plugin {
-// 所有的plugin都必须是rviz::Panel的子类
+
+
+
+    // 所有的plugin都必须是rviz::Panel的子类
     class TeleopPanel : public rviz::Panel {
 // 后边需要用到Qt的信号和槽，都是QObject的子类，所以需要声明Q_OBJECT宏
     Q_OBJECT
     public:
         // 构造函数，在类中会用到QWidget的实例来实现GUI界面，这里先初始化为0即可
-        TeleopPanel(QWidget *parent = 0);
+        explicit TeleopPanel(QWidget *parent = 0);
 
 
         // 公共槽.
@@ -38,6 +44,8 @@ namespace navi_multi_goals_pub_rviz_plugin {
         void setMaxNumGoal(const QString &maxNumGoal);
 
         void writePose(geometry_msgs::Pose pose);
+        void markPose(const geometry_msgs::PoseStamped::ConstPtr &pose);
+        void deleteMark();
 
         // 内部槽.
     protected Q_SLOTS:
@@ -60,7 +68,7 @@ namespace navi_multi_goals_pub_rviz_plugin {
 
         bool checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list);  // check whether arrived the goal
 
-        void startSpin(); // spin for sub
+        static void startSpin(); // spin for sub
         // 内部变量.
     protected:
         QLineEdit *output_maxNumGoal_editor_;
@@ -73,7 +81,7 @@ namespace navi_multi_goals_pub_rviz_plugin {
 
         // The ROS node handle.
         ros::NodeHandle nh_;
-        ros::Publisher goal_pub_, cancel_pub_;
+        ros::Publisher goal_pub_, cancel_pub_, marker_pub_;
         ros::Subscriber goal_sub_, status_sub_;
 
 
@@ -81,8 +89,10 @@ namespace navi_multi_goals_pub_rviz_plugin {
         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