From 545537255bdd0c2954b586dca3c23e42b12c7f0e 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, 10 May 2022 10:29:53 +0800
Subject: [PATCH] add twist

---
 src/include/qnode.hpp               |  25 ++++-
 src/include/roskeyboardteleopnode.h |  58 -----------
 src/main/main.cpp                   |  17 +---
 src/main/mainwindow.cpp             |  25 ++++-
 src/main/qnode.cpp                  |  45 ++++++---
 src/main/roskeyboardteleopnode.cpp  | 138 -------------------------
 src/ui/mainwindow.hpp               |   7 ++
 src/ui/mainwindow.ui                | 151 +++++++++++++++++-----------
 8 files changed, 179 insertions(+), 287 deletions(-)
 delete mode 100644 src/include/roskeyboardteleopnode.h
 delete mode 100644 src/main/roskeyboardteleopnode.cpp

diff --git a/src/include/qnode.hpp b/src/include/qnode.hpp
index 0a79ed1..8585220 100644
--- a/src/include/qnode.hpp
+++ b/src/include/qnode.hpp
@@ -4,25 +4,46 @@
 #ifndef Q_MOC_RUN
 #include <ros/ros.h>
 #endif
+#include <QImage>
 
-#include <geometry_msgs/Twist.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
 
+#include <std_msgs/String.h>
+#include <ros/network.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <geometry_msgs/Twist.h>
 #include <QThread>
 
 class QNode : public QThread{
     Q_OBJECT
+
 public:
     QNode(int argc, char **argv);
     virtual ~QNode();
     bool init();
+    void SubAndPubTopic();
     void KeyboardMove(char key);
+    //void myCallback_img(const sensor_msgs::ImageConstPtr& msg);//camera callback function
+    //QImage image;
+
+
+//Q_SIGNALS:
+    //void loggingCamera();//发出设置相机图片信号
 
 private:
     int init_argc;
     char **init_argv;
-    ros::NodeHandle n;
+
     ros::Subscriber cmdVel_sub;
     ros::Publisher cmd_pub;
+    //ros::Publisher chatter_publisher;
+    //QStringList logging_model;
+    //image_transport::Subscriber image_sub;
+
+//cv::Mat img;
 };
 
 #endif
diff --git a/src/include/roskeyboardteleopnode.h b/src/include/roskeyboardteleopnode.h
deleted file mode 100644
index 5c2c008..0000000
--- a/src/include/roskeyboardteleopnode.h
+++ /dev/null
@@ -1,58 +0,0 @@
-#ifndef ROSKEYBOARDTELEOPNODE_H
-#define ROSKEYBOARDTELEOPNODE_H
-
-#include <termios.h>
-#include <signal.h>
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/poll.h>
-
-#include <boost/thread/thread.hpp>
-#include <ros/ros.h>
-#include <geometry_msgs/Twist.h>
-
-#define KEYCODE_W 0x77
-#define KEYCODE_A 0x61
-#define KEYCODE_S 0x73
-#define KEYCODE_D 0x64
-
-#define KEYCODE_A_CAP 0x41
-#define KEYCODE_D_CAP 0x44
-#define KEYCODE_S_CAP 0x53
-#define KEYCODE_W_CAP 0x57
-
-
-
-class RosKeyboardTeleopNode
-{
-private:
-    double walk_vel_;
-    double run_vel_;
-    double yaw_rate_;
-    double yaw_rate_run_;
-
-    geometry_msgs::Twist cmdvel_;
-    ros::NodeHandle n_;
-    ros::Publisher pub_;
-
-public:
-    RosKeyboardTeleopNode();
-
-    ~RosKeyboardTeleopNode() { }
-    void keyboardLoop();
-
-    void stopRobot()
-    {
-        cmdvel_.linear.x = 0.0;
-        cmdvel_.angular.z = 0.0;
-        pub_.publish(cmdvel_);
-    }
-};
-
-extern int kfd;
-extern struct termios cooked, raw;
-extern bool done;
-extern RosKeyboardTeleopNode* tbk;
-
-#endif // ROSKEYBOARDTELEOPNODE_H
diff --git a/src/main/main.cpp b/src/main/main.cpp
index b4d3b2b..a983b77 100644
--- a/src/main/main.cpp
+++ b/src/main/main.cpp
@@ -1,4 +1,4 @@
-#include "../include/roskeyboardteleopnode.h"
+
 #include "../ui/mainwindow.hpp"
 
 #include <QApplication>
@@ -11,25 +11,12 @@ int main(int argc, char** argv)
 {
 
 
-
-    ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
-
     QApplication app(argc, argv);
     MainWindow CEC(0,0,0);
+
     CEC.show();
     return app.exec();
-/*
-    RosKeyboardTeleopNode tbk;
-
-    boost::thread t = boost::thread(boost::bind(&RosKeyboardTeleopNode::keyboardLoop, &tbk));
-
-    ros::spin();
 
-    t.interrupt();
-    t.join();
-    tbk.stopRobot();
-    tcsetattr(kfd, TCSANOW, &cooked);
-*/
     return(0);
 }
 
diff --git a/src/main/mainwindow.cpp b/src/main/mainwindow.cpp
index 60c5b93..46c1caa 100644
--- a/src/main/mainwindow.cpp
+++ b/src/main/mainwindow.cpp
@@ -1,12 +1,14 @@
 #include "../ui/mainwindow.hpp"
 
 
+
 MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     QMainWindow(parent), qnode(argc, argv),
     ui(new Ui::MainWindow)
+
 {
+    qnode.init();
     ui->setupUi(this);
-
     connections();
 }
 
@@ -41,7 +43,6 @@ void MainWindow::slot_keyboard_control(){
 
 
     qnode.KeyboardMove(key);
-    std::cout<<key<<std::endl;
 
 }
 
@@ -56,4 +57,24 @@ void MainWindow::connections(){
     ui->pushButton_D->setShortcut(Qt::Key_D);
     connect(ui->pushButton_D, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
     connect(ui->pushButton_shift, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
+    //
+    //QObject::connect(&qnode, SIGNAL(loggingCamera()), this, SLOT(slot_updateCamera()));
+}
+
+
+/*
+void MainWindow::slot_dispalyCamera(const QImage &image){
+    qimage_mutex.lock();
+    qimage = image.copy();
+    ui->UGV_Vedio->setPixmap(QPixmap::fromImage(qimage));
+    ui->UGV_Vedio->resize(ui->UGV_Vedio->pixmap()->size());
+    qimage_mutex.unlock();
 }
+
+void MainWindow::slot_updateCamera()
+{
+  slot_dispalyCamera(qnode.image);
+}
+*/
+
+
diff --git a/src/main/qnode.cpp b/src/main/qnode.cpp
index 036425b..35c7ed5 100644
--- a/src/main/qnode.cpp
+++ b/src/main/qnode.cpp
@@ -1,11 +1,11 @@
 #include "../include/qnode.hpp"
 
-#include <ros/ros.h>
-#include <std_msgs/String.h>
-#include <ros/network.h>
+#include "sensor_msgs/image_encodings.h"
 
 QNode::QNode(int argc, char **argv) : init_argc(argc), init_argv(argv){
-    cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+
+
 }
 
 
@@ -18,15 +18,16 @@ QNode::~QNode() {
 }
 
 bool QNode::init() {
-  ros::init(init_argc, init_argv, "cyrobot_monitor",
-            ros::init_options::AnonymousName);
-  if (!ros::master::check()) {
-    return false;
-  }
-  ros::start();  // explicitly needed since our nodehandle is going out of
-                 // scope.
-  start();
-  return true;
+
+    ros::init(init_argc,init_argv,"Air_Ground_CEC",
+            ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
+    SubAndPubTopic();
+    return true;
+}
+
+void QNode::SubAndPubTopic(){
+    ros::NodeHandle n;
+    cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 }
 
 void QNode::KeyboardMove(char key){
@@ -45,3 +46,21 @@ void QNode::KeyboardMove(char key){
     cmd_pub.publish(twist);
     ros::spinOnce();
 }
+
+/*
+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!");
+    Q_EMIT loggingCamera();
+  }
+catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
+  }
+}
+*/
diff --git a/src/main/roskeyboardteleopnode.cpp b/src/main/roskeyboardteleopnode.cpp
deleted file mode 100644
index 37618ea..0000000
--- a/src/main/roskeyboardteleopnode.cpp
+++ /dev/null
@@ -1,138 +0,0 @@
-#include "../include/roskeyboardteleopnode.h"
-
-int kfd = 0;
-struct termios cooked, raw;
-bool done;
-RosKeyboardTeleopNode* tbk;
-
-RosKeyboardTeleopNode::RosKeyboardTeleopNode()
-{
-    pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-
-    ros::NodeHandle n_private("~");
-    n_private.param("walk_vel", walk_vel_, 0.5);
-    n_private.param("run_vel", run_vel_, 1.0);
-    n_private.param("yaw_rate", yaw_rate_, 1.0);
-    n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
-}
-
-
-void RosKeyboardTeleopNode::keyboardLoop()
-{
-    char c;
-    double max_tv = walk_vel_;
-    double max_rv = yaw_rate_;
-    bool dirty = false;
-    int speed = 0;
-    int turn = 0;
-
-    // get the console in raw mode
-    tcgetattr(kfd, &cooked);
-    memcpy(&raw, &cooked, sizeof(struct termios));
-    raw.c_lflag &=~ (ICANON | ECHO);
-    raw.c_cc[VEOL] = 1;
-    raw.c_cc[VEOF] = 2;
-    tcsetattr(kfd, TCSANOW, &raw);
-
-    puts("Reading from keyboard");
-    puts("Use WASD keys to control the robot");
-    puts("Press Shift to move faster");
-
-    struct pollfd ufd;
-    ufd.fd = kfd;
-    ufd.events = POLLIN;
-
-    for(;;)
-    {
-        boost::this_thread::interruption_point();
-
-        // get the next event from the keyboard
-        int num;
-
-        if ((num = poll(&ufd, 1, 250)) < 0)
-        {
-            perror("poll():");
-            return;
-        }
-        else if(num > 0)
-        {
-            if(read(kfd, &c, 1) < 0)
-            {
-                perror("read():");
-                return;
-            }
-        }
-        else
-        {
-            if (dirty == true)
-            {
-                stopRobot();
-                dirty = false;
-            }
-
-            continue;
-        }
-
-        switch(c)
-        {
-            case KEYCODE_W:
-                max_tv = walk_vel_;
-                speed = 1;
-                turn = 0;
-                dirty = true;
-                break;
-            case KEYCODE_S:
-                max_tv = walk_vel_;
-                speed = -1;
-                turn = 0;
-                dirty = true;
-                break;
-            case KEYCODE_A:
-                max_rv = yaw_rate_;
-                speed = 0;
-                turn = 1;
-                dirty = true;
-                break;
-            case KEYCODE_D:
-                max_rv = yaw_rate_;
-                speed = 0;
-                turn = -1;
-                dirty = true;
-                break;
-
-            case KEYCODE_W_CAP:
-                max_tv = run_vel_;
-                speed = 1;
-                turn = 0;
-                dirty = true;
-                break;
-            case KEYCODE_S_CAP:
-                max_tv = run_vel_;
-                speed = -1;
-                turn = 0;
-                dirty = true;
-                break;
-            case KEYCODE_A_CAP:
-                max_rv = yaw_rate_run_;
-                speed = 0;
-                turn = 1;
-                dirty = true;
-                break;
-            case KEYCODE_D_CAP:
-                max_rv = yaw_rate_run_;
-                speed = 0;
-                turn = -1;
-                dirty = true;
-                break;
-            default:
-                max_tv = walk_vel_;
-                max_rv = yaw_rate_;
-                speed = 0;
-                turn = 0;
-                dirty = false;
-        }
-        cmdvel_.linear.x = speed * max_tv;
-        cmdvel_.angular.z = turn * max_rv;
-        pub_.publish(cmdvel_);
-    }
-}
diff --git a/src/ui/mainwindow.hpp b/src/ui/mainwindow.hpp
index dc416b9..3254020 100644
--- a/src/ui/mainwindow.hpp
+++ b/src/ui/mainwindow.hpp
@@ -2,6 +2,9 @@
 #define MAINWINDOW_H
 
 #include <QMainWindow>
+#include <QImage>
+#include <QMutex>
+
 #include "ui_mainwindow.h"
 #include "../include/qnode.hpp"
 
@@ -19,11 +22,15 @@ public:
 
 public slots:
     void slot_keyboard_control();
+    //void slot_updateCamera();
+    //void slot_dispalyCamera(const QImage& image);
 
 private:
     Ui::MainWindow *ui;
     void connections();
     QNode qnode;
+    //QImage qimage;
+    //mutable QMutex qimage_mutex;
 
 };
 
diff --git a/src/ui/mainwindow.ui b/src/ui/mainwindow.ui
index ea78c87..a3c59f5 100644
--- a/src/ui/mainwindow.ui
+++ b/src/ui/mainwindow.ui
@@ -14,29 +14,16 @@
    <string>MainWindow</string>
   </property>
   <widget class="QWidget" name="centralwidget">
-   <widget class="QWidget" name="">
+   <widget class="QWidget" name="layoutWidget">
     <property name="geometry">
      <rect>
-      <x>410</x>
-      <y>260</y>
+      <x>1160</x>
+      <y>240</y>
       <width>314</width>
       <height>164</height>
      </rect>
     </property>
     <layout class="QGridLayout" name="gridLayout">
-     <item row="0" column="0">
-      <spacer name="horizontalSpacer">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>40</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
      <item row="0" column="1">
       <widget class="QPushButton" name="pushButton_W">
        <property name="minimumSize">
@@ -61,21 +48,8 @@
        </property>
       </widget>
      </item>
-     <item row="0" column="2">
-      <spacer name="horizontalSpacer_2">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>40</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item row="1" column="0">
-      <widget class="QPushButton" name="pushButton_A">
+     <item row="1" column="2">
+      <widget class="QPushButton" name="pushButton_D">
        <property name="minimumSize">
         <size>
          <width>100</width>
@@ -94,12 +68,12 @@
         </font>
        </property>
        <property name="text">
-        <string>左</string>
+        <string>右</string>
        </property>
       </widget>
      </item>
-     <item row="1" column="1">
-      <widget class="QPushButton" name="pushButton_shift">
+     <item row="1" column="0">
+      <widget class="QPushButton" name="pushButton_A">
        <property name="minimumSize">
         <size>
          <width>100</width>
@@ -118,12 +92,12 @@
         </font>
        </property>
        <property name="text">
-        <string>加速</string>
+        <string>左</string>
        </property>
       </widget>
      </item>
-     <item row="1" column="2">
-      <widget class="QPushButton" name="pushButton_D">
+     <item row="2" column="1">
+      <widget class="QPushButton" name="pushButton_S">
        <property name="minimumSize">
         <size>
          <width>100</width>
@@ -142,25 +116,12 @@
         </font>
        </property>
        <property name="text">
-        <string>右</string>
+        <string>后</string>
        </property>
       </widget>
      </item>
-     <item row="2" column="0">
-      <spacer name="horizontalSpacer_3">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>40</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item row="2" column="1">
-      <widget class="QPushButton" name="pushButton_S">
+     <item row="1" column="1">
+      <widget class="QPushButton" name="pushButton_shift">
        <property name="minimumSize">
         <size>
          <width>100</width>
@@ -179,23 +140,95 @@
         </font>
        </property>
        <property name="text">
-        <string>后</string>
+        <string>加速</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </widget>
+   <widget class="QLabel" name="UGV_Vedio">
+    <property name="geometry">
+     <rect>
+      <x>610</x>
+      <y>60</y>
+      <width>321</width>
+      <height>211</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>TextLabel</string>
+    </property>
+   </widget>
+   <widget class="QWidget" name="">
+    <property name="geometry">
+     <rect>
+      <x>60</x>
+      <y>30</y>
+      <width>502</width>
+      <height>654</height>
+     </rect>
+    </property>
+    <layout class="QVBoxLayout" name="verticalLayout">
+     <item>
+      <widget class="QLabel" name="UAV_Vedio">
+       <property name="minimumSize">
+        <size>
+         <width>500</width>
+         <height>300</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>500</width>
+         <height>300</height>
+        </size>
+       </property>
+       <property name="mouseTracking">
+        <bool>false</bool>
+       </property>
+       <property name="styleSheet">
+        <string notr="true"/>
+       </property>
+       <property name="text">
+        <string>UAV_VEDIO</string>
+       </property>
+       <property name="scaledContents">
+        <bool>false</bool>
        </property>
       </widget>
      </item>
-     <item row="2" column="2">
-      <spacer name="horizontalSpacer_4">
+     <item>
+      <spacer name="verticalSpacer">
        <property name="orientation">
-        <enum>Qt::Horizontal</enum>
+        <enum>Qt::Vertical</enum>
        </property>
        <property name="sizeHint" stdset="0">
         <size>
-         <width>40</width>
-         <height>20</height>
+         <width>20</width>
+         <height>40</height>
         </size>
        </property>
       </spacer>
      </item>
+     <item>
+      <widget class="QLabel" name="UAV_Path">
+       <property name="minimumSize">
+        <size>
+         <width>500</width>
+         <height>300</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>500</width>
+         <height>300</height>
+        </size>
+       </property>
+       <property name="text">
+        <string>PATH</string>
+       </property>
+      </widget>
+     </item>
     </layout>
    </widget>
   </widget>