基于ROS和Qt开发环境实现点云数据的实时可视化

１ 简介

  本文利用可视化应用程序RViz的可视化小部件编写应用程序，来实现点云数据（PointCloud2类型）的实时可视化。

２ 前提条件

  （１）Ubuntu开发环境，我使用的是Ubuntu18.04；
  （２）安装有ROS开发环境，我使用的是ROS Melodic，安装参考官方网站：（http://wiki.ros.org/melodic/Installation/Ubuntu）；
  （３）安装ROS Qt Creator Plug-in插件，安装参考官方网站：（https://ros-qtc-plugin.readthedocs.io/en/latest/_source/How-to-Install-Users.html#qt-installer-procedure）

３ 功能实现

１）创建头文件

#ifndef MYVIZ_H
#define MYVIZ_H

#include <QWidget>
#include "rviz/add_display_dialog.h"
#include "rviz/displays_panel.h"

namespace rviz
{
    class Display;
    class Line;
    class RenderPanel;
    class DisplaysPanel;
    class VisualizationManager;
    class AddDisplayDialog;
}

// BEGIN_TUTORIAL
// Class "MyViz" implements the top level widget for this example.
class MyViz: public QWidget
{
    Q_OBJECT
    public:
        MyViz( QWidget* parent = 0 );
        virtual ~MyViz();

    private Q_SLOTS:
        void setThickness( int thickness_percent );
        void setCellSize( int cell_size_percent );

    private:
        rviz::VisualizationManager* manager_;   // 创建rviz内已有的display
        rviz::RenderPanel* render_panel_;       // 用于构建和布置渲染画面
        rviz::DisplaysPanel* displays_panel_;
        rviz::Display* grid_;
        rviz::Display* pc_;                     //　要显示的PointCloud2类型的display对象
};
// END_TUTORIAL
#endif // MYVIZ_H

  其中，VisualizationManager类型指针manager_用来创建rviz内已有的display；render_panel_指针用于构建和布置渲染画面；Display类型指针pc_即我们要显示的PointCloud2类型的display对象。

２）编写源文件

#include <QSlider>
#include <QLabel>
#include <QGridLayout>
#include <QVBoxLayout>

#include <QWidget>
#include <QMetaObject>
#include <QDockWidget>
#include <QApplication>

#include "rviz/visualization_manager.h"
#include "rviz/render_panel.h"
#include "rviz/display.h"
#include "rviz/add_display_dialog.h"

#include "rviz/displays_panel.h"
#include "rviz/display_factory.h"

#include "my_rviz/myrviz.h"

// Constructor for MyViz.  This does most of the work of the class.
MyViz::MyViz( QWidget* parent ): QWidget( parent )
{
    // Construct and lay out labels and slider controls.
    QLabel* thickness_label = new QLabel( "Line Thickness" );
    QSlider* thickness_slider = new QSlider( Qt::Horizontal );
    thickness_slider->setMinimum( 1 );
    thickness_slider->setMaximum( 100 );
    QLabel* cell_size_label = new QLabel( "Cell Size" );
    QSlider* cell_size_slider = new QSlider( Qt::Horizontal );
    cell_size_slider->setMinimum( 1 );
    cell_size_slider->setMaximum( 100 );
    QGridLayout* controls_layout = new QGridLayout();
    controls_layout->addWidget( thickness_label, 0, 0 );
    controls_layout->addWidget( thickness_slider, 0, 1 );
    controls_layout->addWidget( cell_size_label, 1, 0 );
    controls_layout->addWidget( cell_size_slider, 1, 1 );

    // Construct and lay out render panel.
    render_panel_ = new rviz::RenderPanel();
    displays_panel_=new rviz::DisplaysPanel();
    QVBoxLayout* main_layout = new QVBoxLayout;
    main_layout->addLayout( controls_layout );
    main_layout->addWidget( render_panel_ );
    main_layout->addWidget( displays_panel_ );
    // Set the top-level layout for this MyViz widget.
    setLayout( main_layout );

    // Make signal/slot connections.
    connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int )));
    connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int )));

    // Next we initialize the main RViz classes.
    // The VisualizationManager is the container for Display objects,
    // holds the main Ogre scene, holds the ViewController, etc.  It is
    // very central and we will probably need one in every usage of
    // librviz.
    
    manager_ = new rviz::VisualizationManager( render_panel_ );
    render_panel_->initialize( manager_->getSceneManager(), manager_ );
    displays_panel_->initialize(manager_);
    manager_->initialize();
    manager_->startUpdate();
    manager_->setFixedFrame("/rslidar"); 

    // Create a Grid display.
    grid_ = manager_->createDisplay( "rviz/Grid", "Grid", true );
    ROS_ASSERT( grid_ != NULL );
    
    grid_->subProp( "Line Style" )->setValue( "Billboards" );
    grid_->subProp( "Color" )->setValue( QColor(Qt::yellow));

    // Create a pc display.
    pc_ = manager_->createDisplay( "rviz/PointCloud2", "PointCloud2", true );
    ROS_ASSERT( pc_ != NULL );
      
    pc_->subProp( "Style" )->setValue( "Flat Squares" );
    pc_->subProp( "Color Transformer" )->setValue( "RGB8" );
    pc_->setTopic( "/rslidar_points","sensor_msgs/PointCloud2" );
  
    thickness_slider->setValue( 10 );
    cell_size_slider->setValue( 15 );
}

// Destructor.
MyViz::~MyViz()
{
    delete manager_;
}

// This function is a Qt slot connected to a QSlider's valueChanged()
// signal.  It sets the line thickness of the grid by changing the
// grid's "Line Width" property.
void MyViz::setThickness( int thickness_percent )
{
    if( grid_ != NULL )
    {
      grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f );
      
    }
	  
}

// This function is a Qt slot connected to a QSlider's valueChanged()
// signal.  It sets the cell size of the grid by changing the grid's
// "Cell Size" Property.
void MyViz::setCellSize( int cell_size_percent )
{
    if( grid_ != NULL )
    {
      grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f );
    }
}

  【注意】manager_->setFixedFrame("/rslidar") 需要根据激光雷达的驱动配置进行修改，本文利用其来显示PointCloud2类型对象。另外，PointCloud2类型对象要为其订阅一个topic，这里用到了Display::setTopic(const QString& topic, const QString& datatype )函数，其中datatype不是直接用displays的类型，而是"sensor_msgs/PointCloud2"，并且话题名需要根据实际情况进行修改。

３）主函数

  主要用于初始化ROS，创建一个QApplication以及MyViz类对象，并实现窗口的显示。

// The main() for this "myviz" example is very simple, it just
// initializes ROS, creates a QApplication, creates the top-level
// widget (of type "MyViz"), shows it, and runs the Qt event loop.

#include <QApplication>
#include <ros/ros.h>
#include "my_rviz/myrviz.h"


int main(int argc, char **argv)
{
    if( !ros::isInitialized() )
    {
        ros::init( argc, argv, "myviz", ros::init_options::AnonymousName );
    }

    QApplication app( argc, argv );

    MyViz* myviz = new MyViz();
    myviz->show();

    app.exec();
    
    delete myviz;
}

４）配置CMakeLists.txt文件

cmake_minimum_required(VERSION 3.0.2)
project(my_rviz)
set(CMAKE_INCLUDE_CURRENT_DIR ON)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  rviz
  std_msgs
  visualization_msgs
)

# Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

link_directories(${catkin_LIBRARY_DIRS})

## This plugin includes Qt widgets, so we must include Qt like so:
find_package(Qt5 REQUIRED Core Widgets Multimedia)
set(QT_LIBRARIES Qt5::Widgets Qt5::Multimedia)

## 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.
file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/my_rviz/myrviz.h)
QT5_WRAP_CPP(QT_MOC_HPP ${QT_MOC})

## Here we specify the list of source files, including the output of
## the previous command which is stored in ``${MOC_FILES}``.
file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS src/*.cpp)

## Add the "myrviz" executable and specify the list of source files we
## collected above in ``${SOURCE_FILES}``.
add_executable(myrviz ${QT_SOURCES} ${QT_MOC_HPP})


## Link the myrviz executable with whatever Qt libraries have been defined by
## the ``find_package(Qt4 ...)`` line above, and with whatever libraries
## catkin has included.
target_link_libraries(myrviz ${QT_LIBRARIES} ${catkin_LIBRARIES})
## END_TUTORIAL

## Install
install(TARGETS myrviz RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

５）编译运行功能包

  运行结果如下：

  本文仅仅实现了PointCloud2类型数据的可视化，其他类型的点云数据可以根据此模块进行扩展。