1.修复旋转时应下发角速度，实则下发角度bug

2.修改通信协议，添加下发两车各自与整车角度
3.添加夹持杆全开位判定
4.修改进库位点后进入车角度基准，由前车角度改为车位点角度

MPC/custom_type.h

@@ -100,7 +100,8 @@ enum ActionType {
     eClampClose = 5,
     eClampOpen = 6,
     eLifterRise = 7,    //提升机构上升
-    eLifterDown =8
+    eLifterDown =8,
+    eClampFullyOpen = 9
 };
 ///////////////////////////////////////////////
 

MPC/monitor/emqx/message.pb.cc

@@ -72,21 +72,33 @@ PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
 template <typename>
 PROTOBUF_CONSTEXPR ToAgvCmd::ToAgvCmd(
     ::_pbi::ConstantInitialized): _impl_{
-    /*decltype(_impl_.h_)*/ 0
+    /*decltype(_impl_.h1_)*/ 0
 
-  , /*decltype(_impl_.m_)*/ 0
+  , /*decltype(_impl_.m1_)*/ 0
 
-  , /*decltype(_impl_.t_)*/ 0
+  , /*decltype(_impl_.t1_)*/ 0
 
-  , /*decltype(_impl_.v_)*/ 0
+  , /*decltype(_impl_.v1_)*/ 0
 
-  , /*decltype(_impl_.w_)*/ 0
+  , /*decltype(_impl_.w1_)*/ 0
 
-  , /*decltype(_impl_.l_)*/ 0
+  , /*decltype(_impl_.v2_)*/ 0
+
+  , /*decltype(_impl_.w2_)*/ 0
+
+  , /*decltype(_impl_.v3_)*/ 0
 
-  , /*decltype(_impl_.p_)*/ 0
+  , /*decltype(_impl_.w3_)*/ 0
 
-  , /*decltype(_impl_.d_)*/ 0
+  , /*decltype(_impl_.l1_)*/ 0
+
+  , /*decltype(_impl_.p1_)*/ 0
+
+  , /*decltype(_impl_.d1_)*/ 0
+
+  , /*decltype(_impl_.y1_)*/ 0
+
+  , /*decltype(_impl_.y2_)*/ 0
 
   , /*decltype(_impl_.end_)*/ 0
 
@@ -351,14 +363,20 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     ~0u,  // no _inlined_string_donated_
     ~0u,  // no _split_
     ~0u,  // no sizeof(Split)
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.h_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.m_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.t_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.v_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.w_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.l_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.p_),
-    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.d_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.h1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.m1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.t1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.v1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.w1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.v2_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.w2_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.v3_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.w3_),
+    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_),
@@ -497,15 +515,15 @@ static const ::_pbi::MigrationSchema
         { 0, -1, -1, sizeof(::NavMessage::LidarOdomStatu)},
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
         { 27, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
-        { 44, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 55, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 69, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 78, 93, -1, sizeof(::NavMessage::NewAction)},
-        { 100, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 111, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 121, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 132, 149, -1, sizeof(::NavMessage::NavStatu)},
-        { 158, 170, -1, sizeof(::NavMessage::RobotStatu)},
+        { 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)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -528,44 +546,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\"o\n\010ToAgvCmd\022\t\n\001H\030\001 \001(\005\022\t\n"
-    "\001M\030\002 \001(\005\022\t\n\001T\030\003 \001(\005\022\t\n\001V\030\004 \001(\002\022\t\n\001W\030\005 \001("
-    "\002\022\t\n\001L\030\006 \001(\002\022\t\n\001P\030\007 \001(\005\022\t\n\001D\030\010 \001(\002\022\013\n\003en"
-    "d\030\t \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\005poses"
-    "\030\001 \003(\0132\022.NavMessage.Pose2d\"\345\001\n\tNewAction"
-    "\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001(\0132\024.NavM"
-    "essage.PathNode\022&\n\010passNode\030\003 \001(\0132\024.NavM"
-    "essage.PathNode\022(\n\nstreetNode\030\004 \001(\0132\024.Na"
-    "vMessage.PathNode\022\'\n\tpathNodes\030\005 \003(\0132\024.N"
-    "avMessage.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\003r"
-    "et\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManualCmd\022\013\n\003k"
-    "ey\030\001 \001(\t\022\026\n\016operation_type\030\002 \001(\005\022\020\n\010velo"
-    "city\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\004o"
-    "dom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013\n"
-    "\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.NavM"
-    "essage.Trajectory\022,\n\014predict_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\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\010agvSta"
-    "tu\030\004 \001(\0132\024.NavMessage.AgvStatu2\302\001\n\nNavEx"
-    "cutor\0226\n\005Start\022\022.NavMessage.NavCmd\032\027.Nav"
-    "Message.NavResponse\"\000\0227\n\006Cancel\022\022.NavMes"
-    "sage.NavCmd\032\027.NavMessage.NavResponse\"\000\022C"
-    "\n\017ManualOperation\022\025.NavMessage.ManualCmd"
-    "\032\027.NavMessage.NavResponse\"\000b\006proto3"
+    "er_other\030\006 \001(\005\"\277\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"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1475,
+    1556,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -1299,21 +1319,33 @@ ToAgvCmd::ToAgvCmd(const ToAgvCmd& from)
 inline void ToAgvCmd::SharedCtor(::_pb::Arena* arena) {
   (void)arena;
   new (&_impl_) Impl_{
-      decltype(_impl_.h_) { 0 }
+      decltype(_impl_.h1_) { 0 }
 
-    , decltype(_impl_.m_) { 0 }
+    , decltype(_impl_.m1_) { 0 }
 
-    , decltype(_impl_.t_) { 0 }
+    , decltype(_impl_.t1_) { 0 }
 
-    , decltype(_impl_.v_) { 0 }
+    , decltype(_impl_.v1_) { 0 }
 
-    , decltype(_impl_.w_) { 0 }
+    , decltype(_impl_.w1_) { 0 }
 
-    , decltype(_impl_.l_) { 0 }
+    , decltype(_impl_.v2_) { 0 }
+
+    , decltype(_impl_.w2_) { 0 }
+
+    , decltype(_impl_.v3_) { 0 }
+
+    , decltype(_impl_.w3_) { 0 }
 
-    , decltype(_impl_.p_) { 0 }
+    , decltype(_impl_.l1_) { 0 }
 
-    , decltype(_impl_.d_) { 0 }
+    , decltype(_impl_.p1_) { 0 }
+
+    , decltype(_impl_.d1_) { 0 }
+
+    , decltype(_impl_.y1_) { 0 }
+
+    , decltype(_impl_.y2_) { 0 }
 
     , decltype(_impl_.end_) { 0 }
 
@@ -1344,9 +1376,9 @@ void ToAgvCmd::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  ::memset(&_impl_.h_, 0, static_cast<::size_t>(
+  ::memset(&_impl_.h1_, 0, static_cast<::size_t>(
       reinterpret_cast<char*>(&_impl_.end_) -
-      reinterpret_cast<char*>(&_impl_.h_)) + sizeof(_impl_.end_));
+      reinterpret_cast<char*>(&_impl_.h1_)) + sizeof(_impl_.end_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1356,81 +1388,135 @@ const char* ToAgvCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
     ::uint32_t tag;
     ptr = ::_pbi::ReadTag(ptr, &tag);
     switch (tag >> 3) {
-      // int32 H = 1;
+      // int32 H1 = 1;
       case 1:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 8)) {
-          _impl_.h_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          _impl_.h1_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
           goto handle_unusual;
         }
         continue;
-      // int32 M = 2;
+      // int32 M1 = 2;
       case 2:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 16)) {
-          _impl_.m_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          _impl_.m1_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
           goto handle_unusual;
         }
         continue;
-      // int32 T = 3;
+      // int32 T1 = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 24)) {
-          _impl_.t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          _impl_.t1_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
           goto handle_unusual;
         }
         continue;
-      // float V = 4;
+      // float V1 = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 37)) {
-          _impl_.v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          _impl_.v1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else {
           goto handle_unusual;
         }
         continue;
-      // float W = 5;
+      // float W1 = 5;
       case 5:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 45)) {
-          _impl_.w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          _impl_.w1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else {
           goto handle_unusual;
         }
         continue;
-      // float L = 6;
+      // float V2 = 6;
       case 6:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 53)) {
-          _impl_.l_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          _impl_.v2_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else {
           goto handle_unusual;
         }
         continue;
-      // int32 P = 7;
+      // float W2 = 7;
       case 7:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 56)) {
-          _impl_.p_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
-          CHK_(ptr);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 61)) {
+          _impl_.w2_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
         } else {
           goto handle_unusual;
         }
         continue;
-      // float D = 8;
+      // float V3 = 8;
       case 8:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 69)) {
-          _impl_.d_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          _impl_.v3_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else {
           goto handle_unusual;
         }
         continue;
-      // int32 end = 9;
+      // float W3 = 9;
       case 9:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 72)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 77)) {
+          _impl_.w3_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float L1 = 10;
+      case 10:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 85)) {
+          _impl_.l1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 P1 = 11;
+      case 11:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 88)) {
+          _impl_.p1_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float D1 = 12;
+      case 12:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 101)) {
+          _impl_.d1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float Y1 = 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)) {
           _impl_.end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
@@ -1466,83 +1552,149 @@ failure:
   ::uint32_t cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // int32 H = 1;
-  if (this->_internal_h() != 0) {
+  // int32 H1 = 1;
+  if (this->_internal_h1() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        1, this->_internal_h(), target);
+        1, this->_internal_h1(), target);
   }
 
-  // int32 M = 2;
-  if (this->_internal_m() != 0) {
+  // int32 M1 = 2;
+  if (this->_internal_m1() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        2, this->_internal_m(), target);
+        2, this->_internal_m1(), target);
   }
 
-  // int32 T = 3;
-  if (this->_internal_t() != 0) {
+  // int32 T1 = 3;
+  if (this->_internal_t1() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        3, this->_internal_t(), target);
+        3, this->_internal_t1(), target);
   }
 
-  // float V = 4;
+  // float V1 = 4;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_v = this->_internal_v();
-  ::uint32_t raw_v;
-  memcpy(&raw_v, &tmp_v, sizeof(tmp_v));
-  if (raw_v != 0) {
+  float tmp_v1 = this->_internal_v1();
+  ::uint32_t raw_v1;
+  memcpy(&raw_v1, &tmp_v1, sizeof(tmp_v1));
+  if (raw_v1 != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteFloatToArray(
-        4, this->_internal_v(), target);
+        4, this->_internal_v1(), target);
   }
 
-  // float W = 5;
+  // float W1 = 5;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_w = this->_internal_w();
-  ::uint32_t raw_w;
-  memcpy(&raw_w, &tmp_w, sizeof(tmp_w));
-  if (raw_w != 0) {
+  float tmp_w1 = this->_internal_w1();
+  ::uint32_t raw_w1;
+  memcpy(&raw_w1, &tmp_w1, sizeof(tmp_w1));
+  if (raw_w1 != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteFloatToArray(
-        5, this->_internal_w(), target);
+        5, this->_internal_w1(), target);
   }
 
-  // float L = 6;
+  // float V2 = 6;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_l = this->_internal_l();
-  ::uint32_t raw_l;
-  memcpy(&raw_l, &tmp_l, sizeof(tmp_l));
-  if (raw_l != 0) {
+  float tmp_v2 = this->_internal_v2();
+  ::uint32_t raw_v2;
+  memcpy(&raw_v2, &tmp_v2, sizeof(tmp_v2));
+  if (raw_v2 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        6, this->_internal_v2(), target);
+  }
+
+  // float W2 = 7;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w2 = this->_internal_w2();
+  ::uint32_t raw_w2;
+  memcpy(&raw_w2, &tmp_w2, sizeof(tmp_w2));
+  if (raw_w2 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        7, this->_internal_w2(), target);
+  }
+
+  // float V3 = 8;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_v3 = this->_internal_v3();
+  ::uint32_t raw_v3;
+  memcpy(&raw_v3, &tmp_v3, sizeof(tmp_v3));
+  if (raw_v3 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        8, this->_internal_v3(), target);
+  }
+
+  // float W3 = 9;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w3 = this->_internal_w3();
+  ::uint32_t raw_w3;
+  memcpy(&raw_w3, &tmp_w3, sizeof(tmp_w3));
+  if (raw_w3 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        9, this->_internal_w3(), target);
+  }
+
+  // float L1 = 10;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_l1 = this->_internal_l1();
+  ::uint32_t raw_l1;
+  memcpy(&raw_l1, &tmp_l1, sizeof(tmp_l1));
+  if (raw_l1 != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteFloatToArray(
-        6, this->_internal_l(), target);
+        10, this->_internal_l1(), target);
   }
 
-  // int32 P = 7;
-  if (this->_internal_p() != 0) {
+  // int32 P1 = 11;
+  if (this->_internal_p1() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        7, this->_internal_p(), target);
+        11, this->_internal_p1(), target);
+  }
+
+  // float D1 = 12;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d1 = this->_internal_d1();
+  ::uint32_t raw_d1;
+  memcpy(&raw_d1, &tmp_d1, sizeof(tmp_d1));
+  if (raw_d1 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        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 D = 8;
+  // float Y2 = 14;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_d = this->_internal_d();
-  ::uint32_t raw_d;
-  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
-  if (raw_d != 0) {
+  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(
-        8, this->_internal_d(), target);
+        14, this->_internal_y2(), target);
   }
 
-  // int32 end = 9;
+  // int32 end = 15;
   if (this->_internal_end() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        9, this->_internal_end(), target);
+        15, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1561,67 +1713,121 @@ failure:
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // int32 H = 1;
-  if (this->_internal_h() != 0) {
+  // int32 H1 = 1;
+  if (this->_internal_h1() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
-        this->_internal_h());
+        this->_internal_h1());
   }
 
-  // int32 M = 2;
-  if (this->_internal_m() != 0) {
+  // int32 M1 = 2;
+  if (this->_internal_m1() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
-        this->_internal_m());
+        this->_internal_m1());
   }
 
-  // int32 T = 3;
-  if (this->_internal_t() != 0) {
+  // int32 T1 = 3;
+  if (this->_internal_t1() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
-        this->_internal_t());
+        this->_internal_t1());
   }
 
-  // float V = 4;
+  // float V1 = 4;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_v = this->_internal_v();
-  ::uint32_t raw_v;
-  memcpy(&raw_v, &tmp_v, sizeof(tmp_v));
-  if (raw_v != 0) {
+  float tmp_v1 = this->_internal_v1();
+  ::uint32_t raw_v1;
+  memcpy(&raw_v1, &tmp_v1, sizeof(tmp_v1));
+  if (raw_v1 != 0) {
     total_size += 5;
   }
 
-  // float W = 5;
+  // float W1 = 5;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_w = this->_internal_w();
-  ::uint32_t raw_w;
-  memcpy(&raw_w, &tmp_w, sizeof(tmp_w));
-  if (raw_w != 0) {
+  float tmp_w1 = this->_internal_w1();
+  ::uint32_t raw_w1;
+  memcpy(&raw_w1, &tmp_w1, sizeof(tmp_w1));
+  if (raw_w1 != 0) {
     total_size += 5;
   }
 
-  // float L = 6;
+  // float V2 = 6;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_l = this->_internal_l();
-  ::uint32_t raw_l;
-  memcpy(&raw_l, &tmp_l, sizeof(tmp_l));
-  if (raw_l != 0) {
+  float tmp_v2 = this->_internal_v2();
+  ::uint32_t raw_v2;
+  memcpy(&raw_v2, &tmp_v2, sizeof(tmp_v2));
+  if (raw_v2 != 0) {
+    total_size += 5;
+  }
+
+  // float W2 = 7;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w2 = this->_internal_w2();
+  ::uint32_t raw_w2;
+  memcpy(&raw_w2, &tmp_w2, sizeof(tmp_w2));
+  if (raw_w2 != 0) {
+    total_size += 5;
+  }
+
+  // float V3 = 8;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_v3 = this->_internal_v3();
+  ::uint32_t raw_v3;
+  memcpy(&raw_v3, &tmp_v3, sizeof(tmp_v3));
+  if (raw_v3 != 0) {
+    total_size += 5;
+  }
+
+  // float W3 = 9;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w3 = this->_internal_w3();
+  ::uint32_t raw_w3;
+  memcpy(&raw_w3, &tmp_w3, sizeof(tmp_w3));
+  if (raw_w3 != 0) {
+    total_size += 5;
+  }
+
+  // float L1 = 10;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_l1 = this->_internal_l1();
+  ::uint32_t raw_l1;
+  memcpy(&raw_l1, &tmp_l1, sizeof(tmp_l1));
+  if (raw_l1 != 0) {
     total_size += 5;
   }
 
-  // int32 P = 7;
-  if (this->_internal_p() != 0) {
+  // int32 P1 = 11;
+  if (this->_internal_p1() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
-        this->_internal_p());
+        this->_internal_p1());
+  }
+
+  // float D1 = 12;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d1 = this->_internal_d1();
+  ::uint32_t raw_d1;
+  memcpy(&raw_d1, &tmp_d1, sizeof(tmp_d1));
+  if (raw_d1 != 0) {
+    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 D = 8;
+  // float Y2 = 14;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_d = this->_internal_d();
-  ::uint32_t raw_d;
-  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
-  if (raw_d != 0) {
+  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 = 9;
+  // int32 end = 15;
   if (this->_internal_end() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
         this->_internal_end());
@@ -1645,45 +1851,87 @@ void ToAgvCmd::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   ::uint32_t cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (from._internal_h() != 0) {
-    _this->_internal_set_h(from._internal_h());
+  if (from._internal_h1() != 0) {
+    _this->_internal_set_h1(from._internal_h1());
   }
-  if (from._internal_m() != 0) {
-    _this->_internal_set_m(from._internal_m());
+  if (from._internal_m1() != 0) {
+    _this->_internal_set_m1(from._internal_m1());
   }
-  if (from._internal_t() != 0) {
-    _this->_internal_set_t(from._internal_t());
+  if (from._internal_t1() != 0) {
+    _this->_internal_set_t1(from._internal_t1());
   }
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_v = from._internal_v();
-  ::uint32_t raw_v;
-  memcpy(&raw_v, &tmp_v, sizeof(tmp_v));
-  if (raw_v != 0) {
-    _this->_internal_set_v(from._internal_v());
+  float tmp_v1 = from._internal_v1();
+  ::uint32_t raw_v1;
+  memcpy(&raw_v1, &tmp_v1, sizeof(tmp_v1));
+  if (raw_v1 != 0) {
+    _this->_internal_set_v1(from._internal_v1());
   }
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_w = from._internal_w();
-  ::uint32_t raw_w;
-  memcpy(&raw_w, &tmp_w, sizeof(tmp_w));
-  if (raw_w != 0) {
-    _this->_internal_set_w(from._internal_w());
+  float tmp_w1 = from._internal_w1();
+  ::uint32_t raw_w1;
+  memcpy(&raw_w1, &tmp_w1, sizeof(tmp_w1));
+  if (raw_w1 != 0) {
+    _this->_internal_set_w1(from._internal_w1());
   }
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
-  float tmp_l = from._internal_l();
-  ::uint32_t raw_l;
-  memcpy(&raw_l, &tmp_l, sizeof(tmp_l));
-  if (raw_l != 0) {
-    _this->_internal_set_l(from._internal_l());
+  float tmp_v2 = from._internal_v2();
+  ::uint32_t raw_v2;
+  memcpy(&raw_v2, &tmp_v2, sizeof(tmp_v2));
+  if (raw_v2 != 0) {
+    _this->_internal_set_v2(from._internal_v2());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w2 = from._internal_w2();
+  ::uint32_t raw_w2;
+  memcpy(&raw_w2, &tmp_w2, sizeof(tmp_w2));
+  if (raw_w2 != 0) {
+    _this->_internal_set_w2(from._internal_w2());
   }
-  if (from._internal_p() != 0) {
-    _this->_internal_set_p(from._internal_p());
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_v3 = from._internal_v3();
+  ::uint32_t raw_v3;
+  memcpy(&raw_v3, &tmp_v3, sizeof(tmp_v3));
+  if (raw_v3 != 0) {
+    _this->_internal_set_v3(from._internal_v3());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_w3 = from._internal_w3();
+  ::uint32_t raw_w3;
+  memcpy(&raw_w3, &tmp_w3, sizeof(tmp_w3));
+  if (raw_w3 != 0) {
+    _this->_internal_set_w3(from._internal_w3());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_l1 = from._internal_l1();
+  ::uint32_t raw_l1;
+  memcpy(&raw_l1, &tmp_l1, sizeof(tmp_l1));
+  if (raw_l1 != 0) {
+    _this->_internal_set_l1(from._internal_l1());
+  }
+  if (from._internal_p1() != 0) {
+    _this->_internal_set_p1(from._internal_p1());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d1 = from._internal_d1();
+  ::uint32_t raw_d1;
+  memcpy(&raw_d1, &tmp_d1, sizeof(tmp_d1));
+  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_d = from._internal_d();
-  ::uint32_t raw_d;
-  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
-  if (raw_d != 0) {
-    _this->_internal_set_d(from._internal_d());
+  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());
@@ -1708,9 +1956,9 @@ void ToAgvCmd::InternalSwap(ToAgvCmd* other) {
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
       PROTOBUF_FIELD_OFFSET(ToAgvCmd, _impl_.end_)
       + sizeof(ToAgvCmd::_impl_.end_)
-      - PROTOBUF_FIELD_OFFSET(ToAgvCmd, _impl_.h_)>(
-          reinterpret_cast<char*>(&_impl_.h_),
-          reinterpret_cast<char*>(&other->_impl_.h_));
+      - PROTOBUF_FIELD_OFFSET(ToAgvCmd, _impl_.h1_)>(
+          reinterpret_cast<char*>(&_impl_.h1_),
+          reinterpret_cast<char*>(&other->_impl_.h1_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata ToAgvCmd::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -668,97 +668,163 @@ class ToAgvCmd final :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kHFieldNumber = 1,
-    kMFieldNumber = 2,
-    kTFieldNumber = 3,
-    kVFieldNumber = 4,
-    kWFieldNumber = 5,
-    kLFieldNumber = 6,
-    kPFieldNumber = 7,
-    kDFieldNumber = 8,
-    kEndFieldNumber = 9,
+    kH1FieldNumber = 1,
+    kM1FieldNumber = 2,
+    kT1FieldNumber = 3,
+    kV1FieldNumber = 4,
+    kW1FieldNumber = 5,
+    kV2FieldNumber = 6,
+    kW2FieldNumber = 7,
+    kV3FieldNumber = 8,
+    kW3FieldNumber = 9,
+    kL1FieldNumber = 10,
+    kP1FieldNumber = 11,
+    kD1FieldNumber = 12,
+    kY1FieldNumber = 13,
+    kY2FieldNumber = 14,
+    kEndFieldNumber = 15,
   };
-  // int32 H = 1;
-  void clear_h() ;
-  ::int32_t h() const;
-  void set_h(::int32_t value);
+  // int32 H1 = 1;
+  void clear_h1() ;
+  ::int32_t h1() const;
+  void set_h1(::int32_t value);
 
   private:
-  ::int32_t _internal_h() const;
-  void _internal_set_h(::int32_t value);
+  ::int32_t _internal_h1() const;
+  void _internal_set_h1(::int32_t value);
 
   public:
-  // int32 M = 2;
-  void clear_m() ;
-  ::int32_t m() const;
-  void set_m(::int32_t value);
+  // int32 M1 = 2;
+  void clear_m1() ;
+  ::int32_t m1() const;
+  void set_m1(::int32_t value);
 
   private:
-  ::int32_t _internal_m() const;
-  void _internal_set_m(::int32_t value);
+  ::int32_t _internal_m1() const;
+  void _internal_set_m1(::int32_t value);
 
   public:
-  // int32 T = 3;
-  void clear_t() ;
-  ::int32_t t() const;
-  void set_t(::int32_t value);
+  // int32 T1 = 3;
+  void clear_t1() ;
+  ::int32_t t1() const;
+  void set_t1(::int32_t value);
 
   private:
-  ::int32_t _internal_t() const;
-  void _internal_set_t(::int32_t value);
+  ::int32_t _internal_t1() const;
+  void _internal_set_t1(::int32_t value);
 
   public:
-  // float V = 4;
-  void clear_v() ;
-  float v() const;
-  void set_v(float value);
+  // float V1 = 4;
+  void clear_v1() ;
+  float v1() const;
+  void set_v1(float value);
 
   private:
-  float _internal_v() const;
-  void _internal_set_v(float value);
+  float _internal_v1() const;
+  void _internal_set_v1(float value);
 
   public:
-  // float W = 5;
-  void clear_w() ;
-  float w() const;
-  void set_w(float value);
+  // float W1 = 5;
+  void clear_w1() ;
+  float w1() const;
+  void set_w1(float value);
 
   private:
-  float _internal_w() const;
-  void _internal_set_w(float value);
+  float _internal_w1() const;
+  void _internal_set_w1(float value);
 
   public:
-  // float L = 6;
-  void clear_l() ;
-  float l() const;
-  void set_l(float value);
+  // float V2 = 6;
+  void clear_v2() ;
+  float v2() const;
+  void set_v2(float value);
 
   private:
-  float _internal_l() const;
-  void _internal_set_l(float value);
+  float _internal_v2() const;
+  void _internal_set_v2(float value);
+
+  public:
+  // float W2 = 7;
+  void clear_w2() ;
+  float w2() const;
+  void set_w2(float value);
+
+  private:
+  float _internal_w2() const;
+  void _internal_set_w2(float value);
+
+  public:
+  // float V3 = 8;
+  void clear_v3() ;
+  float v3() const;
+  void set_v3(float value);
+
+  private:
+  float _internal_v3() const;
+  void _internal_set_v3(float value);
+
+  public:
+  // float W3 = 9;
+  void clear_w3() ;
+  float w3() const;
+  void set_w3(float value);
+
+  private:
+  float _internal_w3() const;
+  void _internal_set_w3(float value);
+
+  public:
+  // float L1 = 10;
+  void clear_l1() ;
+  float l1() const;
+  void set_l1(float value);
+
+  private:
+  float _internal_l1() const;
+  void _internal_set_l1(float value);
 
   public:
-  // int32 P = 7;
-  void clear_p() ;
-  ::int32_t p() const;
-  void set_p(::int32_t value);
+  // int32 P1 = 11;
+  void clear_p1() ;
+  ::int32_t p1() const;
+  void set_p1(::int32_t value);
 
   private:
-  ::int32_t _internal_p() const;
-  void _internal_set_p(::int32_t value);
+  ::int32_t _internal_p1() const;
+  void _internal_set_p1(::int32_t value);
 
   public:
-  // float D = 8;
-  void clear_d() ;
-  float d() const;
-  void set_d(float value);
+  // float D1 = 12;
+  void clear_d1() ;
+  float d1() const;
+  void set_d1(float value);
 
   private:
-  float _internal_d() const;
-  void _internal_set_d(float value);
+  float _internal_d1() const;
+  void _internal_set_d1(float value);
 
   public:
-  // int32 end = 9;
+  // 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;
   void clear_end() ;
   ::int32_t end() const;
   void set_end(::int32_t value);
@@ -776,14 +842,20 @@ class ToAgvCmd final :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   struct Impl_ {
-    ::int32_t h_;
-    ::int32_t m_;
-    ::int32_t t_;
-    float v_;
-    float w_;
-    float l_;
-    ::int32_t p_;
-    float d_;
+    ::int32_t h1_;
+    ::int32_t m1_;
+    ::int32_t t1_;
+    float v1_;
+    float w1_;
+    float v2_;
+    float w2_;
+    float v3_;
+    float w3_;
+    float l1_;
+    ::int32_t p1_;
+    float d1_;
+    float y1_;
+    float y2_;
     ::int32_t end_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
@@ -2899,167 +2971,287 @@ inline void AgvStatu::_internal_set_lifter_other(::int32_t value) {
 
 // ToAgvCmd
 
-// int32 H = 1;
-inline void ToAgvCmd::clear_h() {
-  _impl_.h_ = 0;
+// int32 H1 = 1;
+inline void ToAgvCmd::clear_h1() {
+  _impl_.h1_ = 0;
 }
-inline ::int32_t ToAgvCmd::h() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.H)
-  return _internal_h();
+inline ::int32_t ToAgvCmd::h1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.H1)
+  return _internal_h1();
 }
-inline void ToAgvCmd::set_h(::int32_t value) {
-  _internal_set_h(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.H)
+inline void ToAgvCmd::set_h1(::int32_t value) {
+  _internal_set_h1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.H1)
 }
-inline ::int32_t ToAgvCmd::_internal_h() const {
-  return _impl_.h_;
+inline ::int32_t ToAgvCmd::_internal_h1() const {
+  return _impl_.h1_;
 }
-inline void ToAgvCmd::_internal_set_h(::int32_t value) {
+inline void ToAgvCmd::_internal_set_h1(::int32_t value) {
   ;
-  _impl_.h_ = value;
+  _impl_.h1_ = value;
 }
 
-// int32 M = 2;
-inline void ToAgvCmd::clear_m() {
-  _impl_.m_ = 0;
+// int32 M1 = 2;
+inline void ToAgvCmd::clear_m1() {
+  _impl_.m1_ = 0;
 }
-inline ::int32_t ToAgvCmd::m() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.M)
-  return _internal_m();
+inline ::int32_t ToAgvCmd::m1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.M1)
+  return _internal_m1();
 }
-inline void ToAgvCmd::set_m(::int32_t value) {
-  _internal_set_m(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.M)
+inline void ToAgvCmd::set_m1(::int32_t value) {
+  _internal_set_m1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.M1)
 }
-inline ::int32_t ToAgvCmd::_internal_m() const {
-  return _impl_.m_;
+inline ::int32_t ToAgvCmd::_internal_m1() const {
+  return _impl_.m1_;
 }
-inline void ToAgvCmd::_internal_set_m(::int32_t value) {
+inline void ToAgvCmd::_internal_set_m1(::int32_t value) {
   ;
-  _impl_.m_ = value;
+  _impl_.m1_ = value;
 }
 
-// int32 T = 3;
-inline void ToAgvCmd::clear_t() {
-  _impl_.t_ = 0;
+// int32 T1 = 3;
+inline void ToAgvCmd::clear_t1() {
+  _impl_.t1_ = 0;
 }
-inline ::int32_t ToAgvCmd::t() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.T)
-  return _internal_t();
+inline ::int32_t ToAgvCmd::t1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.T1)
+  return _internal_t1();
 }
-inline void ToAgvCmd::set_t(::int32_t value) {
-  _internal_set_t(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.T)
+inline void ToAgvCmd::set_t1(::int32_t value) {
+  _internal_set_t1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.T1)
 }
-inline ::int32_t ToAgvCmd::_internal_t() const {
-  return _impl_.t_;
+inline ::int32_t ToAgvCmd::_internal_t1() const {
+  return _impl_.t1_;
 }
-inline void ToAgvCmd::_internal_set_t(::int32_t value) {
+inline void ToAgvCmd::_internal_set_t1(::int32_t value) {
   ;
-  _impl_.t_ = value;
+  _impl_.t1_ = value;
 }
 
-// float V = 4;
-inline void ToAgvCmd::clear_v() {
-  _impl_.v_ = 0;
+// float V1 = 4;
+inline void ToAgvCmd::clear_v1() {
+  _impl_.v1_ = 0;
 }
-inline float ToAgvCmd::v() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.V)
-  return _internal_v();
+inline float ToAgvCmd::v1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.V1)
+  return _internal_v1();
 }
-inline void ToAgvCmd::set_v(float value) {
-  _internal_set_v(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.V)
+inline void ToAgvCmd::set_v1(float value) {
+  _internal_set_v1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.V1)
 }
-inline float ToAgvCmd::_internal_v() const {
-  return _impl_.v_;
+inline float ToAgvCmd::_internal_v1() const {
+  return _impl_.v1_;
 }
-inline void ToAgvCmd::_internal_set_v(float value) {
+inline void ToAgvCmd::_internal_set_v1(float value) {
   ;
-  _impl_.v_ = value;
+  _impl_.v1_ = value;
 }
 
-// float W = 5;
-inline void ToAgvCmd::clear_w() {
-  _impl_.w_ = 0;
+// float W1 = 5;
+inline void ToAgvCmd::clear_w1() {
+  _impl_.w1_ = 0;
 }
-inline float ToAgvCmd::w() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.W)
-  return _internal_w();
+inline float ToAgvCmd::w1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.W1)
+  return _internal_w1();
 }
-inline void ToAgvCmd::set_w(float value) {
-  _internal_set_w(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.W)
+inline void ToAgvCmd::set_w1(float value) {
+  _internal_set_w1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.W1)
 }
-inline float ToAgvCmd::_internal_w() const {
-  return _impl_.w_;
+inline float ToAgvCmd::_internal_w1() const {
+  return _impl_.w1_;
 }
-inline void ToAgvCmd::_internal_set_w(float value) {
+inline void ToAgvCmd::_internal_set_w1(float value) {
   ;
-  _impl_.w_ = value;
+  _impl_.w1_ = value;
 }
 
-// float L = 6;
-inline void ToAgvCmd::clear_l() {
-  _impl_.l_ = 0;
+// float V2 = 6;
+inline void ToAgvCmd::clear_v2() {
+  _impl_.v2_ = 0;
 }
-inline float ToAgvCmd::l() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.L)
-  return _internal_l();
+inline float ToAgvCmd::v2() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.V2)
+  return _internal_v2();
 }
-inline void ToAgvCmd::set_l(float value) {
-  _internal_set_l(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.L)
+inline void ToAgvCmd::set_v2(float value) {
+  _internal_set_v2(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.V2)
 }
-inline float ToAgvCmd::_internal_l() const {
-  return _impl_.l_;
+inline float ToAgvCmd::_internal_v2() const {
+  return _impl_.v2_;
 }
-inline void ToAgvCmd::_internal_set_l(float value) {
+inline void ToAgvCmd::_internal_set_v2(float value) {
   ;
-  _impl_.l_ = value;
+  _impl_.v2_ = value;
+}
+
+// float W2 = 7;
+inline void ToAgvCmd::clear_w2() {
+  _impl_.w2_ = 0;
+}
+inline float ToAgvCmd::w2() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.W2)
+  return _internal_w2();
+}
+inline void ToAgvCmd::set_w2(float value) {
+  _internal_set_w2(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.W2)
+}
+inline float ToAgvCmd::_internal_w2() const {
+  return _impl_.w2_;
+}
+inline void ToAgvCmd::_internal_set_w2(float value) {
+  ;
+  _impl_.w2_ = value;
+}
+
+// float V3 = 8;
+inline void ToAgvCmd::clear_v3() {
+  _impl_.v3_ = 0;
+}
+inline float ToAgvCmd::v3() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.V3)
+  return _internal_v3();
+}
+inline void ToAgvCmd::set_v3(float value) {
+  _internal_set_v3(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.V3)
+}
+inline float ToAgvCmd::_internal_v3() const {
+  return _impl_.v3_;
+}
+inline void ToAgvCmd::_internal_set_v3(float value) {
+  ;
+  _impl_.v3_ = value;
+}
+
+// float W3 = 9;
+inline void ToAgvCmd::clear_w3() {
+  _impl_.w3_ = 0;
+}
+inline float ToAgvCmd::w3() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.W3)
+  return _internal_w3();
+}
+inline void ToAgvCmd::set_w3(float value) {
+  _internal_set_w3(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.W3)
+}
+inline float ToAgvCmd::_internal_w3() const {
+  return _impl_.w3_;
+}
+inline void ToAgvCmd::_internal_set_w3(float value) {
+  ;
+  _impl_.w3_ = value;
+}
+
+// float L1 = 10;
+inline void ToAgvCmd::clear_l1() {
+  _impl_.l1_ = 0;
+}
+inline float ToAgvCmd::l1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.L1)
+  return _internal_l1();
+}
+inline void ToAgvCmd::set_l1(float value) {
+  _internal_set_l1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.L1)
+}
+inline float ToAgvCmd::_internal_l1() const {
+  return _impl_.l1_;
+}
+inline void ToAgvCmd::_internal_set_l1(float value) {
+  ;
+  _impl_.l1_ = value;
+}
+
+// int32 P1 = 11;
+inline void ToAgvCmd::clear_p1() {
+  _impl_.p1_ = 0;
+}
+inline ::int32_t ToAgvCmd::p1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.P1)
+  return _internal_p1();
+}
+inline void ToAgvCmd::set_p1(::int32_t value) {
+  _internal_set_p1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.P1)
+}
+inline ::int32_t ToAgvCmd::_internal_p1() const {
+  return _impl_.p1_;
+}
+inline void ToAgvCmd::_internal_set_p1(::int32_t value) {
+  ;
+  _impl_.p1_ = value;
+}
+
+// float D1 = 12;
+inline void ToAgvCmd::clear_d1() {
+  _impl_.d1_ = 0;
+}
+inline float ToAgvCmd::d1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.D1)
+  return _internal_d1();
+}
+inline void ToAgvCmd::set_d1(float value) {
+  _internal_set_d1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.D1)
+}
+inline float ToAgvCmd::_internal_d1() const {
+  return _impl_.d1_;
+}
+inline void ToAgvCmd::_internal_set_d1(float value) {
+  ;
+  _impl_.d1_ = value;
 }
 
-// int32 P = 7;
-inline void ToAgvCmd::clear_p() {
-  _impl_.p_ = 0;
+// float Y1 = 13;
+inline void ToAgvCmd::clear_y1() {
+  _impl_.y1_ = 0;
 }
-inline ::int32_t ToAgvCmd::p() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.P)
-  return _internal_p();
+inline float ToAgvCmd::y1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y1)
+  return _internal_y1();
 }
-inline void ToAgvCmd::set_p(::int32_t value) {
-  _internal_set_p(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.P)
+inline void ToAgvCmd::set_y1(float value) {
+  _internal_set_y1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y1)
 }
-inline ::int32_t ToAgvCmd::_internal_p() const {
-  return _impl_.p_;
+inline float ToAgvCmd::_internal_y1() const {
+  return _impl_.y1_;
 }
-inline void ToAgvCmd::_internal_set_p(::int32_t value) {
+inline void ToAgvCmd::_internal_set_y1(float value) {
   ;
-  _impl_.p_ = value;
+  _impl_.y1_ = value;
 }
 
-// float D = 8;
-inline void ToAgvCmd::clear_d() {
-  _impl_.d_ = 0;
+// float Y2 = 14;
+inline void ToAgvCmd::clear_y2() {
+  _impl_.y2_ = 0;
 }
-inline float ToAgvCmd::d() const {
-  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.D)
-  return _internal_d();
+inline float ToAgvCmd::y2() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y2)
+  return _internal_y2();
 }
-inline void ToAgvCmd::set_d(float value) {
-  _internal_set_d(value);
-  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.D)
+inline void ToAgvCmd::set_y2(float value) {
+  _internal_set_y2(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y2)
 }
-inline float ToAgvCmd::_internal_d() const {
-  return _impl_.d_;
+inline float ToAgvCmd::_internal_y2() const {
+  return _impl_.y2_;
 }
-inline void ToAgvCmd::_internal_set_d(float value) {
+inline void ToAgvCmd::_internal_set_y2(float value) {
   ;
-  _impl_.d_ = value;
+  _impl_.y2_ = value;
 }
 
-// int32 end = 9;
+// int32 end = 15;
 inline void ToAgvCmd::clear_end() {
   _impl_.end_ = 0;
 }

MPC/monitor/monitor_emqx.cpp

@@ -52,6 +52,20 @@ void Monitor_emqx::clamp_open(int mode)
   Publish(speedcmd_topic_,msg);
 }
 
+void Monitor_emqx::clamp_fully_open(int mode)
+{
+    MqttMsg msg;
+    NavMessage::ToAgvCmd speed;
+    heat_=(heat_+1)%255;
+    speed.set_h1(heat_);
+    speed.set_t1(eClampFullyOpen);
+
+    speed.set_m1(mode);
+    speed.set_end(1);
+    msg.fromProtoMessage(speed);
+    Publish(speedcmd_topic_,msg);
+}
+
 void Monitor_emqx::lifter_rise(int mode)
 {
     MqttMsg msg;
@@ -179,5 +193,44 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     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;
+//    }
+    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);
+}
+
 
 

MPC/monitor/monitor_emqx.h

@@ -18,8 +18,10 @@ 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 clamp_close(int mode);
     void clamp_open(int mode);
+    void clamp_fully_open(int mode);
     void lifter_rise(int mode);
     void lifter_down(int mode);
     void stop();

MPC/navigation.cpp

@@ -700,7 +700,9 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     //前车先到,后车进入2点,保持与前车一致的朝向
     if (brother.in_space() && brother.space_id() == space.id()) {
         printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
-        rotated.mutable_theta() = brother.odom().theta();
+//        rotated.mutable_theta() = brother.odom().theta();
+        Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
+        rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
         if (move_mode_ == eSingle) {
             x -= wheelbase * cos(rotated.theta());
             y -= wheelbase * sin(rotated.theta());
@@ -745,7 +747,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
@@ -822,6 +824,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             }
 
         }
+
+        if (space_id == 1101){
+            //单车进入口搬车，夹持杆打开到全开位
+
+        }
 //
 //        //移动到预入库点位，较高精度
 //        NavMessage::PathNode pre_target;
@@ -966,7 +973,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         out.push_back(0);
     } else {
 //        double min_angular_velocity = 1.0 / 180 * M_PI;
-        double min_angular_velocity = 0.00001;
+        double min_angular_velocity = 0.01;
         const int down_count = 3;
         for (int i = 0; i < down_count; ++i) {
             if (fabs(out[i]) < min_angular_velocity) {
@@ -981,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, bool directY) {
+bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, std::vector<double> diff_yaw, bool directY) {
     if (PoseTimeout() == true) {
         printf(" MPC once Error:Pose is timeout \n");
         return false;
@@ -999,6 +1006,9 @@ 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;
+
     Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
 
     statu[0] = pose.x();
@@ -1052,6 +1062,10 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         out.push_back(0);
         out.push_back(0);
     }
+    //
+    diff_yaw.push_back(diffYaw1);
+    diff_yaw.push_back(diffYaw2);
+
     predict_traj_.reset(optimize_trajectory, 1);
     selected_traj_.reset(selected_trajectory, 1);
     return ret == success || ret == no_solution;
@@ -1130,6 +1144,18 @@ 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;
+    }
+}
+
 bool Navigation::clamp_close() {
     if (monitor_) {
         printf("Clamp closing...\n");
@@ -1172,6 +1198,30 @@ bool Navigation::clamp_open() {
     return false;
 }
 
+
+bool Navigation::clamp_fully_open(){
+    if (monitor_){
+        printf("Clamp fully openning...\n");
+        monitor_->clamp_fully_open(move_mode_);
+        actionType_ = eClampFullyOpen;
+        while (cancel_ == false) {
+            if (timed_clamp_.timeout()) {
+                printf("timed clamp is timeout\n");
+                return false;
+            }
+            if (timed_clamp_.Get() == eFullyOpened)
+                return true;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            monitor_->clamp_fully_open(move_mode_);
+            actionType_ = eClampFullyOpen;
+        }
+        return false;
+    }
+    return false;
+
+}
+
+
 bool Navigation::lifter_rise() {
     if (monitor_) {
         printf("Lifter upping...\n");
@@ -1285,9 +1335,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
 
         //一次变速
         std::vector<double> out;
+        std::vector<double> diff_yaw; //两车分别与整车的相对角度
         bool ret;
         TimerRecord::Execute([&, this]() {
-            ret = mpc_once(traj, limit_v, out, directY);
+            ret = mpc_once(traj, limit_v, out, diff_yaw, directY);
         }, "mpc_once");
         if (ret == false) {
             Stop();
@@ -1318,12 +1369,12 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
 
         //入库，行进方向上最后一米不纠偏
         if (newUnfinished_cations_.front().type() == 1 && node.id().find('S')!=-1){//入库
-//            if(fabs(target_in_agv.x()) < 0.4){
-//                printf("target_in_agv: %f", target_in_agv.x());
-//                for (double & i : down_w) {
-//                    i = 0.0;
-//                }
-//            }
+            if(fabs(target_in_agv.x()) < 0.5){
+                printf("target_in_agv: %f", target_in_agv.x());
+                for (double & i : down_w) {
+                    i = 0.0;
+                }
+            }
         }else if (newUnfinished_cations_.front().type() == 2){//出库,行进方向上开始一米不纠偏
             Pose2d agv_in_space =
                     Pose2d::relativePose(
@@ -1346,10 +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]};
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eVertical, down_v, down_w, diff_yaw_, space_id, distance);
         else
-            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
+            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, diff_yaw_, space_id, distance);
         actionType_ = directY ? eHorizontal : eVertical;
 
         //日志打印

MPC/navigation.h

@@ -33,7 +33,8 @@ public:
     enum ClampStatu {
         eInvalid = 0,
         eClosed = 1,
-        eOpened = 2
+        eOpened = 2,    //半开位（默认）
+        eFullyOpened = 3 //全开位
     };
 
     enum MpcResult {
@@ -112,6 +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);
 
     static void RobotPoseCallback(const MqttMsg &msg, void *context);
 
@@ -167,11 +169,13 @@ protected:
 
     virtual bool clamp_open();
 
+    virtual bool clamp_fully_open();
+
     virtual bool lifter_rise();
 
     virtual bool lifter_down();
 
-    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
+    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, std::vector<double> yaw_diff, bool directY = false);
 
     /* 原地旋转mpc
      *

MPC/navigation_main.cpp

@@ -36,6 +36,10 @@ 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);
+
     } else
         Navigation::ResetPose(pose);
 }
@@ -273,27 +277,6 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
         continue;
 
-//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//    Pose2d current = timedPose_.Get();
-//    double yawDiff = (rotated - current).theta();
-//    if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-//      double theta = limit_gause(yawDiff, limit_rotate.min, limit_rotate.max);
-//      double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
-//      double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
-//
-//      SendMoveCmd(move_mode_, Monitor_emqx::eRotation, 0, limit_angular);
-//      actionType_ = eRotation;
-//      printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
-//             timedA_.Get(), angular, limit_angular, yawDiff);
-//
-//      continue;
-//    } else {
-//      if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
-//        printf(" RotateBeforeEnterSpace refer target completed\n");
-//        printf("---------------- update new target :%f %f %f \n",target.x(),target.y(),target.theta());
-//        return true;
-//      }
-//    }
     }
     return eMpcFailed;
 }

MPC/pose2d.cpp

@@ -10,6 +10,8 @@
 Pose2d::Pose2d()
         :m_x(0),m_y(0),m_theta(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,11 +128,14 @@ class Pose2d
       poses.push_back(pose*Pose2d(0,h/2.0,pose.theta()));
       return poses;
     }
-
+public:
+    float         m_diffYaw1;
+    float           m_diffYaw2;
  protected:
     float                     m_x;
     float                     m_y;
     float                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
+
 };
 
 

MPC/rotate_mpc.cpp

@@ -245,8 +245,9 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     out.clear();
     double solve_angular_velocity;
     for (int i = 0; i < down_count; ++i) {
-        solve_angular_velocity = solution.x[i + delay];
+        solve_angular_velocity = solution.x[i + N + delay];
         out.push_back(solve_angular_velocity);
+//        printf("P[%d]: %f\n", i, solve_angular_velocity);
     }
 
     return success;

define/timedlockdata.hpp

@@ -15,6 +15,7 @@ class TimedLockData
     void reset(const T& tdata,double timeout=0.1);
     bool timeout();
     T Get();
+    T& operator()();
 
  protected:
     T data_;
@@ -54,6 +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_;
+}
 
 
 #endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

message.proto

@@ -20,15 +20,21 @@ message AgvStatu {
 }
 
 message ToAgvCmd {
-    int32 H = 1;  //心跳
-    int32 M = 2;    //模式：2整车模式，1：单车
-    int32 T = 3; // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持, 7:提升机构抬升, 8:抬升机构下降, 其他/未接收到：停止
-    float V = 4;  //角速度
-    float W = 5;  //线速度
-    float L = 6;  //轴距
-    int32 P = 7; //车位编号
-    float D = 8; //距目标点距离
-    int32 end = 9;
+    int32 H1 = 1;  //心跳
+    int32 M1 = 2;    //模式：2整车模式，1：单车
+    int32 T1 = 3; // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持半开位, 7:提升机构抬升, 8:抬升机构下降, 9:松夹持全开位,其他/未接收到：停止
+    float V1 = 4;  //线速度
+    float W1 = 5;  //角速度
+    float V2 = 6;  //线速度
+    float W2 = 7;  //角速度
+    float V3 = 8;  //线速度
+    float W3 = 9;  //角速度
+    float L1 = 10;  //轴距
+    int32 P1 = 11; //车位编号
+    float D1 = 12; //距目标点距离
+    float Y1 = 13; //自己角度和整车角度的差值
+    float Y2 = 14; //brother角度和整车角度的差值
+    int32 end = 15;
 }
 
 message Pose2d