diff --git a/src/include/qnode.hpp b/src/include/qnode.hpp index f7e24fd..e325147 100644 --- a/src/include/qnode.hpp +++ b/src/include/qnode.hpp @@ -75,7 +75,7 @@ private: 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 imageCallback(const sensor_msgs::ImageConstPtr &msg);//camera callback function QImage Mat2QImage(cv::Mat const &src); cv::Mat QImage2Mat(QImage &image); diff --git a/src/main/mainwindow.cpp b/src/main/mainwindow.cpp index 8e44df4..f3e3725 100644 --- a/src/main/mainwindow.cpp +++ b/src/main/mainwindow.cpp @@ -1,5 +1,5 @@ #include "../ui/mainwindow.hpp" - +#include MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : @@ -7,7 +7,11 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ui(new Ui::MainWindow) { - qnode.init(); + QtConcurrent::run([=](){ + qnode.init(); + }); + + ui->setupUi(this); initUis(); connections(); @@ -234,13 +238,13 @@ void MainWindow::Slider_linear_valueChanged(int v) { ui->label_linear->setText(QString::number(v)); } -void MainWindow::slot_show_image(QImage image) +void MainWindow::slot_show_image(const QImage& image) { - qimage_mutex_.lock(); - qimage_ = image.copy(); - ui->label_camera->setPixmap(QPixmap::fromImage(qimage_)); + //qimage_mutex_.lock(); + //qimage_ = image.copy(); + ui->label_camera->setPixmap(QPixmap::fromImage(image)); ui->label_camera->resize(ui->label_camera->pixmap()->size()); - qimage_mutex_.unlock(); + //qimage_mutex_.unlock(); } diff --git a/src/main/qnode.cpp b/src/main/qnode.cpp index 445cca8..c0bd5f3 100644 --- a/src/main/qnode.cpp +++ b/src/main/qnode.cpp @@ -1,11 +1,11 @@ #include "../include/qnode.hpp" #include "sensor_msgs/image_encodings.h" +#include QNode::QNode(int argc, char **argv) : init_argc(argc), init_argv(argv){ - } @@ -25,7 +25,10 @@ bool QNode::init() { qRegisterMetaType("sensor_msgs::BatteryState"); ros::init(init_argc,init_argv,"Air_Ground_CEC", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); - SubAndPubTopic(); + QtConcurrent::run([=](){ + SubAndPubTopic(); + }); + //start(); return true; } @@ -44,12 +47,13 @@ void QNode::SubAndPubTopic(){ &QNode::batteryCallback, this); image_transport::ImageTransport it(n); - image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this); - + image_sub = it.subscribe("camera/rgb/image_raw", 100, &QNode::imageCallback, 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); + ros::spin(); } void QNode::batteryCallback(const sensor_msgs::BatteryState& message) { @@ -161,35 +165,34 @@ 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::imageCallback(const sensor_msgs::CompressedImageConstPtr &msg) +void QNode::imageCallback(const sensor_msgs::ImageConstPtr &msg) { - ROS_INFO("I'm setting picture in mul_t callback function!"); - 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(); + try + { + cv_bridge::CvImageConstPtr cv_ptr = + cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); + cv::Mat img = cv_ptr->image; + QImage 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!"); + //image.save("/home/jackyma/test.jpg"); + emit show_image(image); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } + } diff --git a/src/ui/mainwindow.hpp b/src/ui/mainwindow.hpp index 6a9cb94..29dd080 100644 --- a/src/ui/mainwindow.hpp +++ b/src/ui/mainwindow.hpp @@ -47,7 +47,7 @@ public slots: void Slider_raw_valueChanged(int v); void Slider_linear_valueChanged(int v); void importFrame();//read video - void slot_show_image(QImage image);//added + void slot_show_image(const QImage& image);//added private slots: diff --git a/src/ui/mainwindow.ui b/src/ui/mainwindow.ui index 219b7fe..91c85f2 100644 --- a/src/ui/mainwindow.ui +++ b/src/ui/mainwindow.ui @@ -203,7 +203,7 @@ margin:0px 0px 0px 0px; - 1 + 2