commit d238001ec13ef721786042c0aa74d04cbd0c9fab Author: Ryoo Date: Wed Sep 9 15:26:33 2020 +0800 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..da6ed1a --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +# Project exclude paths +/cmake-build-debug/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..419ac91 --- /dev/null +++ b/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}) + diff --git a/package.xml b/package.xml new file mode 100755 index 0000000..4b60f55 --- /dev/null +++ b/package.xml @@ -0,0 +1,48 @@ + +navi_multi_goals_pub_rviz_plugin +0.0.0 +The navi_multi_goals_pub_rviz_plugin package + + + +lentin + + + +TODO + + + + + + + + + + + + + + + + + + + +catkin +roscpp +geometry_msgs +actionlib_msgs +rviz +std_msgs +message_generation +roscpp +rviz +std_msgs +geometry_msgs +message_runtime +actionlib_msgs + + + + diff --git a/plugin_description.xml b/plugin_description.xml new file mode 100755 index 0000000..af287da --- /dev/null +++ b/plugin_description.xml @@ -0,0 +1,5 @@ + + + A panel widget allowing simple diff-drive style robot base control. + + diff --git a/src/teleop_pad.cpp b/src/teleop_pad.cpp new file mode 100755 index 0000000..4366104 --- /dev/null +++ b/src/teleop_pad.cpp @@ -0,0 +1,283 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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("move_base_simple/goal_temp", 1, + boost::bind(&TeleopPanel::goalCntCB, this, _1)); + + status_sub_ = nh_.subscribe("move_base/status", 1, + boost::bind(&TeleopPanel::statusCB, this, _1)); + + goal_pub_ = nh_.advertise("move_base_simple/goal", 1); + + cancel_pub_ = nh_.advertise("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(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 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_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::TeleopPanel, rviz::Panel +) +// END_TUTORIAL diff --git a/src/teleop_pad.h b/src/teleop_pad.h new file mode 100755 index 0000000..9cbe97a --- /dev/null +++ b/src/teleop_pad.h @@ -0,0 +1,90 @@ +#ifndef TELEOP_PAD_H +#define TELEOP_PAD_H + +//所需要包含的头文件 +#include + +#include +#include +#include //plugin基类的头文件 + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +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 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