add AutoMove v1.0

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)

src/include/automove.cpp

@@ -0,0 +1,6 @@
+#include "automove.h"
+
+AutoMove::AutoMove()
+{
+
+}

src/include/automove.h

@@ -0,0 +1,12 @@
+#ifndef AUTOMOVE_H
+#define AUTOMOVE_H
+
+
+class AutoMove
+{
+public:
+    AutoMove();
+
+};
+
+#endif // AUTOMOVE_H

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;

src/main/mainwindow.cpp

@@ -191,6 +191,7 @@ void MainWindow::slot_keyboard_control(){
     //std::cout<<liner<<" "<<turn<<std::endl;
     qnode.KeyboardMove(key, liner, turn);
 
+
 }
 
 

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)
 {