将地图数据通过图像话题/image/map发布出去

include/cyrobot_monitor/main_window.hpp

@@ -59,7 +59,7 @@ public:
     void initUis();
     void initVideos();
     void initTopicList();
-    void initCharts();
+    void initOthers();
 public slots:
     /******************************************
 	** Auto-connections (connectSlotsByName())
@@ -107,6 +107,7 @@ public slots:
     void slot_minWindows();
     void slot_maxWindows();
     void slot_chartTimerTimeout();
+    void slot_pubImageMapTimeOut();
     void slot_updateCursorPos(QPointF pos);
 //    void on_horizontalSlider_raw_valueChanged(int value);
 private slots:
@@ -158,6 +159,7 @@ private:
     QQueue<QPointF> data1;
     QQueue<QPointF> data2;
     QTimer *m_timerChart;
+    QTimer *m_timerPubImageMap;
     QChartView *chartView;
     DashBoard *speedDashBoard;
 };

include/cyrobot_monitor/qnode.hpp

@@ -66,9 +66,12 @@ public:
   void move_base(char k,float speed_linear,float speed_trun);
   void set_goal(QString frame,double x,double y,double z,double w);
   void Sub_Image(QString topic,int frame_id);
+  void pub_imageMap(QImage map);
   QPointF transScenePoint2Map(QPointF pos);
   QPointF transMapPoint2Scene(QPointF pos);
   QMap<QString,QString> get_topic_list();
+  int mapWidth{0};
+  int mapHeight{0};
 	void run();
 
 	/*********************
@@ -110,6 +113,7 @@ private:
     ros::Subscriber m_plannerPathSub;
     ros::Publisher goal_pub;
     ros::Publisher cmd_pub;
+    image_transport::Publisher m_imageMapPub;
     QStringListModel logging_model;
     //图像订阅
     image_transport::Subscriber image_sub0;
@@ -126,8 +130,6 @@ private:
     QPolygon mapPonits;
     QPolygonF plannerPoints;
     QPolygonF laserPoints;
-    int mapWidth;
-    int mapHeight;
     //地图 0 0点坐标对应世界坐标系的坐标
     int m_mapOriginX;
     int m_mapOriginY;
@@ -142,6 +144,7 @@ private:
     //tf::TransformListener m_tfListener(ros::Duration(10));
     //ros::Timer m_rosTimer;
     QImage Mat2QImage(cv::Mat const& src);
+    cv::Mat QImage2Mat(QImage &image);
     cv::Mat RotaMap(cv::Mat const& map);
     void poseCallback(const geometry_msgs::PoseWithCovarianceStamped& pos);
     void speedCallback(const nav_msgs::Odometry::ConstPtr& msg);

src/main_window.cpp

@@ -112,6 +112,7 @@ void MainWindow::initUis()
 //    m_roboMap->setPos(100,100);
     //widget添加视图
     ui.mapViz->setScene(m_qgraphicsScene);
+
     //ui.speed_webView->load(QUrl("file://"+qApp->applicationDirPath()+"/html/gauge-stage.html"));
     ui.horizontalLayout_4->setSpacing(0);
     ui.horizontalLayout_4->setMargin(0);
@@ -126,7 +127,6 @@ void MainWindow::initUis()
     ui.btn_other->setIcon(QIcon("://images/toolbar_other.png"));
     rock_widget =new JoyStick(ui.JoyStick_widget);
     rock_widget->show();
-    initCharts();
     //dashboard
     speedDashBoard =new DashBoard(ui.widget_dashboard);
 }
@@ -211,7 +211,7 @@ void MainWindow::slot_hide_table_widget(){
     ui.table_hide_btn->setStyleSheet("QPushButton{background-image: url(://images/show.png);border:none;}");
   }
 }
-void MainWindow::initCharts(){
+void MainWindow::initOthers(){
   line = new QSplineSeries(this);
     chart = new QChart();
     chart->addSeries(line);
@@ -223,8 +223,12 @@ void MainWindow::initCharts(){
    chartView->setFixedHeight(ui.widget_chart->height());
   chartView->setRenderHint(QPainter::Antialiasing);
   m_timerChart=new QTimer;
+  m_timerPubImageMap=new QTimer;
+  m_timerPubImageMap->setInterval(100);
   m_timerChart->setInterval(100);
   connect(m_timerChart,SIGNAL(timeout()),this,SLOT(slot_chartTimerTimeout()));
+  connect(m_timerPubImageMap,SIGNAL(timeout()),this,SLOT(slot_pubImageMapTimeOut()));
+  m_timerPubImageMap->start();
   m_timerChart->start();
 }
 void MainWindow::slot_chartTimerTimeout(){
@@ -274,6 +278,13 @@ void MainWindow::slot_chartTimerTimeout(){
 
      chartView->setChart(chart);
 }
+void MainWindow::slot_pubImageMapTimeOut(){
+ QImage image(600,600,QImage::Format_RGB888);
+ QPainter painter(&image);
+ painter.setRenderHint(QPainter::Antialiasing);
+ m_qgraphicsScene->render(&painter);
+ qnode.pub_imageMap(image);
+}
 //设置界面
 void MainWindow::slot_setting_frame()
 {
@@ -612,6 +623,7 @@ void MainWindow::on_button_connect_clicked(bool check ) {
              initVideos();
              //显示话题列表
              initTopicList();
+             initOthers();
 		}
     }
     //如果不使用环境变量
@@ -632,6 +644,7 @@ void MainWindow::on_button_connect_clicked(bool check ) {
            initVideos();
            //显示话题列表
            initTopicList();
+           initOthers();
 		}
 	}
 

src/qnode.cpp

@@ -96,6 +96,8 @@ void QNode::SubAndPubTopic(){
    //m_rosTimer=n.createTimer(ros::Duration(1.0),boost::bind(&QNode::transformPoint,boost::ref(m_tfListener)));
    //全局规划Path
    m_plannerPathSub=n.subscribe("/move_base/NavfnROS/plan",1000,&QNode::plannerPathCallback,this);
+   image_transport::ImageTransport it(n);
+   m_imageMapPub = it.advertise("image/map",10);
 }
 
 void QNode::transformPoint(const tf::TransformListener& listener){
@@ -254,8 +256,6 @@ void QNode::mapCallback(nav_msgs::OccupancyGrid::ConstPtr map){
       //沿x轴翻转地图
       cv::Mat rotaedMap=RotaMap(image);
       QImage imageMap=Mat2QImage(rotaedMap);
-//      QImage imageMap=Mat2QImage(image);
-      //imageMap.save("/home/chengyangkj/map.png","png",100);
       emit updateMap(imageMap);
       //计算map坐标系地图中心点坐标
       //scene(0,0) ^
@@ -365,6 +365,11 @@ QPointF QNode::transMapPoint2Scene(QPointF pos){
   roboPos.setX((pos.x()-m_mapCenterPoint.x())/m_mapResolution+m_sceneCenterPoint.x());
   roboPos.setY(-1*(pos.y()-m_mapCenterPoint.y())/m_mapResolution+m_sceneCenterPoint.y());
   return roboPos;
+}
+void QNode::pub_imageMap(QImage map){
+     cv::Mat image = QImage2Mat(map);
+     sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
+     m_imageMapPub.publish(msg);
 }
  //图像话题的回调函数
  void QNode::imageCallback0(const sensor_msgs::ImageConstPtr& msg)
@@ -440,6 +445,26 @@ QPointF QNode::transMapPoint2Scene(QPointF pos){
 
    return dest;
  }
+ cv::Mat QNode::QImage2Mat(QImage &image)
+ {
+   cv::Mat mat;
+      switch(image.format())
+      {
+      case QImage::Format_ARGB32:
+      case QImage::Format_RGB32:
+      case QImage::Format_ARGB32_Premultiplied:
+          mat = cv::Mat(image.height(), image.width(), CV_8UC4, (void*)image.constBits(), image.bytesPerLine());
+          break;
+      case QImage::Format_RGB888:
+          mat = cv::Mat(image.height(), image.width(), CV_8UC3, (void*)image.constBits(), image.bytesPerLine());
+          cv::cvtColor(mat, mat, CV_BGR2RGB);
+          break;
+      case QImage::Format_Indexed8:
+          mat = cv::Mat(image.height(), image.width(), CV_8UC1, (void*)image.constBits(), image.bytesPerLine());
+          break;
+      }
+      return mat;
+ }
 void QNode::log( const LogLevel &level, const std::string &msg) {
 	logging_model.insertRows(logging_model.rowCount(),1);
 	std::stringstream logging_model_msg;

src/robomap.cpp

@@ -48,6 +48,7 @@ void roboMap::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, Q
   drawPlannerPath(painter);
   drawLaserScan(painter);
   drawTools(painter);
+
 }
 void roboMap::drawTools(QPainter *painter){
   if(currCursor == set2DPoseCursor){