ROS学习（25）rviz plugin插件

---

前言

  RVIZ是ROS提供的一款3D可视化工具，其作为一种可扩展化的视图工具，可以使用插件机制添加丰富的功能模块。RVIZ中常用的激光数据、图像数据的可视化显示其实都是官方提供的插件。
  下面，通过实现一个速度控制的插件，熟悉rviz plugin插件的实现过程。
  这篇文章源码来自胡老师的书-《ROS机器人开发实践》。

一、功能描述

界面如图：

该插件使用Qt开发，包含三个输入框，分别是话题名、线速度和角速度。

二、创建功能包

命令如下：

catkin_create_pkg rviz_teleop_commander roscpp rviz std_msgs

这个功能包依赖于RVIZ，因为RVIZ是基于Qt开发的，所以不需要单独列出对Qt的依赖。

三、代码实现

1、创建teleop_pad.h文件

文件内容如下：

#ifndef TELEOP_PAD_H
#define TELEOP_PAD_H

//所需要包含的头文件
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h> //plugin基类的头文件
#endif

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_;

    // ROS节点句柄
    ros::NodeHandle nh_;

    // 当前保存的线速度和角速度值
    float linear_velocity_;
    float angular_velocity_;
};

} // end namespace rviz_teleop_commander

#endif // TELEOP_PANEL_H

2、创建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 );

    // 设置信号与槽的连接
    // 输入topic命名，回车后，调用updateTopic()
    connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() ));    
    // 输入线速度值，回车后，调用update_Linear_Velocity()    
    connect( output_topic_editor_1, SIGNAL( editingFinished() ), this, SLOT( update_Linear_Velocity() )); 
    // 输入角速度值，回车后，调用update_Angular_Velocity()
    connect( output_topic_editor_2, SIGNAL( editingFinished() ), this, SLOT( 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

四、编译插件

1、创建plugin的描述文件

在功能包根目录下，创建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>

2、修改package.xml文件

在package.xml文件中添加plugin_description.xml文件路径，内容如下：

  <export>
      <rviz plugin="${prefix}/plugin_description.xml"/>
  </export>

3、修改CMakeLists.txt文件

添加编译规则，内容如下：

## 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}
)

install(FILES 
  plugin_description.xml
  DESTINATION ${
    CATKIN_PACKAGE_SHARE_DESTINATION})

做好上述修改后，就可以编译该功能包了，命令如下：

catkin_make --pkg rviz_teleop_commander

五、运行插件

首先运行roscore，命令如下：

roscore

运行RVIZ，命令如下：

rosrun rviz rviz

点击菜单栏中的Panels选项，选择Add New Panel，在插件列表中就可以看到刚刚创建的插件，如图：

点击OK后，就可以看到，该插件的界面，输入参数值，如下：

运行命令：

rostopic echo /cmd_vel

效果如下：

可以看到ROS中已经有节点在发布/cmd_vel话题的消息了。