diff --git a/navi_multi_goals_pub_rviz_plugin/src/multi_navi_goal_panel.cpp b/navi_multi_goals_pub_rviz_plugin/src/multi_navi_goal_panel.cpp new file mode 100755 index 0000000..ba09772 --- /dev/null +++ b/navi_multi_goals_pub_rviz_plugin/src/multi_navi_goal_panel.cpp @@ -0,0 +1,311 @@ +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + + +#include "multi_navi_goal_panel.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; + // create a panel about "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/multi_navi_goal_panel.h b/navi_multi_goals_pub_rviz_plugin/src/multi_navi_goal_panel.h new file mode 100755 index 0000000..c27e2c1 --- /dev/null +++ b/navi_multi_goals_pub_rviz_plugin/src/multi_navi_goal_panel.h @@ -0,0 +1,90 @@ +#ifndef MULTI_NAVI_GOAL_PANEL_H +#define MULTI_NAVI_GOAL_PANEL_H + + +#include + +#include +#include + +#include + +#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 // MULTI_NAVI_GOAL_PANEL_H