ros melodic学习之plugin
ROS(Robot Operating System)是一款非常强大的机器人开发框架,但如果想用于真实的生产环境,还得需要用户自己添加相关的插件,该文主要记录ros melodic版本的plugin及rviz plugin的生成过程。
开发环境说明及项目创建
本文相关开发环境为ubuntu18.04+ROS Melodic+RoboWare studio。首先是在RoboWare studio环境下创建工作空间,如下图所示。
plugin具体步骤
pluginlib利用了C++多态的特性,不同的插件只要使用统一的接口就可以替换使用。用户在使用过程中也不需要修改代码或者重新编译,选择需要使用的插件即可扩展相应的功能。一般来讲,要实现一个插件,主要分为以下几个步骤。
创建功能包
在RoboWare中创建一个叫做plugin_tutorials的功能包,如下图,右击刚才创建的工作空间下src文件夹,输入pluginlib_tutorials roscpp pluginlib,点击enter,创建完成后如下图所示。
创建基类
在pluginlib_tutorials/include/pluginlib_tutorials文件夹下创建polygon_base.h头文件。在该文件中创建polygon基类,定义一些相关的接口。
#ifndef PLUGINLIB_TUTORIALS_POLYGON_BASE_H_
#define PLUGINLIB_TUTORIALS_POLYGON_BASE_H_
namespace polygon_base
{
class RegularPolygon
{
public:
//pluginlib要求构造函数不能带有参数,所以定义initialize来完成需要初始化的工作
virtual void initialize(double side_length) = 0;
//计算面积的接口函数
virtual double area() = 0;
virtual ~RegularPolygon(){}
protected:
RegularPolygon(){}
};
};
#endif
创建plugin类
在pluginlib_tutorials/include/pluginlib_tutorials文件加下创建头文件polygon_plugins.h。该文件中定义rectangle_plugin和triangle_plugin类,实现基类的接口,也可以添加plugin自身需要的接口。
#ifndef PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_
#define PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_
#include <pluginlib_tutorials/polygon_base.h>
#include <cmath>
namespace polygon_plugins
{
class Triangle : public polygon_base::RegularPolygon
{
public:
Triangle() : side_length_() {}
// 初始化边长
void initialize(double side_length)
{
side_length_ = side_length;
}
double area()
{
return 0.5 * side_length_ * getHeight();
}
// Triangle类自己的接口
double getHeight()
{
return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
}
private:
double side_length_;
};
class Square : public polygon_base::RegularPolygon
{
public:
Square() : side_length_() {}
// 初始化边长
void initialize(double side_length)
{
side_length_ = side_length;
}
double area()
{
return side_length_ * side_length_;
}
private:
double side_length_;
};
};
#endif
注册插件
在pluginlib_tutorials/src文件夹下创建polygon_plugins.cpp文件用于注册创建好的插件。
//包含pluginlib的头文件,使用pluginlib的宏来注册插件
#include <pluginlib/class_list_macros.h>
#include <pluginlib_tutorials/polygon_base.h>
#include <pluginlib_tutorials/polygon_plugins.h>
//注册插件,宏参数:plugin的实现类,plugin的基类
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon);
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon);
编译插件的动态链接库
为了编译该插件功能包,需要修改CMakefile.txt文件,加入以下两行编译规则,将插件编译成动态链接库:
include_directories(include)
add_library(polygon_plugins src/polygon_plugins.cpp)
然后在该功能包根目录下执行catkin_make进行编译。
将插件加入ros
为了方便使用plugin,还需要编写xml文件将插件加入ros。这里需要创建和修改功能包根目录下的两个xml文件。
创建pluginlib_tutorials/polygon_plugins.xml
<library path="lib/libpluginlib_tutorials">
<class name="pluginlib_tutorials/regular_triangle" type="polygon_plugi
ns::
Triangle" base_class_type="polygon_base::RegularPolygon">
<description>This is a triangle plugin.</description>
</class>
<class name="pluginlib_tutorials/regular_square" type="polygon_plugins
::Square"
base_class_type="polygon_base::RegularPolygon">
<description>This is a square plugin.</description>
</class>
</library>
这个xml文件主要描述了plugin的动态库路径、实现类、基类、功能描述等相关信息。
修改pluginlib_tutorials/package.xml
在package.xml中添加如下代码
<export>
<pluginlib_tutorials_ plugin="${prefix}/polygon_plugins.xml" />
</export>
如果配置正确,将会看到如下图所示的结果。
调用插件
创建调用插件的cpp文件pluginlib_tutorials/src/polygon_loader.cpp,内容如下:
#include <boost/shared_ptr.hpp>
#include <pluginlib/class_loader.h>
#include <pluginlib_tutorials/polygon_base.h>
int main(int argc, char** argv)
{
// 创建一个ClassLoader,用来加载plugin
pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("pluginlib_tutorials", "polygon_base::RegularPolygon");
try
{
// 加载Triangle插件类,路径在polygon_plugins.xml中定义
boost::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createInstance("pluginlib_tutorials/regular_triangle");
// 初始化边长
triangle->initialize(10.0);
ROS_INFO("Triangle area: %.2f", triangle->area());
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}
try
{
boost::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createInstance("pluginlib_tutorials/regular_square");
square->initialize(10.0);
ROS_INFO("Square area: %.2f", square->area());
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}
return 0;
}
在以上调用插件的代码中,plugin可以在程序中动态加载,然后就可以调用plugin的接口实现相应的功能了。修改CMakefile.txt,添加如下编译规则:
add_executable(polygon_loader src/polygon_loader.cpp)
target_link_libraries(polygon_loader ${catkin_LIBRARIES})
编译然后进行编译,编译完成后在~/.bashrc中最后加入:
source ~/ros/plugin/devel/setup.bash
以便加入该功能包所需的环境变量,然后执行如下命令:
rosrun pluginlib_tutorials polygon_loader
可以看到如下图所示结果:
Rviz plugin具体步骤
rviz是ros官方提供的一款3D可视化工具,实际生产中,可以基于此开发相关的机器人上位控制界面。这里根据相关文档实现一个速度控制插件,如下图所示。
其实现步骤大致如下:
创建功能包
与生成插件方式一样,右击工作空间下src文件夹输入如下内容:
rviz_teleop_commander roscpp rviz std_msgs
该功能包依赖于rviz,因为rviz是基于Qt开发的,所以不需要单独列出对Qt的依赖。
代码实现
在rviz_teleop_commander文件夹下的src文件夹下创建teleo_pad.h文件,其定义了实现的插件类TeleopPanel,在类中使用Qt元素定义了输入框、字符串等多个对象,并且声明了对象调用的槽,也就是回调函数,内容如下:
#ifndef TELEOP_PAD_H
#define TELEOP_PAD_H
//所需要包含的头文件
#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h> //plugin基类的头文件
class QLineEdit;
namespace rviz_teleop_commander
{
// 所有的plugin都必须是rviz::Panel的子类
class TeleopPanel: public rviz::Panel
{
// 后边需要用到Qt的信号和槽,都是QObject的子类,所以需要声明Q_OBJECT宏
Q_OBJECT
public:
// 构造函数,在类中会用到QWidget的实例来实现GUI界面,这里先初始化为0即可
TeleopPanel( QWidget* parent = 0 );
// 重载rviz::Panel积累中的函数,用于保存、加载配置文件中的数据,在我们这个plugin
// 中,数据就是topic的名称
virtual void load( const rviz::Config& config );
virtual void save( rviz::Config config ) const;
// 公共槽.
public Q_SLOTS:
// 当用户输入topic的命名并按下回车后,回调用此槽来创建一个相应名称的topic publisher
void setTopic( const QString& topic );
// 内部槽.
protected Q_SLOTS:
void sendVel(); // 发布当前的速度值
void update_Linear_Velocity(); // 根据用户的输入更新线速度值
void update_Angular_Velocity(); // 根据用户的输入更新角速度值
void updateTopic(); // 根据用户的输入更新topic name
// 内部变量.
protected:
// topic name输入框
QLineEdit* output_topic_editor_;
QString output_topic_;
// 线速度值输入框
QLineEdit* output_topic_editor_1;
QString output_topic_1;
// 角速度值输入框
QLineEdit* output_topic_editor_2;
QString output_topic_2;
// ROS的publisher,用来发布速度topic
ros::Publisher velocity_publisher_;
// The ROS node handle.
ros::NodeHandle nh_;
// 当前保存的线速度和角速度值
float linear_velocity_;
float angular_velocity_;
};
} // end namespace rviz_teleop_commander
#endif // TELEOP_PANEL_H
其对应cpp文件teleop_pad.cpp文件如下:
#include <stdio.h>
#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <geometry_msgs/Twist.h>
#include <QDebug>
#include "teleop_pad.h"
namespace rviz_teleop_commander
{
// 构造函数,初始化变量
TeleopPanel::TeleopPanel( QWidget* parent )
: rviz::Panel( parent )
, linear_velocity_( 0 )
, angular_velocity_( 0 )
{
// 创建一个输入topic命名的窗口
QVBoxLayout* topic_layout = new QVBoxLayout;
topic_layout->addWidget( new QLabel( "Teleop Topic:" ));
output_topic_editor_ = new QLineEdit;
topic_layout->addWidget( output_topic_editor_ );
// 创建一个输入线速度的窗口
topic_layout->addWidget( new QLabel( "Linear Velocity:" ));
output_topic_editor_1 = new QLineEdit;
topic_layout->addWidget( output_topic_editor_1 );
// 创建一个输入角速度的窗口
topic_layout->addWidget( new QLabel( "Angular Velocity:" ));
output_topic_editor_2 = new QLineEdit;
topic_layout->addWidget( output_topic_editor_2 );
QHBoxLayout* layout = new QHBoxLayout;
layout->addLayout( topic_layout );
setLayout( layout );
// 创建一个定时器,用来定时发布消息
QTimer* output_timer = new QTimer( this );
// 设置信号与槽的连接
connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() )); // 输入topic命名,回车后,调用updateTopic()
connect( output_topic_editor_1, SIGNAL( editingFinished() ), this, SLOT( update_Linear_Velocity() )); // 输入线速度值,回车后,调用update_Linear_Velocity()
connect( output_topic_editor_2, SIGNAL( editingFinished() ), this, SLOT( update_Angular_Velocity() ));// 输入角速度值,回车后,调用update_Angular_Velocity()
// 设置定时器的回调函数,按周期调用sendVel()
connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
// 设置定时器的周期,100ms
output_timer->start( 100 );
}
// 更新线速度值
void TeleopPanel::update_Linear_Velocity()
{
// 获取输入框内的数据
QString temp_string = output_topic_editor_1->text();
// 将字符串转换成浮点数
float lin = temp_string.toFloat();
// 保存当前的输入值
linear_velocity_ = lin;
}
// 更新角速度值
void TeleopPanel::update_Angular_Velocity()
{
QString temp_string = output_topic_editor_2->text();
float ang = temp_string.toFloat() ;
angular_velocity_ = ang;
}
// 更新topic命名
void TeleopPanel::updateTopic()
{
setTopic( output_topic_editor_->text() );
}
// 设置topic命名
void TeleopPanel::setTopic( const QString& new_topic )
{
// 检查topic是否发生改变.
if( new_topic != output_topic_ )
{
output_topic_ = new_topic;
// 如果命名为空,不发布任何信息
if( output_topic_ == "" )
{
velocity_publisher_.shutdown();
}
// 否则,初始化publisher
else
{
velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
}
Q_EMIT configChanged();
}
}
// 发布消息
void TeleopPanel::sendVel()
{
if( ros::ok() && velocity_publisher_ )
{
geometry_msgs::Twist msg;
msg.linear.x = linear_velocity_;
msg.linear.y = 0;
msg.linear.z = 0;
msg.angular.x = 0;
msg.angular.y = 0;
msg.angular.z = angular_velocity_;
velocity_publisher_.publish( msg );
}
}
// 重载父类的功能
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 rviz_teleop_commander
// 声明此类是一个rviz的插件
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_teleop_commander::TeleopPanel,rviz::Panel )
// END_TUTORIAL
增加plugin描述文件
在该功能包根目录下创建plugin_description.xml文件,内容如下:
<library path="lib/librviz_teleop_commander">
<class name="rviz_teleop_commander/TeleopPanel"
type="rviz_teleop_commander::TeleopPanel"
base_class_type="rviz::Panel">
<description>
A panel widget allowing simple diff-drive style robot base control.
</description>
</class>
</library>
package.xml
在rviz_teleop_commander/package.xml文件里添加plugin_description.xml的路径:
<export>
<rviz plugin="${prefix}/plugin_description.xml"/>
</export>
CMakeList.txt
## 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(rviz_teleop_commander)
find_package(catkin REQUIRED COMPONENTS rviz)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})
## This plugin includes Qt widgets, so we must include Qt like so:
find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
set(QT_LIBRARIES Qt5::Widgets)
## 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.
qt5_wrap_cpp(MOC_FILES
src/teleop_pad.h
)
## 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}
)
segmentation fault (core dumped)
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})
运行插件
到工作空间根目录进行catkin_make进行编译,编译成功后,设置功能包所在工作空间的环境变量,运行roscore,打开rviz:
rosrun rviz rviz
点击菜单栏中‘Panels’选项,选择“Add New Panel”;弹出如下图所示窗口。
点击ok之后,可见如下图所示窗口:
输入相关内容,如下图所示:
然后打开终端,可以看到如下图说是打印信息,说明ROS中已经有节点在发布/cmd_vel话题的消息了。
遇到的相关问题
BOOST_JJOIN处语法错误
在编译时出现usr/include/boost/type_traits/detail/has_binary_operator.hp:50: Parse error at "BOOST_JOIN"错误,解决办法为在该文件相应位置添加#ifndef Q_MOC_RUN … #endif,如下:
#ifndef Q_MOC_RUN
namespace BOOST_JOIN(BOOST_TT_TRAIT_NAME,_impl) {
#endif
...
#ifndef Q_MOC_RUN
} // namespace impl
#endif
编译成功,但运行插件时出现segmentation fault (core dumped)
其原因可能在于Qt版本不符合,本文相关为已经修改之后的,不会再出现改错误。
参考:《ros机器人开发实践》