添加rviz界面

include/cyrobot_monitor/main_window.hpp

@@ -16,6 +16,7 @@
 #include "ui_main_window.h"
 #include "qnode.hpp"
 #include "settings.h"
+#include "qrviz.hpp"
 #include "joystick.h"
 #include "robomap.h"
 #include "QProcess"
@@ -109,6 +110,7 @@ public slots:
     void slot_chartTimerTimeout();
     void slot_pubImageMapTimeOut();
     void slot_updateCursorPos(QPointF pos);
+    void slot_changeMapType(int);
 //    void on_horizontalSlider_raw_valueChanged(int value);
 private slots:
 
@@ -147,6 +149,7 @@ private:
     JoyStick *rock_widget;
     QGraphicsScene  *m_qgraphicsScene=NULL;
     roboMap *m_roboMap=NULL;
+    QRviz *map_rviz=NULL;
     QVariantList m_sendVelList,m_recvVelList,m_timeList;
     //曲线
     QSplineSeries* line;

include/cyrobot_monitor/qrviz.hpp

@@ -0,0 +1,79 @@
+#ifndef QRVIZ_H
+#define QRVIZ_H
+
+#include <QVBoxLayout>
+#include <rviz/visualization_manager.h>
+#include <rviz/render_panel.h>
+#include <rviz/display.h>
+#include <rviz/tool_manager.h>
+#include <rviz/visualization_manager.h>
+#include <rviz/render_panel.h>
+#include <rviz/display.h>
+#include <rviz/tool.h>
+#include "rviz/image/ros_image_texture.h"
+#include<rviz/tool_manager.h>
+#include <QThread>
+#include <QDebug>
+#include <QException>
+class QRviz:public QThread
+{
+    Q_OBJECT
+public:
+    QRviz(QVBoxLayout *layout,QString node_name);
+    void run();
+    void createDisplay(QString display_name,QString topic_name);
+    //显示Grid
+    void Display_Grid(bool enable,QString Reference_frame,int Plan_Cell_count,QColor color=QColor(125,125,125));
+    //显示map
+    void Display_Map(bool enable,QString topic,double Alpha,QString Color_Scheme);
+    //设置全局显示属性
+    void SetGlobalOptions(QString frame_name,QColor backColor,int frame_rate);
+    //显示激光雷达点云
+    void Display_LaserScan(bool enable,QString topic);
+    //显示导航相关控件
+    void Display_Navigate(bool enable,QString Global_topic,QString Global_planner,QString Local_topic,QString Local_planner);
+    //显示tf坐标变换
+    void Display_TF(bool enable);
+    void Set_Pos();
+    void Set_Goal();
+    void Set_MoveCamera();
+    void Set_Select();
+    //发布goal话题的坐标
+    void Send_Goal_topic();
+    void show();
+    void hide();
+    //显示robotmodel
+    void Display_RobotModel(bool enable);
+private:
+    //rviz显示容器
+    rviz::RenderPanel *render_panel_;
+    rviz::VisualizationManager *manager_;
+    rviz::Display* grid_=NULL ;
+
+    //显示tf坐标变换
+    rviz::Display* TF_=NULL ;
+    rviz::Display* map_=NULL ;
+    rviz::Display* laser_=NULL ;
+    rviz::Display* Navigate_localmap=NULL;
+    rviz::Display* Navigate_localplanner=NULL;
+    rviz::Display* Navigate_globalmap=NULL;
+    rviz::Display* Navigate_globalplanner=NULL;
+    rviz::Display* Navigate_amcl=NULL;
+
+
+
+    //rviz工具
+    rviz::Tool *current_tool;
+    //rviz工具控制器
+    rviz::ToolManager *tool_manager_;
+    QVBoxLayout *layout;
+    QString nodename;
+private slots:
+    void addTool( rviz::Tool* );
+
+ //   rviz::VisualizationManager *manager_=NULL;
+//    rviz::RenderPanel *render_panel_;
+
+};
+
+#endif // QRVIZ_H

include/cyrobot_monitor/robomap.h

@@ -12,7 +12,6 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <QCursor>
 #include <QtMath>
-#define PI 3.1415926
 class roboMap  : public QObject, public QGraphicsItem
 {
     Q_OBJECT
@@ -40,6 +39,7 @@ public:
   double m_roboR=5;
   double map_size=1;
   double defaultScale=2;
+  double PI=3.1415926;
   void get_version(){
       qDebug()<<"1.0.0";
   }

src/main_window.cpp

@@ -160,6 +160,7 @@ void MainWindow::connections()
     QObject::connect(m_timerCurrentTime,&QTimer::timeout,[=](){
         ui.label_time->setText(QDateTime::currentDateTime().toString("  hh:mm:ss  "));
     });
+    QObject::connect(ui.comboBox_mapType,SIGNAL(currentIndexChanged(int)),this,SLOT(slot_changeMapType(int)));
     //connect速度的信号
     connect(&qnode,SIGNAL(speed_x(double)),this,SLOT(slot_speed_x(double)));
     connect(&qnode,SIGNAL(speed_y(double)),this,SLOT(slot_speed_yaw(double)));
@@ -209,6 +210,23 @@ void MainWindow::connections()
     connect(this,SIGNAL(signalSetMoveCamera()),m_roboMap,SLOT(slot_setMoveCamera()));
 //    connect(ui.stackedWidget_2,SIGNAL())
 }
+void MainWindow::slot_changeMapType(int index){
+  switch (index){
+     case 0:
+       ui.mapViz->show();
+       if(map_rviz!=NULL){
+         map_rviz->hide();
+       }
+       break;
+     case 1:
+       ui.mapViz->hide();
+       if(map_rviz==NULL){
+         map_rviz=new QRviz(ui.verticalLayout_build_map,"qrviz");
+       }
+       map_rviz->show();
+       break;
+  }
+}
 void MainWindow::slot_updateCursorPos(QPointF pos){
       QPointF mapPos=qnode.transScenePoint2Map(pos);
       ui.label_pos_map->setText("x: "+QString::number(mapPos.x()).mid(0,4)+"  y: "+QString::number(mapPos.y()).mid(0,4));

src/qrviz.cpp

@@ -0,0 +1,235 @@
+#include "../include/cyrobot_monitor/qrviz.hpp"
+
+QRviz::QRviz(QVBoxLayout *layout,QString node_name)
+{
+
+    this->layout=layout;
+    this->nodename=node_name;
+
+
+    //创建rviz容器
+    render_panel_=new rviz::RenderPanel;
+    qDebug()<<layout->sizeHint();
+    render_panel_->resize(layout->sizeHint());
+    render_panel_->setStyleSheet("border:solid 3px red");
+    //向layout添加widget
+    layout->addWidget(render_panel_);
+    //初始化rviz控制对象
+    manager_=new rviz::VisualizationManager(render_panel_);
+    ROS_ASSERT(manager_!=NULL);
+    //获取当前rviz控制对象的 tool控制对象
+    tool_manager_=manager_->getToolManager();
+    ROS_ASSERT(tool_manager_!=NULL);
+   //初始化camera 这行代码实现放大 缩小 平移等操作
+    render_panel_->initialize(manager_->getSceneManager(),manager_);
+    manager_->initialize();
+    tool_manager_->initialize();
+    manager_->removeAllDisplays();
+
+}
+void QRviz::show(){
+  render_panel_->show();
+}
+void QRviz::hide(){
+  render_panel_->hide();
+}
+rviz::Display* RobotModel_=NULL;
+//显示robotModel
+  void QRviz::Display_RobotModel(bool enable)
+  {
+
+      if(RobotModel_==NULL)
+      {
+          RobotModel_=manager_->createDisplay("rviz/RobotModel","Qrviz RobotModel",enable);
+      }
+      else{
+          delete RobotModel_;
+          RobotModel_=manager_->createDisplay("rviz/RobotModel","Qrviz RobotModel",enable);
+      }
+  }
+//显示grid
+void QRviz::Display_Grid(bool enable,QString Reference_frame,int Plan_Cell_count,QColor color)
+{
+    if(grid_==NULL)
+    {
+        grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true );
+        ROS_ASSERT( grid_ != NULL );
+        // Configure the GridDisplay the way we like it.
+        grid_->subProp( "Line Style" )->setValue("Billboards");
+        grid_->subProp( "Color" )->setValue(color);
+        grid_->subProp( "Reference Frame" )->setValue(Reference_frame);
+        grid_->subProp("Plane Cell Count")->setValue(Plan_Cell_count);
+
+    }
+    else{
+        delete grid_;
+        grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true );
+        ROS_ASSERT( grid_ != NULL );
+        // Configure the GridDisplay the way we like it.
+        grid_->subProp( "Line Style" )->setValue("Billboards");
+        grid_->subProp( "Color" )->setValue(color);
+        grid_->subProp( "Reference Frame" )->setValue(Reference_frame);
+        grid_->subProp("Plane Cell Count")->setValue(Plan_Cell_count);
+    }
+    grid_->setEnabled(enable);
+    manager_->startUpdate();
+}
+//显示map
+void QRviz::Display_Map(bool enable,QString topic,double Alpha,QString Color_Scheme)
+{
+    if(!enable&&map_)
+    {
+        map_->setEnabled(false);
+        return ;
+    }
+    if(map_==NULL)
+    {
+        map_=manager_->createDisplay("rviz/Map","QMap",true);
+        ROS_ASSERT(map_);
+        map_->subProp("Topic")->setValue(topic);
+        map_->subProp("Alpha")->setValue(Alpha);
+        map_->subProp("Color Scheme")->setValue(Color_Scheme);
+
+    }
+    else{
+         ROS_ASSERT(map_);
+         qDebug()<<"asdasdasd:"<<topic<<Alpha;
+
+        delete map_;
+        map_=manager_->createDisplay("rviz/Map","QMap",true);
+        ROS_ASSERT(map_);
+        map_->subProp("Topic")->setValue(topic);
+        map_->subProp("Alpha")->setValue(Alpha);
+        map_->subProp("Color Scheme")->setValue(Color_Scheme);
+    }
+
+    map_->setEnabled(enable);
+    manager_->startUpdate();
+}
+//显示激光雷达
+void QRviz::Display_LaserScan(bool enable,QString topic)
+{
+    if(laser_==NULL)
+    {
+        laser_=manager_->createDisplay("rviz/LaserScan","QLaser",enable);
+        ROS_ASSERT(laser_);
+        laser_->subProp("Topic")->setValue(topic);
+    }
+    else{
+        delete laser_;
+        laser_=manager_->createDisplay("rviz/LaserScan","QLaser",enable);
+        ROS_ASSERT(laser_);
+        laser_->subProp("Topic")->setValue(topic);
+    }
+    qDebug()<<"topic:"<<topic;
+    laser_->setEnabled(enable);
+    manager_->startUpdate();
+}
+//设置全局显示
+ void QRviz::SetGlobalOptions(QString frame_name,QColor backColor,int frame_rate)
+ {
+     manager_->setFixedFrame(frame_name);
+     manager_->setProperty("Background Color",backColor);
+     manager_->setProperty("Frame Rate",frame_rate);
+     manager_->startUpdate();
+ }
+
+// "rviz/MoveCamera";
+// "rviz/Interact";
+// "rviz/Select";
+// "rviz/SetInitialPose";
+// "rviz/SetGoal";
+ //设置机器人导航初始位置
+ void QRviz::Set_Pos()
+ {
+     //获取设置Pos的工具
+     //添加工具
+
+     current_tool= tool_manager_->addTool("rviz/SetInitialPose");
+     //设置当前使用的工具为SetInitialPose（实现在地图上标点）
+     tool_manager_->setCurrentTool( current_tool );
+     manager_->startUpdate();
+
+//     tool_manager_->setCurrentTool()
+
+ }
+ //设置机器人导航目标点
+ void QRviz::Set_Goal()
+ {
+     //添加工具
+     current_tool= tool_manager_->addTool("rviz/SetGoal");
+     //设置goal的话题
+     rviz::Property* pro= current_tool->getPropertyContainer();
+     pro->subProp("Topic")->setValue("/move_base_simple/goal");
+     //设置当前frame
+     manager_->setFixedFrame("map");
+     //设置当前使用的工具为SetGoal（实现在地图上标点）
+     tool_manager_->setCurrentTool( current_tool );
+
+     manager_->startUpdate();
+
+ }
+ void QRviz::Set_MoveCamera()
+ {
+     //获取设置Pos的工具
+     //添加工具
+
+     current_tool= tool_manager_->addTool("rviz/MoveCamera");
+     //设置当前使用的工具为SetInitialPose（实现在地图上标点）
+     tool_manager_->setCurrentTool( current_tool );
+     manager_->startUpdate();
+ }
+ void QRviz::Set_Select()
+ {
+     //获取设置Pos的工具
+     //添加工具
+
+     current_tool= tool_manager_->addTool("rviz/Select");
+     //设置当前使用的工具为SetInitialPose（实现在地图上标点）
+     tool_manager_->setCurrentTool( current_tool );
+     manager_->startUpdate();
+ }
+ //显示tf坐标变换
+ void QRviz::Display_TF(bool enable)
+ {
+     if(TF_){delete TF_;TF_=NULL;}
+     TF_=manager_->createDisplay("rviz/TF","QTF",enable);
+ }
+ //显示导航相关
+ void QRviz::Display_Navigate(bool enable,QString Global_topic,QString Global_planner,QString Local_topic,QString Local_planner)
+ {
+    if(Navigate_localmap) {delete Navigate_localmap; Navigate_localmap=NULL;}
+    if(Navigate_localplanner) {delete Navigate_localplanner; Navigate_localplanner=NULL;}
+    if(Navigate_globalmap) {delete Navigate_globalmap; Navigate_globalmap=NULL;}
+    if(Navigate_globalplanner) {delete Navigate_globalplanner; Navigate_globalplanner=NULL;}
+    //local map
+    Navigate_localmap=manager_->createDisplay("rviz/Map","Qlocalmap",enable);
+    Navigate_localmap->subProp("Topic")->setValue(Local_topic);
+    Navigate_localmap->subProp("Color Scheme")->setValue("costmap");
+    Navigate_localplanner=manager_->createDisplay("rviz/Path","QlocalPath",enable);
+    Navigate_localplanner->subProp("Topic")->setValue(Local_planner);
+    Navigate_localplanner->subProp("Color")->setValue(QColor(0,12,255));
+    //global map
+    Navigate_globalmap=manager_->createDisplay("rviz/Map","QGlobalmap",enable);
+    Navigate_globalmap->subProp("Topic")->setValue(Global_topic);
+    Navigate_globalmap->subProp("Color Scheme")->setValue("costmap");
+    Navigate_globalplanner=manager_->createDisplay("rviz/Path","QGlobalpath",enable);
+    Navigate_globalplanner->subProp("Topic")->setValue(Global_planner);
+    Navigate_globalplanner->subProp("Color")->setValue(QColor(255,0,0));
+    //更新画面显示
+    manager_->startUpdate();
+
+ }
+ void QRviz::addTool( rviz::Tool* )
+ {
+
+ }
+void QRviz::createDisplay(QString display_name,QString topic_name)
+{
+
+
+}
+void QRviz::run()
+{
+
+}

ui/main_window.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1248</width>
-    <height>638</height>
+    <width>1266</width>
+    <height>656</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -30,8 +30,26 @@ padding:0;
     <string notr="true">margin:0</string>
    </property>
    <layout class="QVBoxLayout" name="verticalLayout_10">
+    <property name="spacing">
+     <number>0</number>
+    </property>
+    <property name="leftMargin">
+     <number>0</number>
+    </property>
+    <property name="topMargin">
+     <number>0</number>
+    </property>
+    <property name="rightMargin">
+     <number>0</number>
+    </property>
+    <property name="bottomMargin">
+     <number>0</number>
+    </property>
     <item>
      <layout class="QVBoxLayout" name="verticalLayout_6">
+      <property name="spacing">
+       <number>0</number>
+      </property>
       <item>
        <layout class="QHBoxLayout" name="horizontalLayout_4">
         <item>
@@ -867,7 +885,7 @@ QPushButton:pressed{border-image: url(://images/down_right_2.png);}</string>
           <item>
            <widget class="QStackedWidget" name="stackedWidget_main">
             <property name="currentIndex">
-             <number>0</number>
+             <number>1</number>
             </property>
             <widget class="QWidget" name="page_3">
              <layout class="QVBoxLayout" name="verticalLayout_12">
@@ -1023,9 +1041,9 @@ background-color: rgb(255,255,255);
              </layout>
             </widget>
             <widget class="QWidget" name="page_4">
-             <layout class="QVBoxLayout" name="verticalLayout_13">
+             <layout class="QVBoxLayout" name="verticalLayout_14">
               <item>
-               <layout class="QVBoxLayout" name="verticalLayout_14">
+               <layout class="QVBoxLayout" name="verticalLayout_13">
                 <item>
                  <layout class="QHBoxLayout" name="horizontalLayout_11">
                   <item>
@@ -1133,7 +1151,10 @@ background-color: rgb(255,255,255);
                    </widget>
                   </item>
                   <item>
-                   <widget class="QComboBox" name="comboBox">
+                   <widget class="QComboBox" name="comboBox_mapType">
+                    <property name="styleSheet">
+                     <string notr="true">background-color:none</string>
+                    </property>
                     <item>
                      <property name="text">
                       <string>经典</string>
@@ -1150,6 +1171,9 @@ background-color: rgb(255,255,255);
                 </item>
                 <item>
                  <layout class="QVBoxLayout" name="verticalLayout_build_map">
+                  <property name="spacing">
+                   <number>0</number>
+                  </property>
                   <item>
                    <widget class="QGraphicsView" name="mapViz">
                     <property name="layoutDirection">
@@ -1166,98 +1190,98 @@ background-color: rgb(255,255,255);
                     </property>
                    </widget>
                   </item>
+                 </layout>
+                </item>
+                <item>
+                 <layout class="QHBoxLayout" name="horizontalLayout_12">
                   <item>
-                   <layout class="QHBoxLayout" name="horizontalLayout_12">
-                    <item>
-                     <widget class="QLabel" name="label">
-                      <property name="minimumSize">
-                       <size>
-                        <width>40</width>
-                        <height>0</height>
-                       </size>
-                      </property>
-                      <property name="maximumSize">
-                       <size>
-                        <width>40</width>
-                        <height>16777215</height>
-                       </size>
-                      </property>
-                      <property name="text">
-                       <string>map:</string>
-                      </property>
-                     </widget>
-                    </item>
-                    <item>
-                     <widget class="QLabel" name="label_pos_map">
-                      <property name="minimumSize">
-                       <size>
-                        <width>200</width>
-                        <height>0</height>
-                       </size>
-                      </property>
-                      <property name="maximumSize">
-                       <size>
-                        <width>200</width>
-                        <height>16777215</height>
-                       </size>
-                      </property>
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item>
-                     <widget class="QLabel" name="label_5">
-                      <property name="minimumSize">
-                       <size>
-                        <width>40</width>
-                        <height>0</height>
-                       </size>
-                      </property>
-                      <property name="maximumSize">
-                       <size>
-                        <width>40</width>
-                        <height>16777215</height>
-                       </size>
-                      </property>
-                      <property name="text">
-                       <string>scene:</string>
-                      </property>
-                     </widget>
-                    </item>
-                    <item>
-                     <widget class="QLabel" name="label_pos_scene">
-                      <property name="minimumSize">
-                       <size>
-                        <width>200</width>
-                        <height>0</height>
-                       </size>
-                      </property>
-                      <property name="maximumSize">
-                       <size>
-                        <width>200</width>
-                        <height>16777215</height>
-                       </size>
-                      </property>
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item>
-                     <spacer name="horizontalSpacer_3">
-                      <property name="orientation">
-                       <enum>Qt::Horizontal</enum>
-                      </property>
-                      <property name="sizeHint" stdset="0">
-                       <size>
-                        <width>40</width>
-                        <height>20</height>
-                       </size>
-                      </property>
-                     </spacer>
-                    </item>
-                   </layout>
+                   <widget class="QLabel" name="label">
+                    <property name="minimumSize">
+                     <size>
+                      <width>40</width>
+                      <height>0</height>
+                     </size>
+                    </property>
+                    <property name="maximumSize">
+                     <size>
+                      <width>40</width>
+                      <height>16777215</height>
+                     </size>
+                    </property>
+                    <property name="text">
+                     <string>map:</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLabel" name="label_pos_map">
+                    <property name="minimumSize">
+                     <size>
+                      <width>200</width>
+                      <height>0</height>
+                     </size>
+                    </property>
+                    <property name="maximumSize">
+                     <size>
+                      <width>200</width>
+                      <height>16777215</height>
+                     </size>
+                    </property>
+                    <property name="text">
+                     <string/>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLabel" name="label_5">
+                    <property name="minimumSize">
+                     <size>
+                      <width>40</width>
+                      <height>0</height>
+                     </size>
+                    </property>
+                    <property name="maximumSize">
+                     <size>
+                      <width>40</width>
+                      <height>16777215</height>
+                     </size>
+                    </property>
+                    <property name="text">
+                     <string>scene:</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <widget class="QLabel" name="label_pos_scene">
+                    <property name="minimumSize">
+                     <size>
+                      <width>200</width>
+                      <height>0</height>
+                     </size>
+                    </property>
+                    <property name="maximumSize">
+                     <size>
+                      <width>200</width>
+                      <height>16777215</height>
+                     </size>
+                    </property>
+                    <property name="text">
+                     <string/>
+                    </property>
+                   </widget>
+                  </item>
+                  <item>
+                   <spacer name="horizontalSpacer_3">
+                    <property name="orientation">
+                     <enum>Qt::Horizontal</enum>
+                    </property>
+                    <property name="sizeHint" stdset="0">
+                     <size>
+                      <width>40</width>
+                      <height>20</height>
+                     </size>
+                    </property>
+                   </spacer>
                   </item>
                  </layout>
                 </item>
@@ -1505,6 +1529,9 @@ background-color:rgb(211, 215, 207);
 }</string>
         </property>
         <layout class="QHBoxLayout" name="horizontalLayout_24">
+         <property name="spacing">
+          <number>0</number>
+         </property>
          <property name="leftMargin">
           <number>0</number>
          </property>