0. 简介
之前尝试过使用plugin来实现功能的模块化.同时在ROS中,为了使核心的代码是只留下输入输出接口的，所以我们使用plugin来实现.so文件的封装以及动态调取.但是在近期接触后发现在RVIZ的插件开发中,其核心也是plugin插件性质,这里再开一篇文章来进行介绍.详细的配置可以在文章:ROS设置plugin插件中看到.
1. 内容介绍
rviz是ROS官方提供的一款3D可视化工具，几乎我们需要用到的所有机器人相关数据都可以在rviz中展现。当然由于机器人系统的需求不同，很多时候rviz中已有的一些功能仍然无法满足我们的需求，这个时候rviz的plugin机制就派上用场了。rviz可以使用插件机制来扩展丰富的功能，我们完全可以在rviz的基础上，打造属于我们自己的机器人人机界面。
在ros的编程中，ROS中的可视化工具绝大部分都是基于Qt进行开发的，rviz的plugin也不例外。而且古月老师也向我们展示了一个最基础的plugin的开发过程.
这里我们选取了一个最为常用的可发布多目标点任务的SLAM 建图导航插件.
2. 代码分析
plugin_description.xml
<library path="lib/libnavi_multi_goals_pub_rviz_plugin">
<class name="navi_multi_goals_pub_rviz_plugin/MultiNaviGoalsPanel"
type="navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel"
base_class_type="rviz::Panel">
<description>A panel widget allowing multi-goals navigation.</description>
</class>
</library>
可以仔细看一下这个xml文件中的内容。
<library path="lib/libnavi_multi_goals_pub_rviz_plugin"> library标签写明了要输出的lib文件所在的相对路径；
<class></class> class标签内容写明了插件的信息。
type：插件的完整类型，例如polygon_plugins::Triangle;
base_class_type：插件完整类型的父类，例如polygon_base::RegularPolygon;
description：描述插件是做什么的；
CMakeLists.txt
## BEGIN_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)
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/multi_navi_goal_panel.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实现将插件放置到可执行文件或者库文件中
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具体可以参照《CMakeLists文件install的使用》进行设置，ROS当中的CATKIN_PACKAGE_SHARE_DESTINATION可以参照下文。
multi_navi_goal_panel.cpp
#include <cstdio>
#include <ros/console.h>
#include <fstream>
#include <sstream>
#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QLabel>
#include <QTimer>
#include <QDebug>
#include <QtWidgets/QTableWidget>
#include <QtWidgets/qheaderview.h>
#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<geometry_msgs::PoseStamped>("move_base_simple/goal_temp", 1,
boost::bind(&MultiNaviGoalsPanel::goalCntCB, this, _1));
status_sub_ = nh_.subscribe<actionlib_msgs::GoalStatusArray>("move_base/status", 1,
boost::bind(&MultiNaviGoalsPanel::statusCB, this,
_1));
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;
// 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<geometry_msgs::Twist>(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);
poseArray_table_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
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_ = cycle_checkbox_->isChecked();
}
// 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, 69, 0));
poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 69, 0));
poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 69, 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, 69, 0));
poseArray_table_->item(curGoalIdx_, 1)->setBackgroundColor(QColor(255, 69, 0));
poseArray_table_->item(curGoalIdx_, 2)->setBackgroundColor(QColor(255, 69, 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, 69, 0); else color_table = QColor(100, 149, 237);
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) {
bool arrived_pre = arrived_;
arrived_ = checkGoal(statuses->status_list);
if (arrived_) { ROS_ERROR("%d,%d", int(arrived_), int(arrived_pre)); }
if (arrived_ && arrived_ != arrived_pre && ros::ok() && permit_) {
if (cycle_) cycleNavi();
else completeNavi();
}
}
//check the current state of goal
bool MultiNaviGoalsPanel::checkGoal(std::vector<actionlib_msgs::GoalStatus> 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/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel)
上面的代码为plugin的主体代码函数,当中会涉及到Qt中的重要概念——信号、槽，类似于回调函数.这样通过编译就可以在rviz中读取到我们需要的插件了.
在rviz启动之后应该并没有什么不同，点击菜单栏中的“Panels”选项，选择“Add New
Panel”，在打开的窗口中的最下方，就可以看到我们创建的plugin了，点中之后，可以在下方的”Description”中看到我们在plugin_description.xml文件中对plugin的描述。
RViz 的左上角依次点击 Panels -> Add New Panel -> navi_multi_goals_pub_rviz_plugin -> MultiNaviGoalsPanel
参考链接
https://zhuanlan.zhihu.com/p/39390512
https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin
https://www.cnblogs.com/TIANHUAHUA/p/8337979.html
https://blog.csdn.net/lovely_yoshino/article/details/116756219
评论（0）
您还未登录，请登录后发表或查看评论