ROS学习笔记(9)——在RVIZ上打造人机界面
实现功能
创建功能包
catkin_create_pkg <package_name> [depend1] [depend2] [depend3]
catkin_create_pkg MultiNaviGoalsPanel
roscpp rviz std_msgs
其中[depend1]、[depend2]、[depend3]是创建该软件功能包所需的特定软件功能包,一般称为依赖包。std_msgs是消息数据包。
功能包下建立代码
.h文件

#ifndef MULTI_NAVI_GOAL_PANEL_H #define MULTI_NAVI_GOAL_PANEL_H #include <string> #include <ros/ros.h> #include <ros/console.h> #include <rviz/panel.h>//plugin基类的头文件 #include <QPushButton>//QT按钮 #include <QTableWidget>//QT表格 #include <QCheckBox>//QT复选框 #include <visualization_msgs/Marker.h> #include <geometry_msgs/PoseArray.h> #include <geometry_msgs/Point.h> #include <geometry_msgs/PoseStamped.h> #include <std_msgs/String.h> #include <actionlib_msgs/GoalStatus.h> #include <actionlib_msgs/GoalStatusArray.h> #include <tf/transform_datatypes.h> namespace navi_multi_goals_pub_rviz_plugin { // 所有的plugin都必须是rviz::Panel的子类 class MultiNaviGoalsPanel : public rviz::Panel { Q_OBJECT public: // 构造函数,在类中会用到QWidget的实例来实现GUI界面,这里先初始化为0即可 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(); // 更新最大可设置的目标点数量 void initPoseTable(); // 初始化目标点表格 void updatePoseTable(); // 更新目标点表格 void startNavi(); // 开始第一个目标点导航 void cancelNavi(); //取消现在进行中的目标点导航 void goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &pose); //目标数量子回调函数 void statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &statuses); //状态子回调函数 void checkCycle(); //确认循环 void completeNavi(); //第一个任务到达后,继续进行剩下任务点的导航任务 void cycleNavi(); bool checkGoal(std::vector<actionlib_msgs::GoalStatus> status_list); // 检查是否到达目标点 static void startSpin(); // 启用ROS订阅 //内部变量 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, arrived_ = 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
.cpp文件

#include <cstdio> //将stdio.h的内容用C++的头文件形式表现出来 #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)
namespace
Namespace基本知识_snowlyw的博客-CSDN博客_namespace
C++
C++的 ::
C++的:
ros::NodeHandle
ROS Nodehandle句柄的理解_消雨匆匆-CSDN博客_ros::nodehandl
Qt说明:
- 文字编辑——QLineEdit
- 按键——QPushButton
- 列表——QTableWidget
- 复选框——QCheckBox
- 文字显示——QString
- Qt——QTableWidget 与 QTableView开发实践【不负此行!】_風的博客-CSDN博客_qtablewidget
- QWidget *parent QWidget *parent_Evavava啊的博客-CSDN博客
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(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(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})
package.xml

<package> <name>navi_multi_goals_pub_rviz_plugin</name> <version>0.0.0</version> <description>The navi_multi_goals_pub_rviz_plugin package</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="ryoo@todo.todo">ryoo</maintainer> <!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <license>TODO</license> <!-- Url tags are optional, but mutiple are allowed, one per tag --> <!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Example: --> <!-- <url type="website">http://wiki.ros.org/navi_multi_goals_pub_rviz_plugin</url> --> <!-- Author tags are optional, mutiple are allowed, one per tag --> <!-- Authors do not have to be maintianers, but could be --> <!-- Example: --> <!-- <author email="jane.doe@example.com">Jane Doe</author> --> <!-- The *_depend tags are used to specify dependencies --> <!-- Dependencies can be catkin packages or system dependencies --> <!-- Examples: --> <!-- Use build_depend for packages you need at compile time: --> <!-- <build_depend>message_generation</build_depend> --> <!-- Use buildtool_depend for build tool packages: --> <!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- Use run_depend for packages you need at runtime: --> <!-- <run_depend>message_runtime</run_depend> --> <!-- Use test_depend for packages you need only for testing: --> <!-- <test_depend>gtest</test_depend> --> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>geometry_msgs</build_depend> <build_depend>actionlib_msgs</build_depend> <build_depend>rviz</build_depend> <build_depend>std_msgs</build_depend> <build_depend>message_generation</build_depend> <run_depend>roscpp</run_depend> <run_depend>rviz</run_depend> <run_depend>std_msgs</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>message_runtime</run_depend> <run_depend>actionlib_msgs</run_depend> <export> <rviz plugin="${prefix}/plugin_description.xml"/> </export> </package>
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>
ROS说明:
Publisher
- 发送每个目标点消息给/move_base_simple/goal的goal_pub_
- 发送取消指令消息给/move_base/cancel的cancel_pub_
- 发送文字和箭头标记的mark_pub_。
Subsrciber
- 订阅来自rviz中2D Nav Goal的导航目标点消息的goal_sub_
- 订阅目前导航状态的status_sub_
发现问题:
创建步骤
打开功能包路径
cd /home/wheeltec/wheeltec_robot/src
创建功能包
catkin_create_pkg rviz_navi_multi_goals_pub_rviz_pluginr roscpp rviz std_msgs
打开功能包的SRC文件夹
cd /home/wheeltec/wheeltec_robot/src/navi_multi_goals_pub_rviz_plugin/src
创建.h .cpp文件并粘贴内容
cd /home/wheeltec/wheeltec_robot/src/navi_multi_goals_pub_rviz_plugin创建一个plugin的描述文件 plugin_description.xml
然后在 package.xml文件里添加plugin_description.xml
CMakeLists.txt文件粘贴相应的编译规则
修改系统时间为当前时间
sudo date -s “2022-02-20 00:00:00”
打开工程路径编译
cd /home/wheeltec/wheeltec_robot
catkin_make -DCATKIN_WHITELIST_PACKAGES="navi_multi_goals_pub_rviz_plugin"
问题:
我想要安装如链接中所述的插件,想实现第一步,rviz中识别安装的插件功能包,后续我可以更改内容完成自己的可视化界面设计。
但现在的问题是我按如上步骤安装并编译功能包成功,但rviz中没法实现,已尝试下载更换多个代码,尝试失败。
或者如何更改适配WHEELtec的软件?