1.回退下发主副车角度改动

2.修复整车模式下发轴距为0Bug

MPC/monitor/emqx/message.pb.cc

@@ -96,10 +96,6 @@ PROTOBUF_CONSTEXPR ToAgvCmd::ToAgvCmd(
 
   , /*decltype(_impl_.d1_)*/ 0
 
-  , /*decltype(_impl_.y1_)*/ 0
-
-  , /*decltype(_impl_.y2_)*/ 0
-
   , /*decltype(_impl_.end_)*/ 0
 
   , /*decltype(_impl_._cached_size_)*/{}} {}
@@ -375,8 +371,6 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.l1_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.p1_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.d1_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.y1_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.y2_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.end_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
@@ -515,15 +509,15 @@ static const ::_pbi::MigrationSchema
         { 0, -1, -1, sizeof(::NavMessage::LidarOdomStatu)},
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
         { 27, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
-        { 50, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 61, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 75, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 84, 99, -1, sizeof(::NavMessage::NewAction)},
-        { 106, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 117, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 127, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 138, 155, -1, sizeof(::NavMessage::NavStatu)},
-        { 164, 176, -1, sizeof(::NavMessage::RobotStatu)},
+        { 48, -1, -1, sizeof(::NavMessage::Pose2d)},
+        { 59, -1, -1, sizeof(::NavMessage::PathNode)},
+        { 73, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 82, 97, -1, sizeof(::NavMessage::NewAction)},
+        { 104, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 115, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 125, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 136, 153, -1, sizeof(::NavMessage::NavStatu)},
+        { 162, 174, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -546,46 +540,46 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
     "(\002\022\t\n\001v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"j\n\010AgvStatu\022\t"
     "\n\001v\030\001 \001(\002\022\t\n\001w\030\002 \001(\002\022\r\n\005clamp\030\003 \001(\005\022\023\n\013c"
     "lamp_other\030\004 \001(\005\022\016\n\006lifter\030\005 \001(\005\022\024\n\014lift"
-    "er_other\030\006 \001(\005\"\277\001\n\010ToAgvCmd\022\n\n\002H1\030\001 \001(\005\022"
+    "er_other\030\006 \001(\005\"\247\001\n\010ToAgvCmd\022\n\n\002H1\030\001 \001(\005\022"
     "\n\n\002M1\030\002 \001(\005\022\n\n\002T1\030\003 \001(\005\022\n\n\002V1\030\004 \001(\002\022\n\n\002W"
     "1\030\005 \001(\002\022\n\n\002V2\030\006 \001(\002\022\n\n\002W2\030\007 \001(\002\022\n\n\002V3\030\010 "
     "\001(\002\022\n\n\002W3\030\t \001(\002\022\n\n\002L1\030\n \001(\002\022\n\n\002P1\030\013 \001(\005\022"
-    "\n\n\002D1\030\014 \001(\002\022\n\n\002Y1\030\r \001(\002\022\n\n\002Y2\030\016 \001(\002\022\013\n\003e"
-    "nd\030\017 \001(\005\"-\n\006Pose2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002"
-    "\022\r\n\005theta\030\003 \001(\002\"Q\n\010PathNode\022\t\n\001x\030\001 \001(\002\022\t"
-    "\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n\001w\030\004 \001(\002\022\r\n\005theta"
-    "\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nTrajectory\022!\n\005pose"
-    "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\345\001\n\tNewActio"
-    "n\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001(\0132\024.Nav"
-    "Message.PathNode\022&\n\010passNode\030\003 \001(\0132\024.Nav"
-    "Message.PathNode\022(\n\nstreetNode\030\004 \001(\0132\024.N"
-    "avMessage.PathNode\022\'\n\tpathNodes\030\005 \003(\0132\024."
-    "NavMessage.PathNode\022\021\n\twheelbase\030\006 \001(\002\022\023"
-    "\n\013changedMode\030\007 \001(\005\"P\n\006NavCmd\022\016\n\006action\030"
-    "\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewActions\030\005 \003(\0132\025"
-    ".NavMessage.NewAction\"(\n\013NavResponse\022\013\n\003"
-    "ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManualCmd\022\013\n\003"
-    "key\030\001 \001(\t\022\026\n\016operation_type\030\002 \001(\005\022\020\n\010vel"
-    "ocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\005\022"
-    "\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001(\005\022(\n\004"
-    "odom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013"
-    "\n\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.Nav"
-    "Message.Trajectory\022,\n\014predict_traj\030\007 \001(\013"
-    "2\026.NavMessage.Trajectory\022\020\n\010in_space\030\010 \001"
-    "(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobotStatu\022\t\n\001x\030"
-    "\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvSt"
-    "atu\030\004 \001(\0132\024.NavMessage.AgvStatu2\302\001\n\nNavE"
-    "xcutor\0226\n\005Start\022\022.NavMessage.NavCmd\032\027.Na"
-    "vMessage.NavResponse\"\000\0227\n\006Cancel\022\022.NavMe"
-    "ssage.NavCmd\032\027.NavMessage.NavResponse\"\000\022"
-    "C\n\017ManualOperation\022\025.NavMessage.ManualCm"
-    "d\032\027.NavMessage.NavResponse\"\000b\006proto3"
+    "\n\n\002D1\030\014 \001(\002\022\013\n\003end\030\r \001(\005\"-\n\006Pose2d\022\t\n\001x\030"
+    "\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"Q\n\010PathN"
+    "ode\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n\001"
+    "w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nTr"
+    "ajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pos"
+    "e2d\"\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspace"
+    "Node\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010pass"
+    "Node\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstre"
+    "etNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpa"
+    "thNodes\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\tw"
+    "heelbase\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006N"
+    "avCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nne"
+    "wActions\030\005 \003(\0132\025.NavMessage.NewAction\"(\n"
+    "\013NavResponse\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\""
+    "B\n\tManualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_t"
+    "ype\030\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStatu"
+    "\022\r\n\005statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmov"
+    "e_mode\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage."
+    "LidarOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected_"
+    "traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014pr"
+    "edict_traj\030\007 \001(\0132\026.NavMessage.Trajectory"
+    "\022\020\n\010in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\n"
+    "RobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005thet"
+    "a\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage.A"
+    "gvStatu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMes"
+    "sage.NavCmd\032\027.NavMessage.NavResponse\"\000\0227"
+    "\n\006Cancel\022\022.NavMessage.NavCmd\032\027.NavMessag"
+    "e.NavResponse\"\000\022C\n\017ManualOperation\022\025.Nav"
+    "Message.ManualCmd\032\027.NavMessage.NavRespon"
+    "se\"\000b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1556,
+    1532,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -1343,10 +1337,6 @@ inline void ToAgvCmd::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.d1_) { 0 }
 
-    , decltype(_impl_.y1_) { 0 }
-
-    , decltype(_impl_.y2_) { 0 }
-
     , decltype(_impl_.end_) { 0 }
 
     , /*decltype(_impl_._cached_size_)*/{}
@@ -1496,27 +1486,9 @@ const char* ToAgvCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
-      // float Y1 = 13;
+      // int32 end = 13;
       case 13:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 109)) {
-          _impl_.y1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
-        } else {
-          goto handle_unusual;
-        }
-        continue;
-      // float Y2 = 14;
-      case 14:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 117)) {
-          _impl_.y2_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
-        } else {
-          goto handle_unusual;
-        }
-        continue;
-      // int32 end = 15;
-      case 15:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 120)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 104)) {
           _impl_.end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
@@ -1668,33 +1640,11 @@ failure:
         12, this->_internal_d1(), target);
   }
 
-  // float Y1 = 13;
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y1 = this->_internal_y1();
-  ::uint32_t raw_y1;
-  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
-  if (raw_y1 != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::_pbi::WireFormatLite::WriteFloatToArray(
-        13, this->_internal_y1(), target);
-  }
-
-  // float Y2 = 14;
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y2 = this->_internal_y2();
-  ::uint32_t raw_y2;
-  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
-  if (raw_y2 != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::_pbi::WireFormatLite::WriteFloatToArray(
-        14, this->_internal_y2(), target);
-  }
-
-  // int32 end = 15;
+  // int32 end = 13;
   if (this->_internal_end() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        15, this->_internal_end(), target);
+        13, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1809,25 +1759,7 @@ failure:
     total_size += 5;
   }
 
-  // float Y1 = 13;
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y1 = this->_internal_y1();
-  ::uint32_t raw_y1;
-  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
-  if (raw_y1 != 0) {
-    total_size += 5;
-  }
-
-  // float Y2 = 14;
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y2 = this->_internal_y2();
-  ::uint32_t raw_y2;
-  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
-  if (raw_y2 != 0) {
-    total_size += 5;
-  }
-
-  // int32 end = 15;
+  // int32 end = 13;
   if (this->_internal_end() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
         this->_internal_end());
@@ -1919,20 +1851,6 @@ void ToAgvCmd::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   if (raw_d1 != 0) {
     _this->_internal_set_d1(from._internal_d1());
   }
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y1 = from._internal_y1();
-  ::uint32_t raw_y1;
-  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
-  if (raw_y1 != 0) {
-    _this->_internal_set_y1(from._internal_y1());
-  }
-  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_y2 = from._internal_y2();
-  ::uint32_t raw_y2;
-  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
-  if (raw_y2 != 0) {
-    _this->_internal_set_y2(from._internal_y2());
-  }
   if (from._internal_end() != 0) {
     _this->_internal_set_end(from._internal_end());
   }

MPC/monitor/emqx/message.pb.h

@@ -680,9 +680,7 @@ class ToAgvCmd final :
     kL1FieldNumber = 10,
     kP1FieldNumber = 11,
     kD1FieldNumber = 12,
-    kY1FieldNumber = 13,
-    kY2FieldNumber = 14,
-    kEndFieldNumber = 15,
+    kEndFieldNumber = 13,
   };
   // int32 H1 = 1;
   void clear_h1() ;
@@ -804,27 +802,7 @@ class ToAgvCmd final :
   void _internal_set_d1(float value);
 
   public:
-  // float Y1 = 13;
-  void clear_y1() ;
-  float y1() const;
-  void set_y1(float value);
-
-  private:
-  float _internal_y1() const;
-  void _internal_set_y1(float value);
-
-  public:
-  // float Y2 = 14;
-  void clear_y2() ;
-  float y2() const;
-  void set_y2(float value);
-
-  private:
-  float _internal_y2() const;
-  void _internal_set_y2(float value);
-
-  public:
-  // int32 end = 15;
+  // int32 end = 13;
   void clear_end() ;
   ::int32_t end() const;
   void set_end(::int32_t value);
@@ -854,8 +832,6 @@ class ToAgvCmd final :
     float l1_;
     ::int32_t p1_;
     float d1_;
-    float y1_;
-    float y2_;
     ::int32_t end_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
@@ -3211,47 +3187,7 @@ inline void ToAgvCmd::_internal_set_d1(float value) {
   _impl_.d1_ = value;
 }
 
-// float Y1 = 13;
-inline void ToAgvCmd::clear_y1() {
-  _impl_.y1_ = 0;
-}
-inline float ToAgvCmd::y1() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y1)
-  return _internal_y1();
-}
-inline void ToAgvCmd::set_y1(float value) {
-  _internal_set_y1(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y1)
-}
-inline float ToAgvCmd::_internal_y1() const {
-  return _impl_.y1_;
-}
-inline void ToAgvCmd::_internal_set_y1(float value) {
-  ;
-  _impl_.y1_ = value;
-}
-
-// float Y2 = 14;
-inline void ToAgvCmd::clear_y2() {
-  _impl_.y2_ = 0;
-}
-inline float ToAgvCmd::y2() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y2)
-  return _internal_y2();
-}
-inline void ToAgvCmd::set_y2(float value) {
-  _internal_set_y2(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y2)
-}
-inline float ToAgvCmd::_internal_y2() const {
-  return _impl_.y2_;
-}
-inline void ToAgvCmd::_internal_set_y2(float value) {
-  ;
-  _impl_.y2_ = value;
-}
-
-// int32 end = 15;
+// int32 end = 13;
 inline void ToAgvCmd::clear_end() {
   _impl_.end_ = 0;
 }

MPC/monitor/monitor_emqx.cpp

@@ -192,45 +192,45 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     msg.fromProtoMessage(agvCmd);
     Publish(speedcmd_topic_,msg);
 }
-
-void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double diff_yaw[], double L, int P, double D) {
-//    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
-//    {
-//        printf(" Main agv mpc must set wheelBase\n ");
-//        return;
+//
+//void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double diff_yaw[], double L, int P, double D) {
+////    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
+////    {
+////        printf(" Main agv mpc must set wheelBase\n ");
+////        return;
+////    }
+//    int count = 3;
+//    double w_correct[count];
+//    double velocity[count];
+//    for (int i = 0; i < count; ++i) {
+//        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
+//        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
 //    }
-    int count = 3;
-    double w_correct[count];
-    double velocity[count];
-    for (int i = 0; i < count; ++i) {
-        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
-        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
-    }
-    for (int i = 0; i < 2; ++i) {
-        diff_yaw[i] =fabs(diff_yaw[i])>0.0001?diff_yaw[i]:0.0;
-    }
-    MqttMsg msg;
-    NavMessage::ToAgvCmd agvCmd;
-    heat_=(heat_+1)%255;
-    agvCmd.set_h1(heat_);
-    agvCmd.set_m1(mode);
-    agvCmd.set_t1(type);
-    agvCmd.set_v1(velocity[0]);
-    agvCmd.set_w1(w_correct[0]);
-    agvCmd.set_v2(velocity[1]);
-    agvCmd.set_w2(w_correct[1]);
-    agvCmd.set_v3(velocity[2]);
-    agvCmd.set_w3(w_correct[2]);
-    agvCmd.set_l1(L);
-    agvCmd.set_p1(P);
-    agvCmd.set_y1(diff_yaw[0]);
-    agvCmd.set_y2(diff_yaw[1]);
-    agvCmd.set_d1(D);
-
-    agvCmd.set_end(1);
-    msg.fromProtoMessage(agvCmd);
-    Publish(speedcmd_topic_,msg);
-}
+//    for (int i = 0; i < 2; ++i) {
+//        diff_yaw[i] =fabs(diff_yaw[i])>0.0001?diff_yaw[i]:0.0;
+//    }
+//    MqttMsg msg;
+//    NavMessage::ToAgvCmd agvCmd;
+//    heat_=(heat_+1)%255;
+//    agvCmd.set_h1(heat_);
+//    agvCmd.set_m1(mode);
+//    agvCmd.set_t1(type);
+//    agvCmd.set_v1(velocity[0]);
+//    agvCmd.set_w1(w_correct[0]);
+//    agvCmd.set_v2(velocity[1]);
+//    agvCmd.set_w2(w_correct[1]);
+//    agvCmd.set_v3(velocity[2]);
+//    agvCmd.set_w3(w_correct[2]);
+//    agvCmd.set_l1(L);
+//    agvCmd.set_p1(P);
+//    agvCmd.set_y1(diff_yaw[0]);
+//    agvCmd.set_y2(diff_yaw[1]);
+//    agvCmd.set_d1(D);
+//
+//    agvCmd.set_end(1);
+//    msg.fromProtoMessage(agvCmd);
+//    Publish(speedcmd_topic_,msg);
+//}
 
 
 

MPC/monitor/monitor_emqx.h

@@ -18,7 +18,7 @@ class Monitor_emqx : public Terminator_emqx
     void set_speed(int mode,ActionType type,double v,double a);
     void set_speed(int mode,ActionType type,double v,double a,double L);
     void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double L=0,int P=0,double D=0);
-    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double yaw_diff[], double L=0,int P=0,double D=0);
+//    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double yaw_diff[], double L=0,int P=0,double D=0);
     void clamp_close(int mode);
     void clamp_open(int mode);
     void clamp_fully_open(int mode);

MPC/navigation.cpp

@@ -988,7 +988,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     return ret == success || ret == no_solution;
 }
 
-bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, std::vector<double> diff_yaw, bool directY) {
+bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY) {
     if (PoseTimeout() == true) {
         printf(" MPC once Error:Pose is timeout \n");
         return false;
@@ -1006,8 +1006,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     double velocity = timedV_.Get();    //从plc获取状态
     double angular = timedA_.Get();
 
-    float diffYaw1=timedPose_.Get().m_diffYaw1;
-    float diffYaw2=timedPose_.Get().m_diffYaw2;
+//    float diffYaw1=timedPose_.Get().m_diffYaw1;
+//    float diffYaw2=timedPose_.Get().m_diffYaw2;
 
     Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
 
@@ -1063,8 +1063,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         out.push_back(0);
     }
     //
-    diff_yaw.push_back(diffYaw1);
-    diff_yaw.push_back(diffYaw2);
+//    diff_yaw.push_back(0);
+//    diff_yaw.push_back(0);
 
     predict_traj_.reset(optimize_trajectory, 1);
     selected_traj_.reset(selected_trajectory, 1);
@@ -1134,7 +1134,7 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
 
 void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, 0,space_id, distance);
+        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -1144,17 +1144,17 @@ void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angul
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) {
-    if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, yaw_diff ,0,space_id, distance);
-        if (type == eRotation)
-            RWheel_position_ = eR;
-        if (type == eVertical)
-            RWheel_position_ = eX;
-        if (type == eHorizontal)
-            RWheel_position_ = eY;
-    }
-}
+//void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) {
+//    if (monitor_) {
+//        monitor_->set_ToAgvCmd(mode, type, v, angular, yaw_diff ,0,space_id, distance);
+//        if (type == eRotation)
+//            RWheel_position_ = eR;
+//        if (type == eVertical)
+//            RWheel_position_ = eX;
+//        if (type == eHorizontal)
+//            RWheel_position_ = eY;
+//    }
+//}
 
 bool Navigation::clamp_close() {
     if (monitor_) {
@@ -1335,10 +1335,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
 
         //一次变速
         std::vector<double> out;
-        std::vector<double> diff_yaw; //两车分别与整车的相对角度
+//        std::vector<double> diff_yaw; //两车分别与整车的相对角度
         bool ret;
         TimerRecord::Execute([&, this]() {
-            ret = mpc_once(traj, limit_v, out, diff_yaw, directY);
+            ret = mpc_once(traj, limit_v, out, directY);
         }, "mpc_once");
         if (ret == false) {
             Stop();
@@ -1397,11 +1397,11 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             std::string id = node.id().substr(1,node.id().length()-1);
             space_id = stoi(id);
         }
-        double diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]};
+//        double diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]};
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, down_v, down_w, diff_yaw_, space_id, distance);
+            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
         else
-            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, diff_yaw_, space_id, distance);
+            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
         actionType_ = directY ? eHorizontal : eVertical;
 
         //日志打印

MPC/navigation.h

@@ -113,7 +113,7 @@ protected:
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
+//    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
 
@@ -175,7 +175,7 @@ protected:
 
     virtual bool lifter_down();
 
-    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, std::vector<double> yaw_diff, bool directY = false);
+    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
 
     /* 原地旋转mpc
      *

MPC/navigation_main.cpp

@@ -36,9 +36,9 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
 
         Navigation::ResetPose(agv);
-        timedPose_().m_diffYaw1=pose.theta()-theta;
-        timedPose_().m_diffYaw2=brother.theta()-theta;
-        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);
+//        timedPose_().m_diffYaw1=pose.theta()-theta;
+//        timedPose_().m_diffYaw2=brother.theta()-theta;
+//        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);
 
     } else
         Navigation::ResetPose(pose);
@@ -68,6 +68,18 @@ void NavigationMain::SendMoveCmd(int mode, ActionType type,
     }
 }
 
+void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance) {
+    if (monitor_) {
+        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_, space_id, distance);
+        if (type == eRotation)
+            RWheel_position_ = eR;
+        if (type == eVertical)
+            RWheel_position_ = eX;
+        if (type == eHorizontal)
+            RWheel_position_ = eY;
+    }
+}
+
 bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
     if (Navigation::CreateRobotStatuMsg(robotStatu)) {
         robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());

MPC/navigation_main.h

@@ -71,6 +71,7 @@ protected:
     RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
+    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], int space_id, double distance);
 
     virtual bool clamp_close();
 

MPC/pose2d.cpp

@@ -10,8 +10,8 @@
 Pose2d::Pose2d()
         :m_x(0),m_y(0),m_theta(0)
 {
-    m_diffYaw1=0;
-    m_diffYaw2=0;
+//    m_diffYaw1=0;
+//    m_diffYaw2=0;
 }
 Pose2d::Pose2d(float x,float y,float theta)
         :m_x(x),m_y(y)

MPC/pose2d.h

@@ -128,9 +128,9 @@ class Pose2d
       poses.push_back(pose*Pose2d(0,h/2.0,pose.theta()));
       return poses;
     }
-public:
-    float         m_diffYaw1;
-    float           m_diffYaw2;
+//public:
+//    float         m_diffYaw1;
+//    float           m_diffYaw2;
  protected:
     float                     m_x;
     float                     m_y;

define/timedlockdata.hpp

@@ -15,7 +15,7 @@ class TimedLockData
     void reset(const T& tdata,double timeout=0.1);
     bool timeout();
     T Get();
-    T& operator()();
+//    T& operator()();
 
  protected:
     T data_;
@@ -55,11 +55,11 @@ T TimedLockData<T>::Get()
   std::lock_guard<std::mutex> lck (mutex_);
   return data_;
 }
-template <typename T>
-T& TimedLockData<T>::operator()(void ){
-    std::lock_guard<std::mutex> lck (mutex_);
-    return data_;
-}
+//template <typename T>
+//T& TimedLockData<T>::operator()(void ){
+//    std::lock_guard<std::mutex> lck (mutex_);
+//    return data_;
+//}
 
 
 #endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

message.proto

@@ -32,9 +32,7 @@ message ToAgvCmd {
     float L1 = 10;  //轴距
     int32 P1 = 11; //车位编号
     float D1 = 12; //距目标点距离
-    float Y1 = 13; //自己角度和整车角度的差值
-    float Y2 = 14; //brother角度和整车角度的差值
-    int32 end = 15;
+    int32 end = 13;
 }
 
 message Pose2d