From 7c6a3dc7d4fc1398ff7be3481b8d116a5b0dcdec Mon Sep 17 00:00:00 2001 From: Ryoo Date: Mon, 21 Sep 2020 12:00:25 +0800 Subject: [PATCH] update commit --- CMakeLists.txt | 2 +- plugin_description.xml | 4 +-- src/teleop_pad.cpp | 82 ++++++++++++++++++++---------------------- src/teleop_pad.h | 14 ++------ 4 files changed, 43 insertions(+), 59 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b6433a..9171845 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ ## BEGIN_TUTORIAL -## This CMakeLists.txt file for rviz_plugin_tutorials builds both the TeleopPanel tutorial and the ImuDisplay tutorial. +## This CMakeLists.txt file for rviz_plugin_tutorials builds both the MultiNaviGoalsPanel tutorial and the ImuDisplay tutorial. ## ## First start with some standard catkin stuff. cmake_minimum_required(VERSION 2.8.3) diff --git a/plugin_description.xml b/plugin_description.xml index ef17b03..ec4491c 100755 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,6 +1,6 @@ - A panel widget allowing simple diff-drive style robot base control. diff --git a/src/teleop_pad.cpp b/src/teleop_pad.cpp index 9604cad..c4d5a86 100755 --- a/src/teleop_pad.cpp +++ b/src/teleop_pad.cpp @@ -16,14 +16,15 @@ namespace navi_multi_goals_pub_rviz_plugin { // 构造函数，初始化变量 - TeleopPanel::TeleopPanel(QWidget *parent) + MultiNaviGoalsPanel::MultiNaviGoalsPanel(QWidget *parent) : rviz::Panel(parent), nh_(), maxNumGoal_(1) { goal_sub_ = nh_.subscribe("move_base_simple/goal_temp", 1, - boost::bind(&TeleopPanel::goalCntCB, this, _1)); + boost::bind(&MultiNaviGoalsPanel::goalCntCB, this, _1)); status_sub_ = nh_.subscribe("move_base/status", 1, - boost::bind(&TeleopPanel::statusCB, this, _1)); + boost::bind(&MultiNaviGoalsPanel::statusCB, this, + _1)); goal_pub_ = nh_.advertise("move_base_simple/goal", 1); @@ -58,7 +59,7 @@ namespace navi_multi_goals_pub_rviz_plugin { 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); @@ -77,12 +78,12 @@ namespace navi_multi_goals_pub_rviz_plugin { } // 更新maxNumGoal命名 - void TeleopPanel::updateMaxNumGoal() { + void MultiNaviGoalsPanel::updateMaxNumGoal() { setMaxNumGoal(output_maxNumGoal_editor_->text()); } -// 设置maxNumGoal命名 - void TeleopPanel::setMaxNumGoal(const QString &new_maxNumGoal) { +// set up the maximum number of goals + void MultiNaviGoalsPanel::setMaxNumGoal(const QString &new_maxNumGoal) { // 检查maxNumGoal是否发生改变. if (new_maxNumGoal != output_maxNumGoal_) { output_maxNumGoal_ = new_maxNumGoal; @@ -100,7 +101,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::initPoseTable() { + // initialize the table of pose + void MultiNaviGoalsPanel::initPoseTable() { ROS_INFO("Initialize"); curGoalIdx_ = 0, cycleCnt_ = 0; permit_ = false, cycle_ = false; @@ -117,13 +119,15 @@ namespace navi_multi_goals_pub_rviz_plugin { } - void TeleopPanel::deleteMark() { + // 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); } - void TeleopPanel::updatePoseTable() { + //update the table of pose + void MultiNaviGoalsPanel::updatePoseTable() { poseArray_table_->setRowCount(maxNumGoal_); // pose_array_.poses.resize(maxNumGoal_); QStringList pose_header; @@ -132,7 +136,8 @@ namespace navi_multi_goals_pub_rviz_plugin { poseArray_table_->show(); } - void TeleopPanel::goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &pose) { + // 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; @@ -143,7 +148,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::writePose(geometry_msgs::Pose pose) { + // 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))); @@ -155,7 +161,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } - void TeleopPanel::markPose(const geometry_msgs::PoseStamped::ConstPtr &pose) { + // 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; @@ -181,11 +188,13 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::checkCycle() { + // check whether it is in the cycling situation + void MultiNaviGoalsPanel::checkCycle() { cycle_ = true; } - void TeleopPanel::startNavi() { + // start to navigate, and only command the first goal + void MultiNaviGoalsPanel::startNavi() { if (!pose_array_.poses.empty() && curGoalIdx_ < maxNumGoal_) { geometry_msgs::PoseStamped goal; goal.header = pose_array_.header; @@ -202,7 +211,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::completeNavi() { + // complete the remaining goals + void MultiNaviGoalsPanel::completeNavi() { if (curGoalIdx_ < pose_array_.poses.size()) { geometry_msgs::PoseStamped goal; goal.header = pose_array_.header; @@ -220,7 +230,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::cycleNavi() { + // command the goals cyclically + void MultiNaviGoalsPanel::cycleNavi() { if (permit_) { geometry_msgs::PoseStamped goal; goal.header = pose_array_.header; @@ -239,14 +250,16 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - void TeleopPanel::cancelNavi() { + // cancel the current command + void MultiNaviGoalsPanel::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) { + // call back for listening current state + void MultiNaviGoalsPanel::statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses) { if (checkGoal(statuses->status_list) && ros::ok() && permit_) { if (cycle_) cycleNavi(); @@ -254,7 +267,8 @@ namespace navi_multi_goals_pub_rviz_plugin { } } - bool TeleopPanel::checkGoal(std::vector status_list) { + //check the current state of goal + bool MultiNaviGoalsPanel::checkGoal(std::vector status_list) { bool done; if (!status_list.empty()) { for (auto &i : status_list) { @@ -279,38 +293,18 @@ namespace navi_multi_goals_pub_rviz_plugin { return done; } - void TeleopPanel::startSpin() { + // spin for subscribing + void MultiNaviGoalsPanel::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_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::TeleopPanel, rviz::Panel) -// END_TUTORIAL +PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel) + diff --git a/src/teleop_pad.h b/src/teleop_pad.h index de15fe8..dba481b 100755 --- a/src/teleop_pad.h +++ b/src/teleop_pad.h @@ -23,31 +23,23 @@ #include -//class QLineEdit; namespace navi_multi_goals_pub_rviz_plugin { - // 所有的plugin都必须是rviz::Panel的子类 - class TeleopPanel : public rviz::Panel { -// 后边需要用到Qt的信号和槽，都是QObject的子类，所以需要声明Q_OBJECT宏 + class MultiNaviGoalsPanel : public rviz::Panel { Q_OBJECT public: - // 构造函数，在类中会用到QWidget的实例来实现GUI界面，这里先初始化为0即可 - explicit TeleopPanel(QWidget *parent = 0); + explicit MultiNaviGoalsPanel(QWidget *parent = 0); - - // 公共槽. public Q_SLOTS: - // 当用户输入topic的命名并按下回车后，回调用此槽来创建一个相应名称的topic publisher 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(); // update max number of goal @@ -69,7 +61,6 @@ namespace navi_multi_goals_pub_rviz_plugin { bool checkGoal(std::vector status_list); // check whether arrived the goal static void startSpin(); // spin for sub - // 内部变量. protected: QLineEdit *output_maxNumGoal_editor_; QPushButton *output_maxNumGoal_button_, *output_reset_button_, *output_startNavi_button_, *output_cancel_button_; @@ -78,7 +69,6 @@ namespace navi_multi_goals_pub_rviz_plugin { QString output_maxNumGoal_; - // The ROS node handle. ros::NodeHandle nh_; ros::Publisher goal_pub_, cancel_pub_, marker_pub_;