diff --git a/navi_multi_goals_pub_rviz_plugin/CMakeLists.txt b/navi_multi_goals_pub_rviz_plugin/CMakeLists.txt old mode 100644 new mode 100755 index 9171845..69ade3f --- a/navi_multi_goals_pub_rviz_plugin/CMakeLists.txt +++ b/navi_multi_goals_pub_rviz_plugin/CMakeLists.txt @@ -36,7 +36,7 @@ add_definitions(-DQT_NO_KEYWORDS) ## 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 + src/multi_navi_goal_panel.cpp ${MOC_FILES} ) diff --git a/navi_multi_goals_pub_rviz_plugin/package.xml b/navi_multi_goals_pub_rviz_plugin/package.xml old mode 100644 new mode 100755 diff --git a/navi_multi_goals_pub_rviz_plugin/plugin_description.xml b/navi_multi_goals_pub_rviz_plugin/plugin_description.xml old mode 100644 new mode 100755 index ec4491c..dc08633 --- a/navi_multi_goals_pub_rviz_plugin/plugin_description.xml +++ b/navi_multi_goals_pub_rviz_plugin/plugin_description.xml @@ -2,6 +2,6 @@ - A panel widget allowing simple diff-drive style robot base control. + A panel widget allowing multi-goals navigation. diff --git a/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.cpp b/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.cpp deleted file mode 100644 index 393ab82..0000000 --- a/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.cpp +++ /dev/null @@ -1,311 +0,0 @@ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - - -#include "teleop_pad.h" - -namespace navi_multi_goals_pub_rviz_plugin { - -// 构造函数，初始化变量 - MultiNaviGoalsPanel::MultiNaviGoalsPanel(QWidget *parent) - : rviz::Panel(parent), nh_(), maxNumGoal_(1) { - - goal_sub_ = nh_.subscribe("move_base_simple/goal_temp", 1, - 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); - - cancel_pub_ = nh_.advertise("move_base/cancel", 1); - - marker_pub_ = nh_.advertise("visualization_marker", 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); - // 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(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); - 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_ = true; - } - - // 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, 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"); - } - } - - // 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, 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; - } - } - - // 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, 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); - 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) { - - if (checkGoal(statuses->status_list) && 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 true; - } 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(); - } - } - -} // 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/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.h b/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.h deleted file mode 100644 index dba481b..0000000 --- a/navi_multi_goals_pub_rviz_plugin/src/teleop_pad.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef TELEOP_PAD_H -#define TELEOP_PAD_H - -//所需要包含的头文件 -#include - -#include -#include - -#include //plugin基类的头文件 - -#include -#include -#include - -#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); - - 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(); // 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 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_; - QTableWidget *poseArray_table_; - QCheckBox *cycle_checkbox_; - - QString output_maxNumGoal_; - - // The ROS node handle. - ros::NodeHandle nh_; - ros::Publisher goal_pub_, cancel_pub_, marker_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