diff --git a/include/cyrobot_monitor/main_window.hpp b/include/cyrobot_monitor/main_window.hpp index 117c225..e8857c3 100644 --- a/include/cyrobot_monitor/main_window.hpp +++ b/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; diff --git a/include/cyrobot_monitor/qrviz.hpp b/include/cyrobot_monitor/qrviz.hpp new file mode 100644 index 0000000..9d8e2f2 --- /dev/null +++ b/include/cyrobot_monitor/qrviz.hpp @@ -0,0 +1,79 @@ +#ifndef QRVIZ_H +#define QRVIZ_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "rviz/image/ros_image_texture.h" +#include +#include +#include +#include +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 diff --git a/include/cyrobot_monitor/robomap.h b/include/cyrobot_monitor/robomap.h index 4fc8998..2803731 100644 --- a/include/cyrobot_monitor/robomap.h +++ b/include/cyrobot_monitor/robomap.h @@ -12,7 +12,6 @@ #include #include #include -#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"; } diff --git a/src/main_window.cpp b/src/main_window.cpp index 9f29e2b..3661bfe 100644 --- a/src/main_window.cpp +++ b/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)); diff --git a/src/qrviz.cpp b/src/qrviz.cpp new file mode 100644 index 0000000..bdd8290 --- /dev/null +++ b/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()<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:"<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:"<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() +{ + +} diff --git a/ui/main_window.ui b/ui/main_window.ui index cc5d7e5..b87472e 100644 --- a/ui/main_window.ui +++ b/ui/main_window.ui @@ -6,8 +6,8 @@ 0 0 - 1248 - 638 + 1266 + 656 @@ -30,8 +30,26 @@ padding:0; margin:0 + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + @@ -867,7 +885,7 @@ QPushButton:pressed{border-image: url(://images/down_right_2.png);} - 0 + 1 @@ -1023,9 +1041,9 @@ background-color: rgb(255,255,255); - + - + @@ -1133,7 +1151,10 @@ background-color: rgb(255,255,255); - + + + background-color:none + 经典 @@ -1150,6 +1171,9 @@ background-color: rgb(255,255,255); + + 0 + @@ -1166,98 +1190,98 @@ background-color: rgb(255,255,255); + + + + - - - - - - 40 - 0 - - - - - 40 - 16777215 - - - - map: - - - - - - - - 200 - 0 - - - - - 200 - 16777215 - - - - - - - - - - - - 40 - 0 - - - - - 40 - 16777215 - - - - scene: - - - - - - - - 200 - 0 - - - - - 200 - 16777215 - - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - + + + + 40 + 0 + + + + + 40 + 16777215 + + + + map: + + + + + + + + 200 + 0 + + + + + 200 + 16777215 + + + + + + + + + + + + 40 + 0 + + + + + 40 + 16777215 + + + + scene: + + + + + + + + 200 + 0 + + + + + 200 + 16777215 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + @@ -1505,6 +1529,9 @@ background-color:rgb(211, 215, 207); } + + 0 + 0