背景:想要在QT工程中加载Rviz组件,显示点云等相关,但是目前查了很多资料,都没有找到QT的proj工程加载Rviz,都是基于ROS框架加上Rviz包的,因此本文就简要的介绍QT工程加载Rviz。

实现:当前是基于QWidget的框架加载,因为网上说Rviz组件的中有实际上widget组件的容器可以直接显示。后续再研究下qml怎么加载显示。

(1)创建一个QWidget的工程

(2)在pro工程文件中增加ros库跟头文件

#ros
INCLUDEPATH += /opt/ros/kinetic/include
DEPENDPATH += /opt/ros/kinetic/include

LIBS += -L/opt/ros/kinetic/lib -lroscpp -lroslib -lrosconsole -lroscpp_serialization -lrostime -lrviz

INCLUDEPATH += /usr/include/OGRE
LIBS += -lboost_system -ljsoncpp -lboost_thread -lboost_filesystem -lboost_regex -lconsole_bridge

(3)在Main文件中初始化ros

#include "widget.h"
#include <QApplication>

int main(int argc, char *argv[])
{
    QApplication a(argc, argv);
    ros::init(argc, argv, "test_rviz");

    ros::NodeHandle nh_;
    Widget w;
    w.show();
    return a.exec();
}

(4)在Widget控件中调用Rviz库的相关接口

#ifndef WIDGET_H
#define WIDGET_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>
#include<rviz/tool.h>
#include <rviz/default_plugin/view_controllers/orbit_view_controller.h>
#include <rviz/view_manager.h>
//#include <rviz_visual_tools/rviz_visual_tools.h>

class Widget : public QWidget
{
    Q_OBJECT

public:
    Widget(QWidget *parent = nullptr);
    ~Widget();
};
#endif // WIDGET_H
#include "widget.h"
#include <QVBoxLayout>

Widget::Widget(QWidget *parent)
    : QWidget(parent)
{

    //创建rviz容器,其本质是一个QWidget类型
    rviz::RenderPanel *render_panel_=new rviz::RenderPanel;

    //设置布局
    QVBoxLayout *mainLayout= new QVBoxLayout(this);
    mainLayout->addWidget(render_panel_);
    //初始化rviz控制对象
    rviz::VisualizationManager* manager_=new rviz::VisualizationManager(render_panel_);
    //初始化camera 这行代码实现放大 缩小 平移等操作
    render_panel_->initialize(manager_->getSceneManager(),manager_);

    //显示
    manager_->initialize();
    manager_->removeAllDisplays();
    manager_->startUpdate();

    //设置rviz左测的参数设置
    manager_->setFixedFrame("/vehicle_link");

    //创建一个类型为rviz/PointCloud2的图层,用于接收topic为points_map的点云数据,就是我最终底图的图层
    rviz::Display *map_=manager_->createDisplay("rviz/PointCloud2","pointCloud2",true);

    map_->subProp("Topic")->setValue("/os_cloud_node/points");
    map_->subProp("Invert Rainbow")->setValue("true");
    map_->subProp("Style")->setValue("Flat Squares");
    map_->subProp("Size (m)")->setValue("0.1");
    map_->subProp("Color Transformer")->setValue("FlatColor");
    map_->subProp("Queue Size")->setValue("10");
    map_->subProp("Alpha")->setValue("1");


}

Widget::~Widget()
{
}

 其中上述代码中设置左侧的参数是根据原Rviz参数属性设置来的:

 

(5)程序运行的效果为(有点云产生时):

Logo

权威|前沿|技术|干货|国内首个API全生命周期开发者社区

更多推荐