修改导航动作类型，1：旋转 2：横移 3：MPC或者前进

MPC/monitor/monitor_emqx.h

@@ -14,13 +14,12 @@ class Monitor_emqx
 {
 public:
     enum SpeedType{
-        eRotate=0,
-        eHorizontal,
-        eVrtical,
-        eMPC,
+        eStop=0,
+        eRotate=1,
+        eHorizontal=2,
+        eMPC=3,
 
 
-        eStop
     };
  public:
     Monitor_emqx(std::string nodeId,std::string pubTopic,std::string subTopic);

MPC/navigation.cpp

@@ -287,7 +287,7 @@ bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_di
       }
       if (action == 3) {
         if (Pose2d::abs(diff).x() > thresh.x()) {
-          monitor_->set_speed(Monitor_emqx::eVrtical, limit(diff.x(),0.05,0.5), 0);
+          monitor_->set_speed(Monitor_emqx::eMPC, limit(diff.x(),0.05,0.5), 0);
           printf(" Vrtical :%f\n",limit(diff.x(),0.05,0.5));
           continue;
         }

message.proto

@@ -9,7 +9,7 @@ message AGVStatu {
 }
 
 message Speed {
-  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他/未接收到：停止
+  int32 type=1; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
   float v=2;
   float vth=3;
 }

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -10,16 +10,19 @@
 #include <unistd.h>
 #include <chrono>
 #include <random>
+#include "timedlockdata.hpp"
 // All the webots classes are defined in the "webots" namespace
 using namespace webots;
 
 Paho_client* client_= nullptr;
-NavMessage::Speed g_speed;
+TimedLockData<NavMessage::Speed> g_speed;
 
 
 void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
 {
-  msg.toProtoMessage(g_speed);
+  NavMessage::Speed speed;
+  msg.toProtoMessage(speed);
+  g_speed.reset(speed,0.3);
 }
 
 double generae_random(std::default_random_engine& generator,double mean,double sigma)
@@ -62,6 +65,32 @@ void Rotating(double wmg,double w,double l,
   L4=(velocity);
 }
 
+void Horizontal(double velocity,double& R1,double& R2,double& R3,double& R4,
+                double& L1,double& L2,double& L3,double& L4)
+{
+  R1=-M_PI/2.0;
+  R2=M_PI/2.0;
+  R3=M_PI/2.0;
+  R4=-M_PI/2.0;
+  L1=-velocity;
+  L2=velocity;
+  L3=velocity;
+  L4=-velocity;
+}
+
+void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
+                double& L1,double& L2,double& L3,double& L4)
+{
+  R1=0;
+  R2=0;
+  R3=0;
+  R4=0;
+  L1=velocity;
+  L2=velocity;
+  L3=velocity;
+  L4=velocity;
+}
+
 
 int main(int argc, char **argv) {
   //init mqtt
@@ -121,8 +150,8 @@ int main(int argc, char **argv) {
   gps->enable(timeStep);
   imu->enable(timeStep);
 
-  double w=2.5;
-  double l=1.3;
+  double w=2.450;
+  double l=1.274;
   double radius=0.1915;
   double K=1.0/radius;
 
@@ -171,79 +200,103 @@ int main(int argc, char **argv) {
       client_->publish("monitor/statu",1,msg);
     }
 
-    //变速，角速度封顶
-    double cmd_v=g_speed.v();
-    double cmd_vth=g_speed.vth();
-    if(fabs(cmd_vth)>0.4)
+    if(g_speed.timeout())
     {
-      cmd_vth=cmd_vth>0?0.4:-0.4;
+      RM1->setPosition(0);
+      RM2->setPosition(0);
+      RM3->setPosition(0);
+      RM4->setPosition(0);
+
+      R1_l->setVelocity(0);
+      R2_l->setVelocity(0);
+      R3_l->setVelocity(0);
+      R4_l->setVelocity(0);
+      continue;
     }
+
+    //变速----------------------------------
+    double cmd_v=g_speed.Get().v();
+    double cmd_vth=g_speed.Get().vth();
+
     //初始值
     double R1=0,R2=0,R3=0,R4=0;
     double L1=cmd_v;
     double L2=cmd_v;
     double L3=cmd_v;
     double L4=cmd_v;
-    double theta1=0;
-    double theta2=0;
 
-    //无角速度，直线
-    if(fabs(cmd_vth)<0.0001)
+    if(g_speed.Get().type()==1) //原地旋转
     {
-      R1=R2=R3=R4=0;
-      L1=L2=L3=L4=cmd_v;
+      if (fabs(cmd_vth) > 0.0001)
+        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else  //有角速度
+    else if(g_speed.Get().type()==2)    //横移
     {
-      if (fabs(cmd_vth) < 0.00001)
-      {
-        //原地旋转
-        if (fabs(cmd_vth) > 0.0001)
-          Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+      Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
+    }
+    else {    //巡线/前进
+
+      if (fabs(cmd_vth) > 0.4) {
+        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
       }
-      else
+
+
+      //------------------------------------------------
+      //无角速度，直线
+      if (fabs(cmd_vth) < 0.0001) {
+        R1 = R2 = R3 = R4 = 0;
+        L1 = L2 = L3 = L4 = cmd_v;
+      } else  //有角速度
       {
-        //默认原地旋转
-        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
-        double base = pow(cmd_v / cmd_vth,2)-(l*l/4);
-        //满足运动方程
-        if(base>=0.1)
-        {
-          double BaseR=sqrt(base)-w/2;
-          if(BaseR>0)
-          {
-            if(cmd_vth*cmd_v>=0)
-            {
-              R1=atan(l/BaseR);
-              R2=atan(l/(BaseR+w));
-              L3=BaseR*cmd_vth;
-              L4=(BaseR+w)*cmd_vth;
-              R3=0;
-              R4=0;
-              L1=L3/cos(R1);
-              L2=L4/cos(R2);
+        if (fabs(cmd_v) < 0.00001) {
+          //原地旋转
+          if (fabs(cmd_vth) > 0.0001)
+            Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+        } else {
+          //默认原地旋转
+          Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+          double base = pow(cmd_v / cmd_vth, 2) - (l * l / 4);
+          //满足运动方程
+          if (base >= 0.0001) {
+            double BaseR = sqrt(base) - w / 2;
+
+            if(BaseR<0.05)
+              BaseR=0.05;
+            printf("BaseR : %f \n",BaseR);
+
+            if (BaseR > 0) {
+              if (cmd_vth * cmd_v >= 0) {
+                R1 = atan(l / BaseR);
+                R2 = atan(l / (BaseR + w));
+                L3 = BaseR * cmd_vth;
+                L4 = (BaseR + w) * cmd_vth;
+                R3 = 0;
+                R4 = 0;
+                L1 = L3 / cos(R1);
+                L2 = L4 / cos(R2);
+
+              } else {
+                R1 = -atan(l / (BaseR + w));
+                R2 = -atan(l / BaseR);
+                L3 = -(BaseR + w) * cmd_vth;
+                L4 = -BaseR * cmd_vth;
+                R3 = 0;
+                R4 = 0;
+                L1 = L3 / cos(R1);
+                L2 = L4 / cos(R2);
+              }
 
             }
-            else
-            {
-              R1=-atan(l/(BaseR+w));
-              R2=-atan(l/BaseR);
-              L3=-(BaseR+w)*cmd_vth;
-              L4=-BaseR*cmd_vth;
-              R3=0;
-              R4=0;
-              L1=L3/cos(R1);
-              L2=L4/cos(R2);
-            }
-
           }
-        }
 
+        }
       }
     }
 
-    //printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
-     //       R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
+    printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
+            R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
+     
+     
     RM1->setPosition(R1);
     RM2->setPosition(R2);
     RM3->setPosition(R3);

projects/controllers/AGV_controller/CMakeLists.txt

@@ -23,6 +23,7 @@ include_directories(
 
 # Setup the target executable.
 add_executable(${PROJECT} ${SOURCES}
+        timedlockdata.hpp
         emqx-client/paho_client.cpp
         emqx-client/mqttmsg.cpp
         emqx-client/message.pb.cc)

projects/webots_project/AGV.wbt

@@ -12,12 +12,12 @@ WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.5740659191011843 0.5847451344022531 0.5731679058700021 4.205639337485375
-  position -3.4387885340484483 154.09952770083555 49.90848557170683
+  orientation 0.5721756275983894 0.5828187812794944 0.577007209114647 4.194070278298114
+  position -0.7654556650304921 56.36839511374225 33.609829539597385
 }
 DEF BASE Solid {
-  translation 19.99999162516713 0.7911400111798716 0.00016165983097348163
-  rotation -3.8711855862479405e-07 0.9999998663179518 0.0005170724598391645 -0.001531067242145309
+  translation 19.999990829937104 0.7911400000000002 0.000161658626595193
+  rotation 2.2121878061787045e-10 0.9999999999999584 -2.8866226935975905e-07 -0.0015310670374692457
   children [
     DEF BASE Shape {
       appearance Appearance {
@@ -35,12 +35,12 @@ DEF BASE Solid {
   physics Physics {
     density 50
   }
-  linearVelocity -4.1576128148315245e-06 -1.08279694541181e-08 -6.597640722596085e-09
-  angularVelocity -6.626432588347747e-09 9.040283115125151e-17 4.1777643178632495e-06
+  linearVelocity -2.7559371959484336e-09 -1.3339312113357921e-15 -4.219641242788618e-12
+  angularVelocity -4.178905146812936e-12 1.24857802500255e-17 2.7293565217652084e-09
 }
 DEF Foot Solid {
-  translation 9.999393502113564 0.00762862454597632 -0.08621723912530124
-  rotation -0.5617422293774682 0.5636574702178948 -0.6055872554814831 2.06206264418059
+  translation 9.999393502113564 0.007628624545976265 -0.0862172391253013
+  rotation -0.5617422293774681 0.5636574702178947 -0.6055872554814831 2.06206264418059
   children [
     DEF FootGroup Group {
       children [
@@ -305,8 +305,8 @@ DEF Foot Solid {
   physics Physics {
     density 9800
   }
-  linearVelocity -3.970457727554586e-16 -2.0398749832070845e-16 -4.818912372454697e-18
-  angularVelocity -1.7757332017458718e-17 2.209080262389004e-17 5.97144250775885e-15
+  linearVelocity -3.4549234260275445e-16 -6.119624949621267e-17 1.1918508572874506e-16
+  angularVelocity 1.8556182360306814e-15 1.5717121929070687e-17 5.213168734713509e-15
 }
 DEF BODY Solid {
   translation 10 0.1 -1.80464e-16
@@ -547,8 +547,8 @@ DEF BODY Solid {
   boundingObject USE BODY_G
 }
 DEF AGV Robot {
-  translation -15.972732839754032 0.5080410140694867 99.99348791176553
-  rotation -0.00028505706050755406 0.9999997134244 0.0007013512601250627 -1.5672307993259762
+  translation 0.00034903 0.508376 20
+  rotation 0.7650907784602956 0.04955161149340747 -0.642013036093057 0.0010634142032065574
   children [
     Gyro {
       translation 0 0.4 0
@@ -599,7 +599,7 @@ DEF AGV Robot {
     }
     DEF LFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position 1.1331776432387734e-09
+        position 2.4002667736990556e-06
         axis 0 1 0
         anchor 1.25 -0.1 0.65
       }
@@ -611,12 +611,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2498259681333823 -0.3173116083844614 0.6500300316608966
-        rotation -0.3447163646607428 0.09752547357412135 -0.933627018642469 0.0001775248849732293
+        translation 1.2498259682054391 -0.3173116083844616 0.6500300320784578
+        rotation -0.34423265075460147 0.11088480184906173 -0.9323134895910039 0.00017777490497170807
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 10849.904368489606
+              position 11768.095523462096
             }
             device [
               RotationalMotor {
@@ -626,8 +626,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.3705934254400726e-05 0.0011641772083697314 -0.00017053874930456693
-              rotation 0.999999995997778 -4.997678264747281e-05 7.420758028727578e-05 5.126443553661687
+              translation 4.370593425440071e-05 0.0008994249138629575 0.0007585689565389026
+              rotation 0.9999999500227553 -0.0002686931942860267 0.00016660868661685807 5.9725436663308304
               children [
                 DEF WHEEL Shape {
                   appearance Appearance {
@@ -664,8 +664,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -3.187910897540244e-07 4.708011104898974e-09 -8.876850905234138e-08
-              angularVelocity -1.5074074154527665e-08 -2.6395764881469506e-07 1.526965588199187e-06
             }
           }
           DEF FootGroup Group {
@@ -931,13 +929,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -4.2959076643294003e-07 4.943124501951698e-09 -1.2891937660107115e-07
-        angularVelocity -1.3530240171179618e-08 -2.6421066826560306e-07 2.280214620692438e-08
       }
     }
     DEF LBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -9.202152675790929e-07
+        position 1.6935647371860216e-06
         axis 0 1 0
         anchor 1.25 -0.1 -0.65
       }
@@ -948,8 +944,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2502142944709551 -0.31715635361070865 -0.6499857452031614
-        rotation -0.45125234839526157 0.5132040109149485 0.7300636693114997 0.00015117003817987628
+        translation 1.250214294508213 -0.3171563536107088 -0.6499857457632802
+        rotation -0.4472340682926733 0.5257715475344475 0.7235647641791414 0.00015252790169601694
         children [
           DEF RightFootGroup Group {
             children [
@@ -1211,7 +1207,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 10393.08189276774
+              position 11143.83842267585
             }
             device [
               RotationalMotor {
@@ -1221,8 +1217,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation -0.000651963923245617 0.0012178725290791976 0.000849147055218453
-              rotation -0.9999999942003202 4.936510369407265e-05 -9.572066722789987e-05 1.2707798878561394
+              translation -0.0006519639232456156 -0.0012849079282141235 -0.0007438251370345185
+              rotation -0.9999999966351757 -7.442608261151756e-05 -3.450226910176673e-05 4.496486832468516
               children [
                 USE WHEEL
               ]
@@ -1231,8 +1227,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -3.358875467277861e-07 -4.4907216163353555e-09 8.333646571937204e-08
-              angularVelocity 3.9176368567325414e-08 8.909952527242376e-06 2.3100479578788137e-06
             }
           }
         ]
@@ -1241,13 +1235,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity 4.537013505741757e-07 -9.711681620742962e-09 1.0947191989567303e-07
-        angularVelocity 2.2774399519149717e-08 9.039709890610704e-06 3.818981982312481e-08
       }
     }
     DEF RFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -1.7009720762810627e-07
+        position -2.758145839062592e-06
         axis 0 1 0
         anchor -1.25 -0.1 0.65
       }
@@ -1258,8 +1250,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2499103940570822 -0.31904283227123437 0.6499825577091173
-        rotation -5.2230237200391964e-05 0.9999999976721874 -4.390475865564553e-05 3.141621535109839
+        translation -1.249910394011941 -0.3190428322712342 0.6499825579410219
+        rotation -5.223018038559064e-05 0.9999999976721874 -4.3904826242022044e-05 3.1416189470612137
         children [
           DEF RightFootGroup Group {
             children [
@@ -1521,7 +1513,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -7679.709855164719
+              position -8477.08428284596
             }
             device [
               RotationalMotor {
@@ -1531,8 +1523,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 1.5974980094313148e-07 -3.782094311669199e-05 -1.6469918842282497e-05
-              rotation 0.9999999999927158 7.678533205591556e-07 3.7387984582346038e-06 1.1251151443610974
+              translation 1.597498009431314e-07 -2.226004652945113e-05 -3.4729991267118e-05
+              rotation -0.9999999999963773 2.4861850589587016e-07 -2.6802333756186862e-06 4.567963832256156
               children [
                 USE WHEEL
               ]
@@ -1541,8 +1533,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 3.1109739120943386e-08 7.508563614576121e-09 -8.758717625722513e-08
-              angularVelocity -2.5320876085976226e-08 1.4410628326971284e-06 -1.2897695672871894e-06
             }
           }
         ]
@@ -1551,13 +1541,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -2.8957897662560458e-08 6.400995225253747e-09 -1.2351081459161783e-07
-        angularVelocity -1.3638184804563442e-08 1.4469396479670215e-06 -3.866315956978572e-08
       }
     }
     DEF RBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.250757975914993e-06
+        position 3.425815986309712e-09
         axis 0 1 0
         anchor -1.25 -0.1 -0.65
       }
@@ -1568,12 +1556,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2500226474019438 -0.31905992517371806 -0.6499245258899412
-        rotation -0.0002195737263218701 -0.9999993153212807 0.0011494104352281975 3.141573280728237
+        translation -1.2500226472318288 -0.3190599251737182 -0.6499245258388952
+        rotation -0.00021957243096586563 -0.9999993153212808 0.001149410682695147 3.141571026777037
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -7778.981994414451
+              position -8743.555001551553
             }
             device [
               RotationalMotor {
@@ -1583,8 +1571,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.1127246297456354e-07 0.0001809803531815049 0.0002251297412238077
-              rotation -0.9999999999979102 -1.7560414660382957e-06 1.0468450010864225e-06 0.39861554108144437
+              translation 4.112724629745625e-07 -0.00020338661933334 -0.00020511258298153186
+              rotation 0.9999999999999127 -2.323674118774218e-07 -3.4734238086227127e-07 2.638914627587
               children [
                 USE WHEEL
               ]
@@ -1593,8 +1581,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 6.114917015832324e-08 -5.042268220437756e-09 1.117256927937916e-07
-              angularVelocity 3.111518195853589e-08 2.1883589443242202e-05 4.219318118718796e-07
             }
           }
           DEF FootGroup Group {
@@ -1861,8 +1847,6 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.9995261665828107e-06 -3.194138189523615e-09 4.007305517845398e-08
-        angularVelocity 2.343040560218456e-08 2.2283684807958065e-05 -3.112859960097256e-08
       }
     }
     USE BODY_G
@@ -1872,8 +1856,6 @@ DEF AGV Robot {
     density 300
   }
   controller "AGV_controller"
-  linearVelocity -1.4449777312056026e-07 -4.0421471092010943e-10 9.194070979700752e-09
-  angularVelocity 1.1911248601846764e-09 -2.672783830748583e-07 -1.0106544568627156e-08
 }
 TexturedBackground {
 }