From aaf38a9a288f119a3754320bd6406ac380478e00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=A4=A7=E8=80=B3=E5=88=AE=E5=AD=90?= <2433069615@qq.com> Date: Tue, 17 May 2022 23:19:23 +0800 Subject: [PATCH] 517 --- src/include/qnode.hpp | 23 ++-- src/main/main.cpp | 2 + src/main/mainwindow.cpp | 266 ++++++++++++++++++++++------------------ src/main/qnode.cpp | 100 ++++++++++++--- src/ui/mainwindow.hpp | 5 +- src/ui/mainwindow.ui | 4 +- 6 files changed, 248 insertions(+), 152 deletions(-) diff --git a/src/include/qnode.hpp b/src/include/qnode.hpp index f5aa260..f7e24fd 100644 --- a/src/include/qnode.hpp +++ b/src/include/qnode.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -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 loggingCamera();//发出设置相机图片信号 + //void power(float p); + void batteryState(sensor_msgs::BatteryState); + 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); }; diff --git a/src/main/main.cpp b/src/main/main.cpp index e31bc33..473df68 100644 --- a/src/main/main.cpp +++ b/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 + * */ diff --git a/src/main/mainwindow.cpp b/src/main/mainwindow.cpp index d670d0c..ea30a81 100644 --- a/src/main/mainwindow.cpp +++ b/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<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); + } diff --git a/src/main/qnode.cpp b/src/main/qnode.cpp index 4351f7a..445cca8 100644 --- a/src/main/qnode.cpp +++ b/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"); 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(odom_topic.toStdString(), 200, - &QNode::speedCallback, this); + cmdVel_sub = n.subscribe(odom_topic.toStdString(), 10, + &QNode::speedCallback, this); + //power_sub = n.subscribe(batteryState_topic.toStdString(), 1000, // &QNode::powerCallback, this); cmd_pub = n.advertise("cmd_vel", 1); - cmd_pub1 = n.advertise("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_sub = it.subscribe("camera/image",100,&QNode::myCallback_img,this); + image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this); + + + //chatter_publisher = n.advertise("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<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"<encoding.c_str()); + cv_bridge::CvImagePtr cv_ptr; + + try { + //深拷贝转换为opencv类型 + cv_bridge::CvImagePtr cv_ptr_compressed = + 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(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(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(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(i, j); + dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0])); + } + } + } } + + return dest; } diff --git a/src/ui/mainwindow.hpp b/src/ui/mainwindow.hpp index bf88283..01eb5b1 100644 --- a/src/ui/mainwindow.hpp +++ b/src/ui/mainwindow.hpp @@ -9,6 +9,7 @@ #include "../include/joystick.h" #include "../include/qnode.hpp" #include "../include/dashboard.h" +#include 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 displayCamera(const QImage& image);//added + void slot_show_image(QImage image);//added private: diff --git a/src/ui/mainwindow.ui b/src/ui/mainwindow.ui index 58d5687..b4841a7 100644 --- a/src/ui/mainwindow.ui +++ b/src/ui/mainwindow.ui @@ -239,8 +239,8 @@ margin:0px 0px 0px 0px; - 700 - 0 + 680 + 10 735 750