From 45153ce6a6d6f2dc926f23866be502fee604ea76 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: Mon, 16 May 2022 00:30:38 +0800
Subject: [PATCH] add AutoMove v1.0

---
 CMakeLists.txt           |  2 +-
 src/include/automove.cpp |  6 ++++
 src/include/automove.h   | 12 +++++++
 src/include/qnode.hpp    |  3 ++
 src/main/mainwindow.cpp  |  1 +
 src/main/qnode.cpp       | 75 +++++++++++++++++++++++++++++++++++++++-
 6 files changed, 97 insertions(+), 2 deletions(-)
 create mode 100644 src/include/automove.cpp
 create mode 100644 src/include/automove.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7fd0bbf..d32a901 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,7 +27,7 @@ catkin_package(
 ###########
 
 #set(OpenCV_DIR /usr/local/share/opencv4)
-#find_package(OpenCV REQUIRED)
+find_package(OpenCV REQUIRED)
 find_package(Qt5  REQUIRED COMPONENTS  Widgets )
 set(CMAKE_AUTOMOC ON)
 set(CMAKE_AUTOUIC ON)
diff --git a/src/include/automove.cpp b/src/include/automove.cpp
new file mode 100644
index 0000000..3238fd7
--- /dev/null
+++ b/src/include/automove.cpp
@@ -0,0 +1,6 @@
+#include "automove.h"
+
+AutoMove::AutoMove()
+{
+
+}
diff --git a/src/include/automove.h b/src/include/automove.h
new file mode 100644
index 0000000..2890af9
--- /dev/null
+++ b/src/include/automove.h
@@ -0,0 +1,12 @@
+#ifndef AUTOMOVE_H
+#define AUTOMOVE_H
+
+
+class AutoMove
+{
+public:
+    AutoMove();
+
+};
+
+#endif // AUTOMOVE_H
diff --git a/src/include/qnode.hpp b/src/include/qnode.hpp
index 7641131..f5aa260 100644
--- a/src/include/qnode.hpp
+++ b/src/include/qnode.hpp
@@ -35,6 +35,8 @@ public:
     bool init();
     void SubAndPubTopic();
     void KeyboardMove(char key, float speed_linear, float speed_trun);
+    void AutoMove(float speed_linear, float speed_turn,
+                  float target_linear, float target_turn);
     QImage image;
 
 Q_SIGNALS:
@@ -52,6 +54,7 @@ private:
     ros::Publisher chatter_publisher;
     ros::Subscriber chatter_subscriber;
     ros::Publisher cmd_pub;
+    ros::Publisher cmd_pub1;
     ros::Subscriber cmdVel_sub;
     ros::Subscriber power_sub;
     QStringListModel logging_model;
diff --git a/src/main/mainwindow.cpp b/src/main/mainwindow.cpp
index 8c545d4..d670d0c 100644
--- a/src/main/mainwindow.cpp
+++ b/src/main/mainwindow.cpp
@@ -191,6 +191,7 @@ void MainWindow::slot_keyboard_control(){
     //std::cout<<liner<<" "<<turn<<std::endl;
     qnode.KeyboardMove(key, liner, turn);
 
+
 }
 
 
diff --git a/src/main/qnode.cpp b/src/main/qnode.cpp
index f29746e..54baa10 100644
--- a/src/main/qnode.cpp
+++ b/src/main/qnode.cpp
@@ -37,7 +37,7 @@ void QNode::SubAndPubTopic(){
     //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);
     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);
@@ -76,6 +76,79 @@ void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
     ros::spinOnce();
 }
 
+void QNode::AutoMove(float speed_linear, float speed_turn,
+              float target_linear, float target_turn){
+
+    // wrong command
+    if (speed_linear==0 && speed_turn ==0){
+        std::cout<<"wrong command"<<endl;
+        return;
+    }
+
+
+    //How fast will we update the robot's movement?
+    double rate = 50;
+    //Set the equivalent ROS rate variable
+    ros::Rate loopRate(rate);
+
+    std::map<char, std::vector<float>> Directions {
+        {'w',{1, 0, 0, 0}}, {'x',{-1, 0, 0, 0}}, {'s',{0, 0, 0, 0}},
+        {'a',{0, 0, 0, 1}}, {'d',{0, 0, 0, -1}}
+    };
+    char key = 's';
+    if(target_linear < 0){
+        key = 'x';
+    }
+    else if (target_linear > 0){
+        key = 'w';
+    }
+    else if (target_turn > 0){
+        key = 'a';
+    }
+    else if (target_turn < 0){
+        key = 'd';
+    }
+
+    float x = Directions[key][0];
+    float y = Directions[key][1];
+    float z = Directions[key][2];
+    float th = Directions[key][3];
+
+
+
+    float total_times = 0;
+    if (target_linear == 0){
+        total_times = fabs(target_turn)/speed_turn;
+    }
+    else{
+        total_times = fabs(target_linear)/speed_linear;
+    }
+
+    std::cout<< total_times<<std::endl;
+
+    int ticks;
+    geometry_msgs::Twist twist;
+
+
+    twist.linear.x = speed_linear * x;
+    twist.linear.y = speed_linear * y;
+    twist.linear.z = speed_linear * z;
+
+    twist.angular.x = 0;
+    twist.angular.y = 0;
+    twist.angular.z = speed_turn * th;
+
+    ticks = int(total_times * rate);
+
+    for(int i = 0; i < ticks; i++)
+    {
+      cmd_pub.publish(twist); // 将刚才设置的指令发送给机器人
+      loopRate.sleep();
+    }
+
+    ros::spinOnce();
+}
+
 //速度回调函数
 void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
 {