Rviz 实现 pannel 插件

这个是鉴于另一个Github上的项目学习的

https://github.com/chaoli2/rviz_teleop_commander/tree/master/src

其中代码注释很清楚，大家也可以去看源代码

首先是CmakeList.txt文件

cmake_minimum_required(VERSION 2.8.3)
project(bing_pannel)
find_package(catkin REQUIRED COMPONENTS rviz)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})

set(CMAKE_AUTOMOC ON)

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)
  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)
  set(QT_LIBRARIES Qt5::Widgets)
endif()

add_definitions(-DQT_NO_KEYWORDS)
set(SOURCE_FILES
  src/bing_pannel.cpp ##这里是你的主文件
  ${MOC_FILES}
)
add_library(${PROJECT_NAME} ${SOURCE_FILES})

target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})

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_discription.xml ##这里是你的plugin_discription.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

只要在workspace下catkin_make后，这个包里的插件便直接安装了。package.xml文件应该如下：

<?xml version="1.0"?>
<package format="1">
  <name>bing_pannel</name>
  <version>0.0.0</version>
  <description>The bing_pannel package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="bing@todo.todo">bing</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 multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/bing_pannel</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>rviz</build_depend>
  <build_depend>std_msgs</build_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rviz</run_depend>
  <run_depend>std_msgs</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
      <rviz plugin="${prefix}/plugin_discription.xml"/>
    <!-- 这里面可以修改plugin_discription.xml -->
  </export>
</package>    

而真正描述的plugin_discription.xml为如下：

<library path="lib/libbing_pannel">
  <class name="bing_pannel/BingPannel"
         type="bing_rviz_plugin::BingPannel"
         base_class_type="rviz::Panel">
    <description>
      Bing test pannel.
    </description>
  </class>
</library>

<class name="bing_pannel/BingPannel"
         type="bing_rviz_plugin::BingPannel"
         base_class_type="rviz::Panel">
中name中的bing_pannel/BingPannel里，写的是package的名字加class
type中是namespace加class名。

我们在src文件夹下加入bing_pannel.cpp和bing_pannel.h。这个package的文件夹如下。

我们先看.h文件。

#ifndef BING_PANNEL_H
#define BING_PANNEL_H

#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h>

class QLineEdit;

namespace bing_rviz_plugin
{
  class  BingPannel: public rviz::Panel
  {
      Q_OBJECT
      public:
        BingPannel( QWidget* parent = 0 );
        virtual void load( const rviz::Config& config );
        virtual void save( rviz::Config config ) const;

      public Q_SLOTS:
        void setTopic( const QString& topic );

      protected Q_SLOTS:
        void sendVel();
        void update_Linear_Velocity();
        void update_Angular_Velocity();
        void updateTopic();

      protected:
        QLineEdit* output_topic_editor_;
        QString output_topic_;
        QLineEdit* output_topic_editor_1;
        QString output_topic_1;

        ros::Publisher velocity_publisher_;
        ros::NodeHandle nh_;
        float linear_velocity_;
  };
}

#endif

 头文件中，rviz/panel.h 就是 pannel头文件。插件显示如下图。在rviz中加入pannel的话，要在标题栏中，找到Panels 〉Add New Panel。选择并加入需要的pannel。

 选择添加的Pannel。

我们这里的插件会通过文本框里的输入发送一个输入的Topic名字的消息。在后面需要填写速度X和速度Y，并且回车就可以发布消息了。 

回车后，rostopic list会显示/test_velocity。我们这里发布的是geometry_msgs::Twist。

下面我们的代码，里面是.h文件。

#ifndef BING_PANNEL_H
#define BING_PANNEL_H

#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h>

class QLineEdit;

namespace bing_rviz_plugin
{
  class  BingPannel: public rviz::Panel
  {
      Q_OBJECT
      public:
　　　　//用QWigdet的实例来实现GUI界面
        BingPannel( QWidget* parent = 0 );
        virtual void load( const rviz::Config& config );
        virtual void save( rviz::Config config ) const;

      public Q_SLOTS:
　　　　//当填写完时候，回车后，执行会调函数
        void setTopic( const QString& topic );

      protected Q_SLOTS:
        void sendVel();
        void update_Linear_Velocity_X();
        void update_Linear_Velocity_Y();
        void updateTopic();

      protected:
　　　　//这里是三个显示的文字框的定义
        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消息发布
        ros::Publisher velocity_publisher_;
        ros::NodeHandle nh_;

　　　　//x速度，y速度
        float linear_velocity_X;
        float linear_velocity_Y;
  };
}

#endif

相应的.cpp文件如下：

#include <stdio.h>

#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>

#include <geometry_msgs/Twist.h>
#include <QDebug>

#include "bing_pannel.h"

namespace bing_rviz_plugin{
  BingPannel::BingPannel( QWidget* parent ): rviz::Panel( parent ), linear_velocity_X( 0 ),linear_velocity_Y( 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_ );
　　//xvelocity窗口
    topic_layout->addWidget( new QLabel( "Linear Velocity X:" ));
    output_topic_editor_1 = new QLineEdit;
    topic_layout->addWidget( output_topic_editor_1 );
　　//yvelocity窗口
    topic_layout->addWidget( new QLabel( "Linear Velocity Y:" ));
    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() ));
    connect( output_topic_editor_1, SIGNAL( editingFinished() ), this, SLOT( update_Linear_Velocity_X() ));
    connect( output_topic_editor_1, SIGNAL( editingFinished() ), this, SLOT( update_Linear_Velocity_Y() ));
    
　　//定时器回调，定时运行sendVel（）
　　 connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));

    output_timer->start( 100 );
  }
　　//读取x速度
  void BingPannel::update_Linear_Velocity_X()
  {
      QString temp_string = output_topic_editor_1->text();
      float lin = temp_string.toFloat();
      linear_velocity_X = lin;
  }
　　//读取y速度
  void BingPannel::update_Linear_Velocity_Y()
  {
      QString temp_string = output_topic_editor_2->text();
      float lin = temp_string.toFloat();
      linear_velocity_Y = lin;
  }
　　//更新topic
  void BingPannel::updateTopic()
  {
    setTopic( output_topic_editor_->text() );
  }
　　//topic名字
  void BingPannel::setTopic( const QString& new_topic )
  {
    if( new_topic != output_topic_ )
    {
      output_topic_ = new_topic;
      if( output_topic_ == "" )
      {
        velocity_publisher_.shutdown();
      }
      else
      {
        velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
      }
      Q_EMIT configChanged();
    }
  }

  void BingPannel::sendVel()
  {
    if( ros::ok() && velocity_publisher_ )
    {
      geometry_msgs::Twist msg;
      msg.linear.x = linear_velocity_X;
      msg.linear.y = linear_velocity_Y ;
      msg.linear.z = 0;
      msg.angular.x = 0;
      msg.angular.y = 0;
      msg.angular.z = 1;
      velocity_publisher_.publish( msg );
    }
  }

  void BingPannel::save( rviz::Config config ) const
  {
    rviz::Panel::save( config );
    config.mapSetValue( "Topic", output_topic_ );
  }

  void BingPannel::load( const rviz::Config& config )
  {
    rviz::Panel::load( config );
    QString topic;
    if( config.mapGetString( "Topic", &topic ))
    {
      output_topic_editor_->setText( topic );
      updateTopic();
    }
  }

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(bing_rviz_plugin::BingPannel,rviz::Panel )

保存后，catkin_make 就可以在rviz里找到了。

参考：

http://www.guyuehome.com/945