同意

src/include/qnode.hpp

@@ -20,6 +20,7 @@
 #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>
@@ -37,14 +38,16 @@ 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 loggingCamera();//发出设置相机图片信号
+    void show_image(QImage image);//发出设置相机图片信号
 
 
 
@@ -56,20 +59,26 @@ private:
     ros::Publisher cmd_pub;
     ros::Publisher cmd_pub1;
     ros::Subscriber cmdVel_sub;
-    ros::Subscriber power_sub;
+    ros::Subscriber battery_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
 
-    void myCallback_img(const sensor_msgs::ImageConstPtr& msg);//camera callback function
+    QImage  Mat2QImage(cv::Mat const &src);
-
+    cv::Mat QImage2Mat(QImage &image);
 
 };
 

src/main/main.cpp

@@ -24,4 +24,6 @@ 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,123 +41,7 @@ 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(){
@@ -189,6 +73,7 @@ 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);
 
 
@@ -228,6 +113,26 @@ 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);
@@ -279,7 +184,11 @@ 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)));
 }
 
 
@@ -325,7 +234,7 @@ void MainWindow::Slider_linear_valueChanged(int v) {
   ui->label_linear->setText(QString::number(v));
 }
 
-void MainWindow::displayCamera(const QImage &image)
+void MainWindow::slot_show_image(QImage image)
 {
     qimage_mutex_.lock();
     qimage_ = image.copy();
@@ -333,7 +242,122 @@ void MainWindow::displayCamera(const QImage &image)
     ui->label_camera->resize(ui->label_camera->pixmap()->size());
     qimage_mutex_.unlock();
 }
-void MainWindow::updateLogcamera()
+
-{
+
-  displayCamera(qnode.image);
+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);
+
 }

src/main/qnode.cpp

@@ -22,6 +22,7 @@ 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();
@@ -31,17 +32,28 @@ bool QNode::init() {
 
 void QNode::SubAndPubTopic(){
     ros::NodeHandle n;
+
     //创建速度话题的订阅者
-    cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 200,
+    cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 10,
-                                                   &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);
-    cmd_pub1 = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    battery_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
+                              &QNode::batteryCallback, this);
+
     image_transport::ImageTransport it(n);
-    //chatter_subscriber = n.subscribe("chatter",1000,&QNode::myCallback,this);
+    image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this);
-    image_sub = it.subscribe("camera/image",100,&QNode::myCallback_img,this);
+
+
+
     //chatter_publisher = n.advertise<std_msgs::String>("chatter1", 1000);
+    //chatter_subscriber = n.subscribe("chatter",1000,&QNode::myCallback,this);
+}
+
+void QNode::batteryCallback(const sensor_msgs::BatteryState& message) {
+  emit batteryState(message);
 }
 
 void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
@@ -49,7 +61,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];
@@ -73,6 +85,7 @@ void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
     twist.angular.z = th * turn;
 
     cmd_pub.publish(twist);
+
     ros::spinOnce();
 }
 
@@ -148,29 +161,76 @@ 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::myCallback_img(const sensor_msgs::ImageConstPtr &msg)
+void QNode::imageCallback(const sensor_msgs::CompressedImageConstPtr &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!");
-    std::cout<<"10"<<std::endl;
+    cv_bridge::CvImagePtr cv_ptr;
-    emit loggingCamera();
+
-  }
+    try {
-catch (cv_bridge::Exception& e)
+      //深拷贝转换为opencv类型
-  {
+      cv_bridge::CvImagePtr cv_ptr_compressed =
-    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+          cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+      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,6 +9,7 @@
 #include "../include/joystick.h"
 #include "../include/qnode.hpp"
 #include "../include/dashboard.h"
+#include <sensor_msgs/BatteryState.h>
 
 namespace Ui {
 class MainWindow;
@@ -41,11 +42,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 updateLogcamera();//added
+    void slot_show_image(QImage image);//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>700</x>
+          <x>680</x>
-          <y>0</y>
+          <y>10</y>
           <width>735</width>
           <height>750</height>
          </rect>