src/include/qnode.hpp

@@ -20,7 +20,6 @@
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
 #include <geometry_msgs/Twist.h>
-#include <sensor_msgs/BatteryState.h>
 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/Float32.h>
@@ -38,16 +37,14 @@ public:
     void KeyboardMove(char key, float speed_linear, float speed_trun);
     void AutoMove(float speed_linear, float speed_turn,
                   float target_linear, float target_turn);
-    //void subImage();
     QImage image;
 
-
 Q_SIGNALS:
     void speed_x(double x);
     void speed_th(double th);
-    //void power(float p);
+    void power(float p);
-    void batteryState(sensor_msgs::BatteryState);
+
-    void show_image(QImage image);//发出设置相机图片信号
+    void loggingCamera();//发出设置相机图片信号
 
 
 
@@ -59,26 +56,20 @@ private:
     ros::Publisher cmd_pub;
     ros::Publisher cmd_pub1;
     ros::Subscriber cmdVel_sub;
-    ros::Subscriber battery_sub;
+    ros::Subscriber power_sub;
-    ros::Subscriber image_sub0;
     QStringListModel logging_model;
-
+    image_transport::Subscriber image_sub;
     cv::Mat img;
     QString odom_topic;
     QString batteryState_topic;
 
-    // image subscribe
-    image_transport::Subscriber image_sub;
-
 
     void speedCallback(const nav_msgs::Odometry::ConstPtr& msg);
     void powerCallback(const std_msgs::Float32& message_holder);
     //void myCallback(const std_msgs::Float64& message_holder);
-    void batteryCallback(const sensor_msgs::BatteryState &message);
-    void imageCallback(const sensor_msgs::CompressedImageConstPtr &msg);//camera callback function
 
-    QImage  Mat2QImage(cv::Mat const &src);
+    void myCallback_img(const sensor_msgs::ImageConstPtr& msg);//camera callback function
-    cv::Mat QImage2Mat(QImage &image);
+
 
 };
 

src/main/main.cpp

@@ -24,6 +24,4 @@ int main(int argc, char** argv)
 
 /*
  * find . "(" -name "*.cpp" -or -name "*.h" -or -name "*.hpp" -or -name "*.qrc" ")" -print | xargs wc -l
- * rosrun rqt_graph rqt_graph
- *
  */

src/main/mainwindow.cpp

@@ -41,7 +41,123 @@ void MainWindow::initUis(){
 }
 
 
+void MainWindow::setBtnStyles(){
+    /*
+     * set PushButton state
+     */
+
+    ui->btn_main->setIcon(QIcon("://images/up.png"));
+    ui->btn_main->setText("mainWidget");
+    ui->btn_main->setStyleSheet(
+                "QPushButton:hover{"
+                "background-color:rgb(186, 189, 182);"
+                "border-bottom:2px solid rgb(67, 154, 246);}"
+                "QPushButton:checked{"
+                "background-color:cyan;"
+                "border-bottom:2px solid white}"
+                "QPushButton:pressed{"
+                "background-color:rgb(67, 154, 246)}"
+                "QPushButton{"
+                "background-color:rgb(238, 238, 236);"
+                "border:none;"
+                "padding:0px 0px 0px 0px;"
+                "margin:0px 0px 0px 0px;}");
+
+    ui->btn_uav->setStyleSheet(
+                "QPushButton:hover{"
+                "background-color:rgb(186, 189, 182);"
+                "border-bottom:2px solid rgb(67, 154, 246);}"
+                "QPushButton:checked{"
+                "background-color:cyan;"
+                "border-bottom:2px solid white}"
+                "QPushButton:pressed{"
+                "background-color:rgb(67, 154, 246)}"
+                "QPushButton{"
+                "background-color:rgb(238, 238, 236);"
+                "border:none;"
+                "padding:0px 0px 0px 0px;"
+                "margin:0px 0px 0px 0px;}");
+
+    ui->btn_ugv->setStyleSheet(
+                "QPushButton:hover{"
+                "background-color:rgb(186, 189, 182);"
+                "border-bottom:2px solid rgb(67, 154, 246);}"
+                "QPushButton:checked{"
+                "background-color:cyan;"
+                "border-bottom:2px solid white}"
+                "QPushButton:pressed{"
+                "background-color:rgb(67, 154, 246)}"
+                "QPushButton{"
+                "background-color:rgb(238, 238, 236);"
+                "border:none;"
+                "padding:0px 0px 0px 0px;"
+                "margin:0px 0px 0px 0px;}");
+
+    // 8 -> Forward: ↑
+    ui->pushButton_Forward->setShortcut(Qt::Key_8);
+    ui->pushButton_Forward->setStyleSheet(
+                "QPushButton{border-image: url(://images/up.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/up_2.png)}");
+    ui->pushButton_Forward->setFlat(true);
+
+    // 2 -> Back: ↓
+    ui->pushButton_Back->setShortcut(Qt::Key_2);
+    ui->pushButton_Back->setStyleSheet(
+                "QPushButton{border-image: url(://images/down.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/down_2.png)}");
+    ui->pushButton_Back->setFlat(true);
+
+    // 4 -> Left: ←
+    ui->pushButton_Left->setShortcut(Qt::Key_4);
+    ui->pushButton_Left->setStyleSheet(
+                "QPushButton{border-image: url(://images/left.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/left_2.png)}");
+    ui->pushButton_Left->setFlat(true);
+
+    // 6 -> Right: →
+    ui->pushButton_Right->setShortcut(Qt::Key_6);
+    ui->pushButton_Right->setStyleSheet(
+                "QPushButton{border-image: url(://images/right.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/right_2.png)}");
+    ui->pushButton_Right->setFlat(true);
+
+    // 7 -> Left_Forward: ↖
+    ui->pushButton_Left_Forward->setShortcut(Qt::Key_7);
+    ui->pushButton_Left_Forward->setStyleSheet(
+                "QPushButton{border-image: url(://images/up_left.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/up_left_2.png)}");
+    ui->pushButton_Left_Forward->setFlat(true);
+
+    // 9 -> Right_Forward: ↗
+    ui->pushButton_Right_Forward->setShortcut(Qt::Key_9);
+    ui->pushButton_Right_Forward->setStyleSheet(
+                "QPushButton{border-image: url(://images/up_right.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/up_right_2.png)}");
+    ui->pushButton_Right_Forward->setFlat(true);
+
+    // 1 -> Left_Back: ↙
+    ui->pushButton_Left_Back->setShortcut(Qt::Key_1);
+    ui->pushButton_Left_Back->setStyleSheet(
+                "QPushButton{border-image: url(://images/down_left.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/down_left_2.png)}");
+    ui->pushButton_Left_Back->setFlat(true);
 
+    // 3 -> Right_Back: ↘
+    ui->pushButton_Right_Back->setShortcut(Qt::Key_3);
+    ui->pushButton_Right_Back->setStyleSheet(
+                "QPushButton{border-image: url(://images/down_right.png)}"
+                "QPushButton{border:none}"
+                "QPushButton:pressed{border-image: url(://images/down_right_2.png)}");
+    ui->pushButton_Right_Back->setFlat(true);
+
+}
 
 
 void MainWindow::slot_keyboard_control(){
@@ -73,7 +189,6 @@ void MainWindow::slot_keyboard_control(){
     float turn = ui->horizontalSlider_raw->value() * 0.01;
     //bool is_rage_mode = ui->checkBox_rage_mode->isChecked();
     //std::cout<<liner<<" "<<turn<<std::endl;
-    //qnode.AutoMove(0.2,0,1,0);
     qnode.KeyboardMove(key, liner, turn);
 
 
@@ -113,26 +228,6 @@ void MainWindow::slot_rockKeyChange(int key){
 
 }
 
-void MainWindow::slot_batteryState(sensor_msgs::BatteryState msg) {
-    ui->label_power->setText("10V");
-  ui->label_power->setText(QString::number(msg.voltage).mid(0, 5) + "V");
-  double percentage = msg.percentage;
-  m_DashBoard_x->set_oil(percentage);
-  ui->progressBar->setValue(percentage > 100 ? 100 : percentage);
-  //当电量过低时发出提示
-  if (percentage <= 20) {
-    ui->progressBar->setStyleSheet(
-        "QProgressBar::chunk {background-color: red;width: 20px;} QProgressBar "
-        "{border: 2px solid grey;border-radius: 5px;text-align: center;}");
-    // QMessageBox::warning(NULL, "电量不足", "电量不足，请及时充电！",
-    // QMessageBox::Yes , QMessageBox::Yes);
-  } else {
-    ui->progressBar->setStyleSheet(
-        "QProgressBar {border: 2px solid grey;border-radius: 5px;text-align: "
-        "center;}");
-  }
-}
-
 void MainWindow::connections(){
     QObject::connect(ui->btn_main, &QPushButton::clicked, this, [=](){
         ui->stackedWidget_main->setCurrentIndex(0);
@@ -184,11 +279,7 @@ void MainWindow::connections(){
     connect(&qnode, SIGNAL(speed_x(double)), this, SLOT(slot_speed_x(double)));
     connect(&qnode, SIGNAL(speed_th(double)), this, SLOT(slot_speed_th(double)));
 
-    //电源的信号
+    QObject::connect(&qnode,SIGNAL(loggingCamera()),this,SLOT(updateLogcamera()));
-    connect(&qnode, SIGNAL(batteryState(sensor_msgs::BatteryState)), this,
-            SLOT(slot_batteryState(sensor_msgs::BatteryState)));
-
-    connect(&qnode, SIGNAL(show_image(QImage)), this, SLOT(slot_show_image(QImage)));
 }
 
 
@@ -234,7 +325,7 @@ void MainWindow::Slider_linear_valueChanged(int v) {
   ui->label_linear->setText(QString::number(v));
 }
 
-void MainWindow::slot_show_image(QImage image)
+void MainWindow::displayCamera(const QImage &image)
 {
     qimage_mutex_.lock();
     qimage_ = image.copy();
@@ -242,122 +333,7 @@ void MainWindow::slot_show_image(QImage image)
     ui->label_camera->resize(ui->label_camera->pixmap()->size());
     qimage_mutex_.unlock();
 }
-
+void MainWindow::updateLogcamera()
-
+{
-void MainWindow::setBtnStyles(){
+  displayCamera(qnode.image);
-    /*
-     * set PushButton state
-     */
-
-    ui->btn_main->setIcon(QIcon("://images/up.png"));
-    ui->btn_main->setText("mainWidget");
-    ui->btn_main->setStyleSheet(
-                "QPushButton:hover{"
-                "background-color:rgb(186, 189, 182);"
-                "border-bottom:2px solid rgb(67, 154, 246);}"
-                "QPushButton:checked{"
-                "background-color:cyan;"
-                "border-bottom:2px solid white}"
-                "QPushButton:pressed{"
-                "background-color:rgb(67, 154, 246)}"
-                "QPushButton{"
-                "background-color:rgb(238, 238, 236);"
-                "border:none;"
-                "padding:0px 0px 0px 0px;"
-                "margin:0px 0px 0px 0px;}");
-
-    ui->btn_uav->setStyleSheet(
-                "QPushButton:hover{"
-                "background-color:rgb(186, 189, 182);"
-                "border-bottom:2px solid rgb(67, 154, 246);}"
-                "QPushButton:checked{"
-                "background-color:cyan;"
-                "border-bottom:2px solid white}"
-                "QPushButton:pressed{"
-                "background-color:rgb(67, 154, 246)}"
-                "QPushButton{"
-                "background-color:rgb(238, 238, 236);"
-                "border:none;"
-                "padding:0px 0px 0px 0px;"
-                "margin:0px 0px 0px 0px;}");
-
-    ui->btn_ugv->setStyleSheet(
-                "QPushButton:hover{"
-                "background-color:rgb(186, 189, 182);"
-                "border-bottom:2px solid rgb(67, 154, 246);}"
-                "QPushButton:checked{"
-                "background-color:cyan;"
-                "border-bottom:2px solid white}"
-                "QPushButton:pressed{"
-                "background-color:rgb(67, 154, 246)}"
-                "QPushButton{"
-                "background-color:rgb(238, 238, 236);"
-                "border:none;"
-                "padding:0px 0px 0px 0px;"
-                "margin:0px 0px 0px 0px;}");
-
-    // 8 -> Forward: ↑
-    ui->pushButton_Forward->setShortcut(Qt::Key_8);
-    ui->pushButton_Forward->setStyleSheet(
-                "QPushButton{border-image: url(://images/up.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/up_2.png)}");
-    ui->pushButton_Forward->setFlat(true);
-
-    // 2 -> Back: ↓
-    ui->pushButton_Back->setShortcut(Qt::Key_2);
-    ui->pushButton_Back->setStyleSheet(
-                "QPushButton{border-image: url(://images/down.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/down_2.png)}");
-    ui->pushButton_Back->setFlat(true);
-
-    // 4 -> Left: ←
-    ui->pushButton_Left->setShortcut(Qt::Key_4);
-    ui->pushButton_Left->setStyleSheet(
-                "QPushButton{border-image: url(://images/left.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/left_2.png)}");
-    ui->pushButton_Left->setFlat(true);
-
-    // 6 -> Right: →
-    ui->pushButton_Right->setShortcut(Qt::Key_6);
-    ui->pushButton_Right->setStyleSheet(
-                "QPushButton{border-image: url(://images/right.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/right_2.png)}");
-    ui->pushButton_Right->setFlat(true);
-
-    // 7 -> Left_Forward: ↖
-    ui->pushButton_Left_Forward->setShortcut(Qt::Key_7);
-    ui->pushButton_Left_Forward->setStyleSheet(
-                "QPushButton{border-image: url(://images/up_left.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/up_left_2.png)}");
-    ui->pushButton_Left_Forward->setFlat(true);
-
-    // 9 -> Right_Forward: ↗
-    ui->pushButton_Right_Forward->setShortcut(Qt::Key_9);
-    ui->pushButton_Right_Forward->setStyleSheet(
-                "QPushButton{border-image: url(://images/up_right.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/up_right_2.png)}");
-    ui->pushButton_Right_Forward->setFlat(true);
-
-    // 1 -> Left_Back: ↙
-    ui->pushButton_Left_Back->setShortcut(Qt::Key_1);
-    ui->pushButton_Left_Back->setStyleSheet(
-                "QPushButton{border-image: url(://images/down_left.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/down_left_2.png)}");
-    ui->pushButton_Left_Back->setFlat(true);
-
-    // 3 -> Right_Back: ↘
-    ui->pushButton_Right_Back->setShortcut(Qt::Key_3);
-    ui->pushButton_Right_Back->setStyleSheet(
-                "QPushButton{border-image: url(://images/down_right.png)}"
-                "QPushButton{border:none}"
-                "QPushButton:pressed{border-image: url(://images/down_right_2.png)}");
-    ui->pushButton_Right_Back->setFlat(true);
-
 }

src/main/qnode.cpp

@@ -22,7 +22,6 @@ bool QNode::init() {
     odom_topic = topic_setting.value("topic/topic_odom", "odom").toString();
     batteryState_topic =
           topic_setting.value("topic/topic_power", "battery_state").toString();
-    qRegisterMetaType<sensor_msgs::BatteryState>("sensor_msgs::BatteryState");
     ros::init(init_argc,init_argv,"Air_Ground_CEC",
             ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
     SubAndPubTopic();
@@ -32,28 +31,17 @@ bool QNode::init() {
 
 void QNode::SubAndPubTopic(){
     ros::NodeHandle n;
-
     //创建速度话题的订阅者
-    cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 10,
+    cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 200,
-                                                 &QNode::speedCallback, this);
+                                                   &QNode::speedCallback, this);
-
     //power_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
       //                          &QNode::powerCallback, this);
     cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-    battery_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
+    cmd_pub1 = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-                              &QNode::batteryCallback, this);
-
     image_transport::ImageTransport it(n);
-    image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this);
-
-
-
-    //chatter_publisher = n.advertise<std_msgs::String>("chatter1", 1000);
     //chatter_subscriber = n.subscribe("chatter",1000,&QNode::myCallback,this);
-}
+    image_sub = it.subscribe("camera/image",100,&QNode::myCallback_img,this);
-
+    //chatter_publisher = n.advertise<std_msgs::String>("chatter1", 1000);
-void QNode::batteryCallback(const sensor_msgs::BatteryState& message) {
-  emit batteryState(message);
 }
 
 void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
@@ -61,7 +49,7 @@ void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
         // {'key', {x, y, z, th}}
         {'7', {1, 1, 0, 1}},  {'8', {1, 0, 0, 0}},  {'9', {1, -1, 0, -1}},
         {'4', {0, 0, 0, 1}}, {'5', {0, 0, 0, 0}},   {'6', {0, 0, 0, -1}},
-        {'1', {-1, 1, 0, -1}}, {'2', {-1, 0, 0, 0}}, {'3', {-1, -1, 0, 1}}
+        {'1', {-1, 1, 0, 1}}, {'2', {-1, 0, 0, 0}}, {'3', {-1, -1, 0, -1}}
     };
 
     float x = moveBindings[key][0];
@@ -85,7 +73,6 @@ void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
     twist.angular.z = th * turn;
 
     cmd_pub.publish(twist);
-
     ros::spinOnce();
 }
 
@@ -161,76 +148,29 @@ void QNode::AutoMove(float speed_linear, float speed_turn,
 
     ros::spinOnce();
 }
-int i =0;
+
 //速度回调函数
 void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
 {
-
-    std::cout<<i++<<endl;
-    ROS_INFO("i am ok");
     emit speed_x(msg->twist.twist.linear.x);
     emit speed_th(msg->twist.twist.angular.z);
-
 }
 
 
-void QNode::imageCallback(const sensor_msgs::CompressedImageConstPtr &msg)
+
+void QNode::myCallback_img(const sensor_msgs::ImageConstPtr &msg)
 {
+  try
+  {
+    cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
+    img = cv_ptr->image;
+    image = QImage(img.data,img.cols,img.rows,img.step[0],QImage::Format_RGB888);//change  to QImage format
     ROS_INFO("I'm setting picture in mul_t callback function!");
-    cv_bridge::CvImagePtr cv_ptr;
+    std::cout<<"10"<<std::endl;
-
+    emit loggingCamera();
-    try {
+  }
-      //深拷贝转换为opencv类型
+catch (cv_bridge::Exception& e)
-      cv_bridge::CvImagePtr cv_ptr_compressed =
+  {
-          cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
-      QImage im = Mat2QImage(cv_ptr_compressed->image);
-      emit show_image(im);
-    } catch (cv_bridge::Exception& e) {
-      //log(Error, ("video frame0 exception: " + QString(e.what())).toStdString());
-      return;
-    }
-    ros::spin();
-}
-
-
-QImage QNode::Mat2QImage(cv::Mat const& src) {
-  QImage dest(src.cols, src.rows, QImage::Format_ARGB32);
-
-  const float scale = 255.0;
-
-  if (src.depth() == CV_8U) {
-    if (src.channels() == 1) {
-      for (int i = 0; i < src.rows; ++i) {
-        for (int j = 0; j < src.cols; ++j) {
-          int level = src.at<quint8>(i, j);
-          dest.setPixel(j, i, qRgb(level, level, level));
-        }
-      }
-    } else if (src.channels() == 3) {
-      for (int i = 0; i < src.rows; ++i) {
-        for (int j = 0; j < src.cols; ++j) {
-          cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
-          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
-        }
-      }
-    }
-  } else if (src.depth() == CV_32F) {
-    if (src.channels() == 1) {
-      for (int i = 0; i < src.rows; ++i) {
-        for (int j = 0; j < src.cols; ++j) {
-          int level = scale * src.at<float>(i, j);
-          dest.setPixel(j, i, qRgb(level, level, level));
-        }
-      }
-    } else if (src.channels() == 3) {
-      for (int i = 0; i < src.rows; ++i) {
-        for (int j = 0; j < src.cols; ++j) {
-          cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
-          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
-        }
-      }
-    }
   }
-
-  return dest;
 }

src/ui/mainwindow.hpp

@@ -9,7 +9,6 @@
 #include "../include/joystick.h"
 #include "../include/qnode.hpp"
 #include "../include/dashboard.h"
-#include <sensor_msgs/BatteryState.h>
 
 namespace Ui {
 class MainWindow;
@@ -42,11 +41,11 @@ public slots:
     void slot_rockKeyChange(int);
     void slot_speed_x(double x);
     void slot_speed_th(double th);
-    void slot_batteryState(sensor_msgs::BatteryState);
     void Slider_raw_valueChanged(int v);
     void Slider_linear_valueChanged(int v);
 
-    void slot_show_image(QImage image);//added
+    void updateLogcamera();//added
+    void displayCamera(const QImage& image);//added
 
 
 private:

src/ui/mainwindow.ui

@@ -239,8 +239,8 @@ margin:0px 0px 0px 0px;
        <widget class="QWidget" name="">
         <property name="geometry">
          <rect>
-          <x>680</x>
+          <x>700</x>
-          <y>10</y>
+          <y>0</y>
           <width>735</width>
           <height>750</height>
          </rect>