消息增加主从agv标志位，增加agv主从模式消息标志位，修复状态发布死循环问题

CMakeLists.txt

@@ -71,7 +71,7 @@ add_executable(${PROJECT_NAME}
 		tool/pathcreator.cpp
 		tool/singleton.cpp
 		tool/proto_tool.cpp
-       )
+        MPC/navigation_main.cpp MPC/navigation_main.h)
 
 #---------------------------------------------------------------------------------------
 # link libraries ${Boost_LIBRARY} -lpthread

MPC/loaded_mpc.cpp

@@ -250,7 +250,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     constraints_upperbound[i] = max_acc_dlt;
   }
   // 与障碍物保持距离
-  double dobs=2.0;
+  double dobs=2.2;
   for(int i=nobs;i<nobs+N;++i)
   {
     constraints_lowerbound[i] = dobs*dobs;//pow(float(nobs+N-i)/float(N)*dobs,2);

MPC/monitor/emqx/message.pb.cc

@@ -111,8 +111,9 @@ static void InitDefaultsscc_info_NavCmd_message_2eproto() {
   ::NavMessage::NavCmd::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_NavCmd_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavCmd_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+      &scc_info_Pose2d_message_2eproto.base,
       &scc_info_Action_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_NavStatu_message_2eproto() {
@@ -216,6 +217,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, m_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
@@ -260,6 +262,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, action_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, key_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, transform_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, actions_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, _internal_metadata_),
@@ -267,6 +270,8 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, statu_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, main_agv_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, move_mode_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, unfinished_actions_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
@@ -276,12 +281,12 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::AGVSpeed)},
   { 17, -1, sizeof(::NavMessage::Speed)},
-  { 27, -1, sizeof(::NavMessage::SpeedLimit)},
-  { 34, -1, sizeof(::NavMessage::Pose2d)},
-  { 42, -1, sizeof(::NavMessage::Trajectory)},
-  { 48, -1, sizeof(::NavMessage::Action)},
-  { 60, -1, sizeof(::NavMessage::NavCmd)},
-  { 68, -1, sizeof(::NavMessage::NavStatu)},
+  { 28, -1, sizeof(::NavMessage::SpeedLimit)},
+  { 35, -1, sizeof(::NavMessage::Pose2d)},
+  { 43, -1, sizeof(::NavMessage::Trajectory)},
+  { 49, -1, sizeof(::NavMessage::Action)},
+  { 61, -1, sizeof(::NavMessage::NavCmd)},
+  { 70, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -300,25 +305,28 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\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\t\n\001"
   "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\" \n\010AGVSpeed\022\t\n\001v\030\001 "
-  "\001(\002\022\t\n\001w\030\002 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005\022\t\n\001T\030"
-  "\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end\030\005 \001(\005"
-  "\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013\n\003max\030\002 \001(\002"
-  "\"-\n\006Pose2d\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\"/\n\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.N"
-  "avMessage.Pose2d\"\223\002\n\006Action\022\014\n\004type\030\001 \001("
-  "\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage.Pose2d\022\"\n\006"
-  "target\030\003 \001(\0132\022.NavMessage.Pose2d\022\'\n\013targ"
-  "et_diff\030\004 \001(\0132\022.NavMessage.Pose2d\022.\n\016vel"
-  "ocity_limit\030\005 \001(\0132\026.NavMessage.SpeedLimi"
-  "t\022-\n\rangular_limit\030\006 \001(\0132\026.NavMessage.Sp"
-  "eedLimit\022,\n\014horize_limit\030\007 \001(\0132\026.NavMess"
-  "age.SpeedLimit\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005"
-  "\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\0132\022.NavMess"
-  "age.Action\"\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013"
-  "\n\003key\030\002 \001(\t\022.\n\022unfinished_actions\030\003 \003(\0132"
-  "\022.NavMessage.Action\022-\n\rselected_traj\030\004 \001"
-  "(\0132\026.NavMessage.Trajectory\022,\n\014predict_tr"
-  "aj\030\005 \001(\0132\026.NavMessage.Trajectoryb\006proto3"
+  "\001(\002\022\t\n\001w\030\002 \001(\002\"K\n\005Speed\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\013"
+  "\n\003end\030\006 \001(\005\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013"
+  "\n\003max\030\002 \001(\002\"-\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\"/\n\nTrajectory\022!\n\005pose"
+  "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\223\002\n\006Action\022\014"
+  "\n\004type\030\001 \001(\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage"
+  ".Pose2d\022\"\n\006target\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\022\'\n\013target_diff\030\004 \001(\0132\022.NavMessage.Po"
+  "se2d\022.\n\016velocity_limit\030\005 \001(\0132\026.NavMessag"
+  "e.SpeedLimit\022-\n\rangular_limit\030\006 \001(\0132\026.Na"
+  "vMessage.SpeedLimit\022,\n\014horize_limit\030\007 \001("
+  "\0132\026.NavMessage.SpeedLimit\"q\n\006NavCmd\022\016\n\006a"
+  "ction\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022%\n\ttransform\030\003 "
+  "\001(\0132\022.NavMessage.Pose2d\022#\n\007actions\030\004 \003(\013"
+  "2\022.NavMessage.Action\"\330\001\n\010NavStatu\022\r\n\005sta"
+  "tu\030\001 \001(\010\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030"
+  "\003 \001(\005\022\013\n\003key\030\004 \001(\t\022.\n\022unfinished_actions"
+  "\030\005 \003(\0132\022.NavMessage.Action\022-\n\rselected_t"
+  "raj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014pre"
+  "dict_traj\030\007 \001(\0132\026.NavMessage.Trajectoryb"
+  "\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -335,7 +343,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 880,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 967,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 9, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 9, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -943,30 +951,37 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // int32 T = 2;
+      // int32 M = 2;
       case 2:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
-          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          m_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float V = 3;
+      // int32 T = 3;
       case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 24)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float W = 4;
+      // float V = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
-          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
-      // int32 end = 5;
+      // float W = 5;
       case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 45)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // int32 end = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
           end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -1005,28 +1020,34 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_m(), target);
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_t(), target);
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_v(), target);
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(5, this->_internal_w(), target);
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(6, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1052,24 +1073,31 @@ size_t Speed::ByteSizeLong() const {
         this->_internal_h());
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_m());
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
         this->_internal_t());
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
@@ -1110,6 +1138,9 @@ void Speed::MergeFrom(const Speed& from) {
   if (from.h() != 0) {
     _internal_set_h(from._internal_h());
   }
+  if (from.m() != 0) {
+    _internal_set_m(from._internal_m());
+  }
   if (from.t() != 0) {
     _internal_set_t(from._internal_t());
   }
@@ -2296,11 +2327,18 @@ void Action::InternalSwap(Action* other) {
 // ===================================================================
 
 void NavCmd::InitAsDefaultInstance() {
+  ::NavMessage::_NavCmd_default_instance_._instance.get_mutable()->transform_ = const_cast< ::NavMessage::Pose2d*>(
+      ::NavMessage::Pose2d::internal_default_instance());
 }
 class NavCmd::_Internal {
  public:
+  static const ::NavMessage::Pose2d& transform(const NavCmd* msg);
 };
 
+const ::NavMessage::Pose2d&
+NavCmd::_Internal::transform(const NavCmd* msg) {
+  return *msg->transform_;
+}
 NavCmd::NavCmd(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena),
   actions_(arena) {
@@ -2317,6 +2355,11 @@ NavCmd::NavCmd(const NavCmd& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
+  if (from._internal_has_transform()) {
+    transform_ = new ::NavMessage::Pose2d(*from.transform_);
+  } else {
+    transform_ = nullptr;
+  }
   action_ = from.action_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavCmd)
 }
@@ -2324,7 +2367,9 @@ NavCmd::NavCmd(const NavCmd& from)
 void NavCmd::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavCmd_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  action_ = 0;
+  ::memset(&transform_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&action_) -
+      reinterpret_cast<char*>(&transform_)) + sizeof(action_));
 }
 
 NavCmd::~NavCmd() {
@@ -2336,6 +2381,7 @@ NavCmd::~NavCmd() {
 void NavCmd::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (this != internal_default_instance()) delete transform_;
 }
 
 void NavCmd::ArenaDtor(void* object) {
@@ -2361,6 +2407,10 @@ void NavCmd::Clear() {
 
   actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
   action_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
@@ -2389,16 +2439,23 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // repeated .NavMessage.Action actions = 3;
+      // .NavMessage.Pose2d transform = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
+          ptr = ctx->ParseMessage(_internal_mutable_transform(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // repeated .NavMessage.Action actions = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
           ptr -= 1;
           do {
             ptr += 1;
             ptr = ctx->ParseMessage(_internal_add_actions(), ptr);
             CHK_(ptr);
             if (!ctx->DataAvailable(ptr)) break;
-          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<34>(ptr));
         } else goto handle_unusual;
         continue;
       default: {
@@ -2445,12 +2502,20 @@ failure:
         2, this->_internal_key(), target);
   }
 
-  // repeated .NavMessage.Action actions = 3;
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        3, _Internal::transform(this), target, stream);
+  }
+
+  // repeated .NavMessage.Action actions = 4;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_actions_size()); i < n; i++) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(3, this->_internal_actions(i), target, stream);
+      InternalWriteMessage(4, this->_internal_actions(i), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2469,7 +2534,7 @@ size_t NavCmd::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   total_size += 1UL * this->_internal_actions_size();
   for (const auto& msg : this->actions_) {
     total_size +=
@@ -2483,6 +2548,13 @@ size_t NavCmd::ByteSizeLong() const {
         this->_internal_key());
   }
 
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *transform_);
+  }
+
   // int32 action = 1;
   if (this->action() != 0) {
     total_size += 1 +
@@ -2525,6 +2597,9 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
+  if (from.has_transform()) {
+    _internal_mutable_transform()->::NavMessage::Pose2d::MergeFrom(from._internal_transform());
+  }
   if (from.action() != 0) {
     _internal_set_action(from._internal_action());
   }
@@ -2553,7 +2628,12 @@ void NavCmd::InternalSwap(NavCmd* other) {
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   actions_.InternalSwap(&other->actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  swap(action_, other->action_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(NavCmd, action_)
+      + sizeof(NavCmd::action_)
+      - PROTOBUF_FIELD_OFFSET(NavCmd, transform_)>(
+          reinterpret_cast<char*>(&transform_),
+          reinterpret_cast<char*>(&other->transform_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {
@@ -2609,7 +2689,9 @@ NavStatu::NavStatu(const NavStatu& from)
   } else {
     predict_traj_ = nullptr;
   }
-  statu_ = from.statu_;
+  ::memcpy(&statu_, &from.statu_,
+    static_cast<size_t>(reinterpret_cast<char*>(&move_mode_) -
+    reinterpret_cast<char*>(&statu_)) + sizeof(move_mode_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavStatu)
 }
 
@@ -2617,8 +2699,8 @@ void NavStatu::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavStatu_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
   ::memset(&selected_traj_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&statu_) -
-      reinterpret_cast<char*>(&selected_traj_)) + sizeof(statu_));
+      reinterpret_cast<char*>(&move_mode_) -
+      reinterpret_cast<char*>(&selected_traj_)) + sizeof(move_mode_));
 }
 
 NavStatu::~NavStatu() {
@@ -2665,7 +2747,9 @@ void NavStatu::Clear() {
     delete predict_traj_;
   }
   predict_traj_ = nullptr;
-  statu_ = false;
+  ::memset(&statu_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&move_mode_) -
+      reinterpret_cast<char*>(&statu_)) + sizeof(move_mode_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -2684,37 +2768,51 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // string key = 2;
+      // bool main_agv = 2;
       case 2:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 18)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
+          main_agv_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // int32 move_mode = 3;
+      case 3:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 24)) {
+          move_mode_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // string key = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
           auto str = _internal_mutable_key();
           ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
           CHK_(::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "NavMessage.NavStatu.key"));
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // repeated .NavMessage.Action unfinished_actions = 3;
-      case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
+      // repeated .NavMessage.Action unfinished_actions = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
           ptr -= 1;
           do {
             ptr += 1;
             ptr = ctx->ParseMessage(_internal_add_unfinished_actions(), ptr);
             CHK_(ptr);
             if (!ctx->DataAvailable(ptr)) break;
-          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<42>(ptr));
         } else goto handle_unusual;
         continue;
-      // .NavMessage.Trajectory selected_traj = 4;
-      case 4:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
+      // .NavMessage.Trajectory selected_traj = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 50)) {
           ptr = ctx->ParseMessage(_internal_mutable_selected_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // .NavMessage.Trajectory predict_traj = 5;
-      case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
+      // .NavMessage.Trajectory predict_traj = 7;
+      case 7:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 58)) {
           ptr = ctx->ParseMessage(_internal_mutable_predict_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -2753,38 +2851,50 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(1, this->_internal_statu(), target);
   }
 
-  // string key = 2;
+  // bool main_agv = 2;
+  if (this->main_agv() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(2, this->_internal_main_agv(), target);
+  }
+
+  // int32 move_mode = 3;
+  if (this->move_mode() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_move_mode(), target);
+  }
+
+  // string key = 4;
   if (this->key().size() > 0) {
     ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
       this->_internal_key().data(), static_cast<int>(this->_internal_key().length()),
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE,
       "NavMessage.NavStatu.key");
     target = stream->WriteStringMaybeAliased(
-        2, this->_internal_key(), target);
+        4, this->_internal_key(), target);
   }
 
-  // repeated .NavMessage.Action unfinished_actions = 3;
+  // repeated .NavMessage.Action unfinished_actions = 5;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_unfinished_actions_size()); i < n; i++) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(3, this->_internal_unfinished_actions(i), target, stream);
+      InternalWriteMessage(5, this->_internal_unfinished_actions(i), target, stream);
   }
 
-  // .NavMessage.Trajectory selected_traj = 4;
+  // .NavMessage.Trajectory selected_traj = 6;
   if (this->has_selected_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        4, _Internal::selected_traj(this), target, stream);
+        6, _Internal::selected_traj(this), target, stream);
   }
 
-  // .NavMessage.Trajectory predict_traj = 5;
+  // .NavMessage.Trajectory predict_traj = 7;
   if (this->has_predict_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        5, _Internal::predict_traj(this), target, stream);
+        7, _Internal::predict_traj(this), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2803,28 +2913,28 @@ size_t NavStatu::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // repeated .NavMessage.Action unfinished_actions = 3;
+  // repeated .NavMessage.Action unfinished_actions = 5;
   total_size += 1UL * this->_internal_unfinished_actions_size();
   for (const auto& msg : this->unfinished_actions_) {
     total_size +=
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(msg);
   }
 
-  // string key = 2;
+  // string key = 4;
   if (this->key().size() > 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
         this->_internal_key());
   }
 
-  // .NavMessage.Trajectory selected_traj = 4;
+  // .NavMessage.Trajectory selected_traj = 6;
   if (this->has_selected_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *selected_traj_);
   }
 
-  // .NavMessage.Trajectory predict_traj = 5;
+  // .NavMessage.Trajectory predict_traj = 7;
   if (this->has_predict_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
@@ -2836,6 +2946,18 @@ size_t NavStatu::ByteSizeLong() const {
     total_size += 1 + 1;
   }
 
+  // bool main_agv = 2;
+  if (this->main_agv() != 0) {
+    total_size += 1 + 1;
+  }
+
+  // int32 move_mode = 3;
+  if (this->move_mode() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_move_mode());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -2880,6 +3002,12 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.statu() != 0) {
     _internal_set_statu(from._internal_statu());
   }
+  if (from.main_agv() != 0) {
+    _internal_set_main_agv(from._internal_main_agv());
+  }
+  if (from.move_mode() != 0) {
+    _internal_set_move_mode(from._internal_move_mode());
+  }
 }
 
 void NavStatu::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -2906,8 +3034,8 @@ void NavStatu::InternalSwap(NavStatu* other) {
   unfinished_actions_.InternalSwap(&other->unfinished_actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(NavStatu, statu_)
-      + sizeof(NavStatu::statu_)
+      PROTOBUF_FIELD_OFFSET(NavStatu, move_mode_)
+      + sizeof(NavStatu::move_mode_)
       - PROTOBUF_FIELD_OFFSET(NavStatu, selected_traj_)>(
           reinterpret_cast<char*>(&selected_traj_),
           reinterpret_cast<char*>(&other->selected_traj_));

MPC/monitor/emqx/message.pb.h

@@ -541,10 +541,11 @@ class Speed PROTOBUF_FINAL :
 
   enum : int {
     kHFieldNumber = 1,
-    kTFieldNumber = 2,
-    kVFieldNumber = 3,
-    kWFieldNumber = 4,
-    kEndFieldNumber = 5,
+    kMFieldNumber = 2,
+    kTFieldNumber = 3,
+    kVFieldNumber = 4,
+    kWFieldNumber = 5,
+    kEndFieldNumber = 6,
   };
   // int32 H = 1;
   void clear_h();
@@ -555,7 +556,16 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_h(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // int32 T = 2;
+  // int32 M = 2;
+  void clear_m();
+  ::PROTOBUF_NAMESPACE_ID::int32 m() const;
+  void set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_m() const;
+  void _internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
+  // int32 T = 3;
   void clear_t();
   ::PROTOBUF_NAMESPACE_ID::int32 t() const;
   void set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -564,7 +574,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // float V = 3;
+  // float V = 4;
   void clear_v();
   float v() const;
   void set_v(float value);
@@ -573,7 +583,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_v(float value);
   public:
 
-  // float W = 4;
+  // float W = 5;
   void clear_w();
   float w() const;
   void set_w(float value);
@@ -582,7 +592,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_w(float value);
   public:
 
-  // int32 end = 5;
+  // int32 end = 6;
   void clear_end();
   ::PROTOBUF_NAMESPACE_ID::int32 end() const;
   void set_end(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -599,6 +609,7 @@ class Speed PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::int32 h_;
+  ::PROTOBUF_NAMESPACE_ID::int32 m_;
   ::PROTOBUF_NAMESPACE_ID::int32 t_;
   float v_;
   float w_;
@@ -1431,11 +1442,12 @@ class NavCmd PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kActionsFieldNumber = 3,
+    kActionsFieldNumber = 4,
     kKeyFieldNumber = 2,
+    kTransformFieldNumber = 3,
     kActionFieldNumber = 1,
   };
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   int actions_size() const;
   private:
   int _internal_actions_size() const;
@@ -1469,6 +1481,24 @@ class NavCmd PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
+  // .NavMessage.Pose2d transform = 3;
+  bool has_transform() const;
+  private:
+  bool _internal_has_transform() const;
+  public:
+  void clear_transform();
+  const ::NavMessage::Pose2d& transform() const;
+  ::NavMessage::Pose2d* release_transform();
+  ::NavMessage::Pose2d* mutable_transform();
+  void set_allocated_transform(::NavMessage::Pose2d* transform);
+  private:
+  const ::NavMessage::Pose2d& _internal_transform() const;
+  ::NavMessage::Pose2d* _internal_mutable_transform();
+  public:
+  void unsafe_arena_set_allocated_transform(
+      ::NavMessage::Pose2d* transform);
+  ::NavMessage::Pose2d* unsafe_arena_release_transform();
+
   // int32 action = 1;
   void clear_action();
   ::PROTOBUF_NAMESPACE_ID::int32 action() const;
@@ -1487,6 +1517,7 @@ class NavCmd PROTOBUF_FINAL :
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action > actions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
+  ::NavMessage::Pose2d* transform_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
@@ -1606,13 +1637,15 @@ class NavStatu PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kUnfinishedActionsFieldNumber = 3,
-    kKeyFieldNumber = 2,
-    kSelectedTrajFieldNumber = 4,
-    kPredictTrajFieldNumber = 5,
+    kUnfinishedActionsFieldNumber = 5,
+    kKeyFieldNumber = 4,
+    kSelectedTrajFieldNumber = 6,
+    kPredictTrajFieldNumber = 7,
     kStatuFieldNumber = 1,
+    kMainAgvFieldNumber = 2,
+    kMoveModeFieldNumber = 3,
   };
-  // repeated .NavMessage.Action unfinished_actions = 3;
+  // repeated .NavMessage.Action unfinished_actions = 5;
   int unfinished_actions_size() const;
   private:
   int _internal_unfinished_actions_size() const;
@@ -1630,7 +1663,7 @@ class NavStatu PROTOBUF_FINAL :
   const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action >&
       unfinished_actions() const;
 
-  // string key = 2;
+  // string key = 4;
   void clear_key();
   const std::string& key() const;
   void set_key(const std::string& value);
@@ -1646,7 +1679,7 @@ class NavStatu PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
-  // .NavMessage.Trajectory selected_traj = 4;
+  // .NavMessage.Trajectory selected_traj = 6;
   bool has_selected_traj() const;
   private:
   bool _internal_has_selected_traj() const;
@@ -1664,7 +1697,7 @@ class NavStatu PROTOBUF_FINAL :
       ::NavMessage::Trajectory* selected_traj);
   ::NavMessage::Trajectory* unsafe_arena_release_selected_traj();
 
-  // .NavMessage.Trajectory predict_traj = 5;
+  // .NavMessage.Trajectory predict_traj = 7;
   bool has_predict_traj() const;
   private:
   bool _internal_has_predict_traj() const;
@@ -1691,6 +1724,24 @@ class NavStatu PROTOBUF_FINAL :
   void _internal_set_statu(bool value);
   public:
 
+  // bool main_agv = 2;
+  void clear_main_agv();
+  bool main_agv() const;
+  void set_main_agv(bool value);
+  private:
+  bool _internal_main_agv() const;
+  void _internal_set_main_agv(bool value);
+  public:
+
+  // int32 move_mode = 3;
+  void clear_move_mode();
+  ::PROTOBUF_NAMESPACE_ID::int32 move_mode() const;
+  void set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_move_mode() const;
+  void _internal_set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
   // @@protoc_insertion_point(class_scope:NavMessage.NavStatu)
  private:
   class _Internal;
@@ -1703,6 +1754,8 @@ class NavStatu PROTOBUF_FINAL :
   ::NavMessage::Trajectory* selected_traj_;
   ::NavMessage::Trajectory* predict_traj_;
   bool statu_;
+  bool main_agv_;
+  ::PROTOBUF_NAMESPACE_ID::int32 move_mode_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -1885,7 +1938,27 @@ inline void Speed::set_h(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.H)
 }
 
-// int32 T = 2;
+// int32 M = 2;
+inline void Speed::clear_m() {
+  m_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_m() const {
+  return m_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::m() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.M)
+  return _internal_m();
+}
+inline void Speed::_internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  m_ = value;
+}
+inline void Speed::set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_m(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.M)
+}
+
+// int32 T = 3;
 inline void Speed::clear_t() {
   t_ = 0;
 }
@@ -1905,7 +1978,7 @@ inline void Speed::set_t(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.T)
 }
 
-// float V = 3;
+// float V = 4;
 inline void Speed::clear_v() {
   v_ = 0;
 }
@@ -1925,7 +1998,7 @@ inline void Speed::set_v(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.V)
 }
 
-// float W = 4;
+// float W = 5;
 inline void Speed::clear_w() {
   w_ = 0;
 }
@@ -1945,7 +2018,7 @@ inline void Speed::set_w(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.W)
 }
 
-// int32 end = 5;
+// int32 end = 6;
 inline void Speed::clear_end() {
   end_ = 0;
 }
@@ -2724,7 +2797,90 @@ inline void NavCmd::set_allocated_key(std::string* key) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.key)
 }
 
-// repeated .NavMessage.Action actions = 3;
+// .NavMessage.Pose2d transform = 3;
+inline bool NavCmd::_internal_has_transform() const {
+  return this != internal_default_instance() && transform_ != nullptr;
+}
+inline bool NavCmd::has_transform() const {
+  return _internal_has_transform();
+}
+inline void NavCmd::clear_transform() {
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
+}
+inline const ::NavMessage::Pose2d& NavCmd::_internal_transform() const {
+  const ::NavMessage::Pose2d* p = transform_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::Pose2d*>(
+      &::NavMessage::_Pose2d_default_instance_);
+}
+inline const ::NavMessage::Pose2d& NavCmd::transform() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.transform)
+  return _internal_transform();
+}
+inline void NavCmd::unsafe_arena_set_allocated_transform(
+    ::NavMessage::Pose2d* transform) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(transform_);
+  }
+  transform_ = transform;
+  if (transform) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.NavCmd.transform)
+}
+inline ::NavMessage::Pose2d* NavCmd::release_transform() {
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::unsafe_arena_release_transform() {
+  // @@protoc_insertion_point(field_release:NavMessage.NavCmd.transform)
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::_internal_mutable_transform() {
+  
+  if (transform_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::Pose2d>(GetArena());
+    transform_ = p;
+  }
+  return transform_;
+}
+inline ::NavMessage::Pose2d* NavCmd::mutable_transform() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavCmd.transform)
+  return _internal_mutable_transform();
+}
+inline void NavCmd::set_allocated_transform(::NavMessage::Pose2d* transform) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete transform_;
+  }
+  if (transform) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(transform);
+    if (message_arena != submessage_arena) {
+      transform = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, transform, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  transform_ = transform;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.transform)
+}
+
+// repeated .NavMessage.Action actions = 4;
 inline int NavCmd::_internal_actions_size() const {
   return actions_.size();
 }
@@ -2787,7 +2943,47 @@ inline void NavStatu::set_statu(bool value) {
   // @@protoc_insertion_point(field_set:NavMessage.NavStatu.statu)
 }
 
-// string key = 2;
+// bool main_agv = 2;
+inline void NavStatu::clear_main_agv() {
+  main_agv_ = false;
+}
+inline bool NavStatu::_internal_main_agv() const {
+  return main_agv_;
+}
+inline bool NavStatu::main_agv() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.main_agv)
+  return _internal_main_agv();
+}
+inline void NavStatu::_internal_set_main_agv(bool value) {
+  
+  main_agv_ = value;
+}
+inline void NavStatu::set_main_agv(bool value) {
+  _internal_set_main_agv(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.main_agv)
+}
+
+// int32 move_mode = 3;
+inline void NavStatu::clear_move_mode() {
+  move_mode_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavStatu::_internal_move_mode() const {
+  return move_mode_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 NavStatu::move_mode() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.move_mode)
+  return _internal_move_mode();
+}
+inline void NavStatu::_internal_set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  move_mode_ = value;
+}
+inline void NavStatu::set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_move_mode(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.move_mode)
+}
+
+// string key = 4;
 inline void NavStatu::clear_key() {
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
 }
@@ -2849,7 +3045,7 @@ inline void NavStatu::set_allocated_key(std::string* key) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.key)
 }
 
-// repeated .NavMessage.Action unfinished_actions = 3;
+// repeated .NavMessage.Action unfinished_actions = 5;
 inline int NavStatu::_internal_unfinished_actions_size() const {
   return unfinished_actions_.size();
 }
@@ -2888,7 +3084,7 @@ NavStatu::unfinished_actions() const {
   return unfinished_actions_;
 }
 
-// .NavMessage.Trajectory selected_traj = 4;
+// .NavMessage.Trajectory selected_traj = 6;
 inline bool NavStatu::_internal_has_selected_traj() const {
   return this != internal_default_instance() && selected_traj_ != nullptr;
 }
@@ -2971,7 +3167,7 @@ inline void NavStatu::set_allocated_selected_traj(::NavMessage::Trajectory* sele
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.selected_traj)
 }
 
-// .NavMessage.Trajectory predict_traj = 5;
+// .NavMessage.Trajectory predict_traj = 7;
 inline bool NavStatu::_internal_has_predict_traj() const {
   return this != internal_default_instance() && predict_traj_ != nullptr;
 }

MPC/monitor/monitor_emqx.cpp

@@ -24,8 +24,9 @@ void Monitor_emqx::set_speedcmd_topic(std::string speedcmd)
   speedcmd_topic_=speedcmd;
 }
 
-void Monitor_emqx::set_speed(SpeedType type,double v,double a)
+void Monitor_emqx::set_speed(MoveMode mode,SpeedType type,double v,double a)
 {
+
   double w=fabs(a)>0.001?a:0.0;
   MqttMsg msg;
   NavMessage::Speed speed;
@@ -34,6 +35,8 @@ void Monitor_emqx::set_speed(SpeedType type,double v,double a)
   speed.set_t(type);
   speed.set_v(v);
   speed.set_w(w);
+
+  speed.set_m(mode);
   speed.set_end(1);
   msg.fromProtoMessage(speed);
   Publish(speedcmd_topic_,msg);

MPC/monitor/monitor_emqx.h

@@ -12,6 +12,10 @@
 class Monitor_emqx : public Terminator_emqx
 {
 public:
+    enum MoveMode{
+        eSingle=1,
+        eMain
+    };
     enum SpeedType{
         eStop=0,
         eRotate=1,
@@ -24,7 +28,7 @@ public:
     Monitor_emqx(std::string nodeId);
     ~Monitor_emqx();
     void set_speedcmd_topic(std::string speedcmd);
-    void set_speed(SpeedType type,double v,double a);
+    void set_speed(MoveMode mode,SpeedType type,double v,double a);
     void stop();
 
  protected:

MPC/navigation.cpp

@@ -79,7 +79,7 @@ bool Navigation::Init(const Navigation_parameter& parameter)
     delete pubthread_;
   }
   exit_=false;
-  pubthread_=new std::thread(&Navigation::publish_statu,this);
+  pubthread_=new std::thread(&Navigation::pubStatuThreadFuc,this);
 
   return true;
 
@@ -104,14 +104,24 @@ void Navigation::RobotSpeedCallback(const MqttMsg& msg,void* context)
   }
 }
 
-void Navigation::publish_statu() {
-  while (exit_ == false)
-  {
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+void Navigation::pubStatuThreadFuc(void* p) {
+  Navigation *navogator = (Navigation *) p;
+
+  if (navogator) {
+    while (navogator->exit_ == false) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      NavMessage::NavStatu statu;
+      statu.set_main_agv(false);
+      navogator->publish_statu(statu);
+    }
+  }
+}
+
+void Navigation::publish_statu(NavMessage::NavStatu& statu) {
 
     //发布nav状态
-    NavMessage::NavStatu statu;
     statu.set_statu(eReady); //默认ready
+    statu.set_move_mode(move_mode_);
     NavMessage::Action *current_action = nullptr;
     if (running_)
     {
@@ -158,7 +168,7 @@ void Navigation::publish_statu() {
       terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);
 
     //发布位姿  --------------------------------
-    if (timedPose_.timeout() == false ||
+    if (timedPose_.timeout() == false &&
         (timedV_.timeout() == false && timedA_.timeout() == false)) {
       NavMessage::AGVStatu agvStatu;
       Pose2d pose = timedPose_.Get();
@@ -173,9 +183,6 @@ void Navigation::publish_statu() {
         terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
       }
     }
-
-
-  }
 }
 
 void Navigation::ResetStatu(double v,double a)
@@ -229,6 +236,7 @@ void Navigation::Pause()
 
 Navigation::Navigation()
 {
+  move_mode_=Monitor_emqx::eSingle;
   thread_= nullptr;
   monitor_= nullptr;
   terminator_= nullptr;
@@ -252,34 +260,41 @@ void Navigation::BrotherAgvStatuCallback(const MqttMsg& msg,void* context)
   navigator->timedBrotherA_.reset(brother_statu.vth(),1);
 }
 
-void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
+void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
 {
-  Navigation* navigator=(Navigation*)context;
-
-  NavMessage::NavCmd cmd;
-  if(msg.toProtoMessage(cmd)==false) {
-    printf(" msg transform to NavCmd failed ..!!\n");
-    return;
-  }
-
-
   if(cmd.action()==3)
   {
     printf(" Nav cancel");
-    navigator->Cancel();
+    Cancel();
     return ;
   }
   if(cmd.action()==2) {
     printf(" Nav continue");
-    navigator->pause_=false;
+    pause_=false;
     return ;
   }
   if(cmd.action()==1) {
     printf(" Nav pause");
-    navigator->Pause();
+    Pause();
     return ;
   }
-  navigator->Start(cmd);
+  if(cmd.action()==0)
+  {
+    Start(cmd);
+  }
+  else
+    printf(" Invalid Cmd this agv is SingleAgv,action:%d\n",cmd.action());
+}
+
+void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
+{
+  NavMessage::NavCmd cmd;
+  if(msg.toProtoMessage(cmd)==false) {
+    printf(" msg transform to NavCmd failed ..!!\n");
+    return;
+  }
+  Navigation* navigator=(Navigation*)context;
+  navigator->HandleNavCmd(cmd);
 }
 
 bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
@@ -332,21 +347,21 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
           double theta = limit(diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
-          monitor_->set_speed(Monitor_emqx::eRotate, 0, theta);
+          SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, theta);
           printf(" Ratate :%f\n",theta);
           continue;
         }
       }
       if (action == 2) {
         if (Pose2d::abs(diff).y() > thresh.y()) {
-          monitor_->set_speed(Monitor_emqx::eHorizontal, limit(diff.y(),limit_h.min,limit_h.max), 0);
+          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal, limit(diff.y(),limit_h.min,limit_h.max), 0);
           printf(" Horizontal :%f\n",limit(diff.y(),limit_h.min,limit_h.max));
           continue;
         }
       }
       if (action == 3) {
         if (Pose2d::abs(diff).x() > thresh.x()) {
-          monitor_->set_speed(Monitor_emqx::eMPC, limit(diff.x(),limit_v.min,limit_v.max), 0);
+          SendMoveCmd(move_mode_,Monitor_emqx::eMPC, limit(diff.x(),limit_v.min,limit_v.max), 0);
           printf(" Vrtical :%f\n",limit(diff.x(),limit_v.min,limit_v.max));
           continue;
         }
@@ -408,6 +423,13 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
   //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
     return false;
 }
+
+void Navigation::SendMoveCmd(Monitor_emqx::MoveMode mode,Monitor_emqx::SpeedType type,
+                             double v,double angular){
+  if(monitor_)
+    monitor_->set_speed(mode,type,v,angular);
+}
+
 bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v)
 {
   if(inited_==false) {
@@ -453,7 +475,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
         monitor_->stop();
         return false;
       }
-      monitor_->set_speed(Monitor_emqx::eMPC,out[0],out[1]);
+      SendMoveCmd(move_mode_,Monitor_emqx::eMPC,out[0],out[1]);
       //std::this_thread::sleep_for(std::chrono::milliseconds (400));
     }
 

MPC/navigation.h

@@ -24,14 +24,15 @@ public:
         eReady=0,
         eRunning
     };
+
  public:
     Navigation();
-    ~Navigation();
+    virtual ~Navigation();
     //连接AGV emqx服务
 
     bool Init(const Navigation_parameter& parameter);
 
-    void ResetPose(const Pose2d& pose);
+    virtual void ResetPose(const Pose2d& pose);
     void ResetStatu(double v,double a);
     TimedLockData<Pose2d>& RealTimePose(){
       return timedPose_;
@@ -42,11 +43,15 @@ public:
     TimedLockData<double>& RealTimeA(){
       return timedA_;
     }
-    bool Start(const NavMessage::NavCmd& cmd);
+    virtual bool Start(const NavMessage::NavCmd& cmd);
     void Cancel();
     void Pause();
 
+
  protected:
+    virtual void SendMoveCmd(Monitor_emqx::MoveMode mode,Monitor_emqx::SpeedType type,
+                             double v,double angular);
+    virtual void HandleNavCmd(const NavMessage::NavCmd& cmd);
 
     static void RobotPoseCallback(const MqttMsg& msg,void* context);
     static void RobotSpeedCallback(const MqttMsg& msg,void* context);
@@ -61,10 +66,13 @@ public:
     bool mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out);
     void navigatting();
 
+
+
     /*
      * 发布导航模块状态
      */
-    void publish_statu();
+    static void pubStatuThreadFuc(void* p);
+    virtual void publish_statu(NavMessage::NavStatu& statu);
 
  protected:
     std::mutex  mtx_;
@@ -89,6 +97,8 @@ public:
     bool pause_=false;
     bool cancel_=false;
 
+    Monitor_emqx::MoveMode move_mode_=Monitor_emqx::eSingle;
+
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
 

MPC/navigation_main.cpp

@@ -0,0 +1,59 @@
+//
+// Created by zx on 23-5-8.
+//
+
+#include "navigation_main.h"
+
+
+NavigationMain::NavigationMain(){
+  move_mode_=Monitor_emqx::eSingle;
+}
+NavigationMain::~NavigationMain(){
+
+}
+
+void NavigationMain::ResetPose(const Pose2d& pose){
+  if(move_mode_==Monitor_emqx::eMain)
+    Navigation::ResetPose(pose*transform_);
+  else
+    Navigation::ResetPose(pose);
+}
+
+void NavigationMain::publish_statu(NavMessage::NavStatu& statu)
+{
+  statu.set_main_agv(true);
+  Navigation::publish_statu(statu);
+}
+
+void NavigationMain::HandleNavCmd(const NavMessage::NavCmd& cmd)
+{
+  if(cmd.action()==4)
+  {
+    printf(" Switch MoveMode --> main\n");
+    NavMessage::Pose2d transform=cmd.transform();
+    SwitchMode(Monitor_emqx::eMain,Pose2d(transform.x(),transform.y(),transform.theta()));
+    return ;
+  }
+  if(cmd.action()==5)
+  {
+    printf(" Switch MoveMode --> single\n");
+    SwitchMode(Monitor_emqx::eSingle,Pose2d());
+    return ;
+  }
+  else
+  {
+    Navigation::HandleNavCmd(cmd);
+  }
+}
+
+bool NavigationMain::Start(const NavMessage::NavCmd& cmd)
+{
+  /*if(move_mode_!=Monitor_emqx::eMain)
+  {
+    printf(" navigation mode must set main,parameter:Pose2d\n");
+    return false;
+  }*/
+
+  return Navigation::Start(cmd);
+
+}

MPC/navigation_main.h

@@ -0,0 +1,30 @@
+//
+// Created by zx on 23-5-8.
+//
+
+#ifndef NAVIGATION_NAVIGATION_MAIN_H
+#define NAVIGATION_NAVIGATION_MAIN_H
+#include "navigation.h"
+class NavigationMain:public Navigation {
+
+public:
+    NavigationMain();
+    ~NavigationMain();
+
+    virtual void ResetPose(const Pose2d& pose);
+    virtual bool Start(const NavMessage::NavCmd& cmd);
+    virtual void publish_statu(NavMessage::NavStatu& statu);
+    virtual void SwitchMode(Monitor_emqx::MoveMode mode,const Pose2d& transform){
+      move_mode_=mode;
+      if(move_mode_==Monitor_emqx::eMain)
+      {
+        transform_=transform;
+      }
+    }
+protected:
+    virtual void HandleNavCmd(const NavMessage::NavCmd& cmd);
+protected:
+    Pose2d  transform_;   //两节agv的位姿与单节的位姿变换
+};
+
+#endif //NAVIGATION_NAVIGATION_MAIN_H

MPC/parameter.pb.cc

@@ -105,6 +105,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_parameter_2eproto::offsets[] P
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::Navigation_parameter, main_agv_),
   PROTOBUF_FIELD_OFFSET(::Navigation_parameter, agv_emqx_),
   PROTOBUF_FIELD_OFFSET(::Navigation_parameter, terminal_emqx_),
 };
@@ -128,10 +129,11 @@ const char descriptor_table_protodef_parameter_2eproto[] PROTOBUF_SECTION_VARIAB
   "arameter\022\016\n\006NodeId\030\001 \001(\t\022\n\n\002ip\030\002 \001(\t\022\014\n\004"
   "port\030\003 \001(\005\022\025\n\rpubStatuTopic\030\004 \001(\t\022\030\n\020pub"
   "NavStatuTopic\030\005 \001(\t\022\026\n\016subNavCmdTopic\030\006 "
-  "\001(\t\022\034\n\024subBrotherStatuTopic\030\007 \001(\t\"d\n\024Nav"
-  "igation_parameter\022$\n\010Agv_emqx\030\001 \001(\0132\022.Ag"
-  "vEmqx_parameter\022&\n\rTerminal_emqx\030\002 \001(\0132\017"
-  ".Emqx_parameterb\006proto3"
+  "\001(\t\022\034\n\024subBrotherStatuTopic\030\007 \001(\t\"v\n\024Nav"
+  "igation_parameter\022\020\n\010main_agv\030\001 \001(\010\022$\n\010A"
+  "gv_emqx\030\002 \001(\0132\022.AgvEmqx_parameter\022&\n\rTer"
+  "minal_emqx\030\003 \001(\0132\017.Emqx_parameterb\006proto"
+  "3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_parameter_2eproto_deps[1] = {
 };
@@ -142,7 +144,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_par
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_parameter_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_parameter_2eproto = {
-  false, false, descriptor_table_protodef_parameter_2eproto, "parameter.proto", 423,
+  false, false, descriptor_table_protodef_parameter_2eproto, "parameter.proto", 441,
   &descriptor_table_parameter_2eproto_once, descriptor_table_parameter_2eproto_sccs, descriptor_table_parameter_2eproto_deps, 3, 0,
   schemas, file_default_instances, TableStruct_parameter_2eproto::offsets,
   file_level_metadata_parameter_2eproto, 3, file_level_enum_descriptors_parameter_2eproto, file_level_service_descriptors_parameter_2eproto,
@@ -1006,14 +1008,15 @@ Navigation_parameter::Navigation_parameter(const Navigation_parameter& from)
   } else {
     terminal_emqx_ = nullptr;
   }
+  main_agv_ = from.main_agv_;
   // @@protoc_insertion_point(copy_constructor:Navigation_parameter)
 }
 
 void Navigation_parameter::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_Navigation_parameter_parameter_2eproto.base);
   ::memset(&agv_emqx_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&terminal_emqx_) -
-      reinterpret_cast<char*>(&agv_emqx_)) + sizeof(terminal_emqx_));
+      reinterpret_cast<char*>(&main_agv_) -
+      reinterpret_cast<char*>(&agv_emqx_)) + sizeof(main_agv_));
 }
 
 Navigation_parameter::~Navigation_parameter() {
@@ -1057,6 +1060,7 @@ void Navigation_parameter::Clear() {
     delete terminal_emqx_;
   }
   terminal_emqx_ = nullptr;
+  main_agv_ = false;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1068,16 +1072,23 @@ const char* Navigation_parameter::_InternalParse(const char* ptr, ::PROTOBUF_NAM
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // .AgvEmqx_parameter Agv_emqx = 1;
+      // bool main_agv = 1;
       case 1:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 10)) {
-          ptr = ctx->ParseMessage(_internal_mutable_agv_emqx(), ptr);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
+          main_agv_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // .Emqx_parameter Terminal_emqx = 2;
+      // .AgvEmqx_parameter Agv_emqx = 2;
       case 2:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 18)) {
+          ptr = ctx->ParseMessage(_internal_mutable_agv_emqx(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // .Emqx_parameter Terminal_emqx = 3;
+      case 3:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
           ptr = ctx->ParseMessage(_internal_mutable_terminal_emqx(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -1110,20 +1121,26 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // .AgvEmqx_parameter Agv_emqx = 1;
+  // bool main_agv = 1;
+  if (this->main_agv() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(1, this->_internal_main_agv(), target);
+  }
+
+  // .AgvEmqx_parameter Agv_emqx = 2;
   if (this->has_agv_emqx()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        1, _Internal::agv_emqx(this), target, stream);
+        2, _Internal::agv_emqx(this), target, stream);
   }
 
-  // .Emqx_parameter Terminal_emqx = 2;
+  // .Emqx_parameter Terminal_emqx = 3;
   if (this->has_terminal_emqx()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        2, _Internal::terminal_emqx(this), target, stream);
+        3, _Internal::terminal_emqx(this), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1142,20 +1159,25 @@ size_t Navigation_parameter::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // .AgvEmqx_parameter Agv_emqx = 1;
+  // .AgvEmqx_parameter Agv_emqx = 2;
   if (this->has_agv_emqx()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *agv_emqx_);
   }
 
-  // .Emqx_parameter Terminal_emqx = 2;
+  // .Emqx_parameter Terminal_emqx = 3;
   if (this->has_terminal_emqx()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *terminal_emqx_);
   }
 
+  // bool main_agv = 1;
+  if (this->main_agv() != 0) {
+    total_size += 1 + 1;
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -1193,6 +1215,9 @@ void Navigation_parameter::MergeFrom(const Navigation_parameter& from) {
   if (from.has_terminal_emqx()) {
     _internal_mutable_terminal_emqx()->::Emqx_parameter::MergeFrom(from._internal_terminal_emqx());
   }
+  if (from.main_agv() != 0) {
+    _internal_set_main_agv(from._internal_main_agv());
+  }
 }
 
 void Navigation_parameter::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -1217,8 +1242,8 @@ void Navigation_parameter::InternalSwap(Navigation_parameter* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(Navigation_parameter, terminal_emqx_)
-      + sizeof(Navigation_parameter::terminal_emqx_)
+      PROTOBUF_FIELD_OFFSET(Navigation_parameter, main_agv_)
+      + sizeof(Navigation_parameter::main_agv_)
       - PROTOBUF_FIELD_OFFSET(Navigation_parameter, agv_emqx_)>(
           reinterpret_cast<char*>(&agv_emqx_),
           reinterpret_cast<char*>(&other->agv_emqx_));

MPC/parameter.pb.h

@@ -656,10 +656,11 @@ class Navigation_parameter PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kAgvEmqxFieldNumber = 1,
-    kTerminalEmqxFieldNumber = 2,
+    kAgvEmqxFieldNumber = 2,
+    kTerminalEmqxFieldNumber = 3,
+    kMainAgvFieldNumber = 1,
   };
-  // .AgvEmqx_parameter Agv_emqx = 1;
+  // .AgvEmqx_parameter Agv_emqx = 2;
   bool has_agv_emqx() const;
   private:
   bool _internal_has_agv_emqx() const;
@@ -677,7 +678,7 @@ class Navigation_parameter PROTOBUF_FINAL :
       ::AgvEmqx_parameter* agv_emqx);
   ::AgvEmqx_parameter* unsafe_arena_release_agv_emqx();
 
-  // .Emqx_parameter Terminal_emqx = 2;
+  // .Emqx_parameter Terminal_emqx = 3;
   bool has_terminal_emqx() const;
   private:
   bool _internal_has_terminal_emqx() const;
@@ -695,6 +696,15 @@ class Navigation_parameter PROTOBUF_FINAL :
       ::Emqx_parameter* terminal_emqx);
   ::Emqx_parameter* unsafe_arena_release_terminal_emqx();
 
+  // bool main_agv = 1;
+  void clear_main_agv();
+  bool main_agv() const;
+  void set_main_agv(bool value);
+  private:
+  bool _internal_main_agv() const;
+  void _internal_set_main_agv(bool value);
+  public:
+
   // @@protoc_insertion_point(class_scope:Navigation_parameter)
  private:
   class _Internal;
@@ -704,6 +714,7 @@ class Navigation_parameter PROTOBUF_FINAL :
   typedef void DestructorSkippable_;
   ::AgvEmqx_parameter* agv_emqx_;
   ::Emqx_parameter* terminal_emqx_;
+  bool main_agv_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_parameter_2eproto;
 };
@@ -1448,7 +1459,27 @@ inline void Emqx_parameter::set_allocated_subbrotherstatutopic(std::string* subb
 
 // Navigation_parameter
 
-// .AgvEmqx_parameter Agv_emqx = 1;
+// bool main_agv = 1;
+inline void Navigation_parameter::clear_main_agv() {
+  main_agv_ = false;
+}
+inline bool Navigation_parameter::_internal_main_agv() const {
+  return main_agv_;
+}
+inline bool Navigation_parameter::main_agv() const {
+  // @@protoc_insertion_point(field_get:Navigation_parameter.main_agv)
+  return _internal_main_agv();
+}
+inline void Navigation_parameter::_internal_set_main_agv(bool value) {
+  
+  main_agv_ = value;
+}
+inline void Navigation_parameter::set_main_agv(bool value) {
+  _internal_set_main_agv(value);
+  // @@protoc_insertion_point(field_set:Navigation_parameter.main_agv)
+}
+
+// .AgvEmqx_parameter Agv_emqx = 2;
 inline bool Navigation_parameter::_internal_has_agv_emqx() const {
   return this != internal_default_instance() && agv_emqx_ != nullptr;
 }
@@ -1531,7 +1562,7 @@ inline void Navigation_parameter::set_allocated_agv_emqx(::AgvEmqx_parameter* ag
   // @@protoc_insertion_point(field_set_allocated:Navigation_parameter.Agv_emqx)
 }
 
-// .Emqx_parameter Terminal_emqx = 2;
+// .Emqx_parameter Terminal_emqx = 3;
 inline bool Navigation_parameter::_internal_has_terminal_emqx() const {
   return this != internal_default_instance() && terminal_emqx_ != nullptr;
 }

MPC/pose2d.h

@@ -81,6 +81,12 @@ class Pose2d
       return x()<pose.x()&&y()<pose.y()&&theta()<pose.theta();
     }
 
+    Pose2d operator*(const Pose2d& t)const
+    {
+      Pose2d rpos=t.rotate(theta());
+      return Pose2d(x()+rpos.x(),y()+rpos.y(),rpos.theta());
+    }
+
     /*
      * 将点顺时针旋转theta
      */

config/navigation_child.prototxt

@@ -1,3 +1,5 @@
+
+main_agv:true
 Agv_emqx
 {
 	NodeId:"agv-child"

controller.cpp

@@ -2,7 +2,7 @@
 // Created by zx on 23-4-11.
 //
 #include "define/timedlockdata.hpp"
-#include "MPC/navigation.h"
+#include "MPC/navigation_main.h"
 #include "define/TimerRecord.h"
 #include <glog/logging.h>
 #include "tool/pathcreator.h"
@@ -16,7 +16,7 @@ int main(int argc,char* argv[])
     return -1;
   }
   std::string parameter_file=argv[1];
-  Navigation* g_navigator=new Navigation();
+
   Navigation_parameter parameter;
 
   if(proto_tool::get_instance_pointer()->read_proto_param(parameter_file,parameter)==false)
@@ -24,6 +24,12 @@ int main(int argc,char* argv[])
     printf(" read proto parameter failed:%s\n",parameter_file.c_str());
     return -2;
   }
+  Navigation* g_navigator= nullptr;
+  if(parameter.main_agv()==true)
+    g_navigator=new NavigationMain();
+  else
+    g_navigator=new Navigation();
+
   if(g_navigator->Init(parameter))
     printf(" navigation inited\n");
   else{

message.proto

@@ -15,10 +15,11 @@ message AGVSpeed{
 
 message Speed {
   int32 H=1;  //心跳
-  int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
-  float V=3;  //角速度
-  float W=4;  //线速度
-  int32 end=5;
+  int32 M=2;	//模式：1整车模式，其他：单车
+  int32 T=3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
+  float V=4;  //角速度
+  float W=5;  //线速度
+  int32 end=6;
 }
 
 message SpeedLimit
@@ -53,18 +54,21 @@ message Action
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式
   string key=2;
-  repeated Action actions=3;
+  Pose2d transform=3;		//整车中心与单节的变换
+  repeated Action actions=4;
 }
 
 message NavStatu
 {
   bool statu = 1; //0:ready  1:running
-  string key = 2; // 任务唯一码
-  repeated Action unfinished_actions = 3;  //未完成的动作，第一个即为当前动作
-  Trajectory selected_traj = 4;
-  Trajectory predict_traj = 5;
+  bool main_agv=2; //是否是两节控制plc
+  int32 move_mode=3; //运动模式，1：single，2：双车
+  string key = 4; // 任务唯一码
+  repeated Action unfinished_actions = 5;  //未完成的动作，第一个即为当前动作
+  Trajectory selected_traj = 6;
+  Trajectory predict_traj = 7;
 
 }
 

parameter.proto

@@ -22,7 +22,8 @@ message Emqx_parameter
 
 message Navigation_parameter
 {
-  AgvEmqx_parameter Agv_emqx=1;
-  Emqx_parameter Terminal_emqx=2;
+  bool main_agv=1;   //是否是两车整体
+  AgvEmqx_parameter Agv_emqx=2;
+  Emqx_parameter Terminal_emqx=3;
 }
 

projects/controllers/AGV_controller/emqx-client/message.pb.cc

@@ -111,8 +111,9 @@ static void InitDefaultsscc_info_NavCmd_message_2eproto() {
   ::NavMessage::NavCmd::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_NavCmd_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavCmd_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+      &scc_info_Pose2d_message_2eproto.base,
       &scc_info_Action_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_NavStatu_message_2eproto() {
@@ -216,6 +217,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, m_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
@@ -260,6 +262,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, action_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, key_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, transform_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, actions_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, _internal_metadata_),
@@ -276,12 +279,12 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::AGVSpeed)},
   { 17, -1, sizeof(::NavMessage::Speed)},
-  { 27, -1, sizeof(::NavMessage::SpeedLimit)},
-  { 34, -1, sizeof(::NavMessage::Pose2d)},
-  { 42, -1, sizeof(::NavMessage::Trajectory)},
-  { 48, -1, sizeof(::NavMessage::Action)},
-  { 60, -1, sizeof(::NavMessage::NavCmd)},
-  { 68, -1, sizeof(::NavMessage::NavStatu)},
+  { 28, -1, sizeof(::NavMessage::SpeedLimit)},
+  { 35, -1, sizeof(::NavMessage::Pose2d)},
+  { 43, -1, sizeof(::NavMessage::Trajectory)},
+  { 49, -1, sizeof(::NavMessage::Action)},
+  { 61, -1, sizeof(::NavMessage::NavCmd)},
+  { 70, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -300,25 +303,27 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\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\t\n\001"
   "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\" \n\010AGVSpeed\022\t\n\001v\030\001 "
-  "\001(\002\022\t\n\001w\030\002 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005\022\t\n\001T\030"
-  "\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end\030\005 \001(\005"
-  "\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013\n\003max\030\002 \001(\002"
-  "\"-\n\006Pose2d\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\"/\n\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.N"
-  "avMessage.Pose2d\"\223\002\n\006Action\022\014\n\004type\030\001 \001("
-  "\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage.Pose2d\022\"\n\006"
-  "target\030\003 \001(\0132\022.NavMessage.Pose2d\022\'\n\013targ"
-  "et_diff\030\004 \001(\0132\022.NavMessage.Pose2d\022.\n\016vel"
-  "ocity_limit\030\005 \001(\0132\026.NavMessage.SpeedLimi"
-  "t\022-\n\rangular_limit\030\006 \001(\0132\026.NavMessage.Sp"
-  "eedLimit\022,\n\014horize_limit\030\007 \001(\0132\026.NavMess"
-  "age.SpeedLimit\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005"
-  "\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\0132\022.NavMess"
-  "age.Action\"\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013"
-  "\n\003key\030\002 \001(\t\022.\n\022unfinished_actions\030\003 \003(\0132"
-  "\022.NavMessage.Action\022-\n\rselected_traj\030\004 \001"
-  "(\0132\026.NavMessage.Trajectory\022,\n\014predict_tr"
-  "aj\030\005 \001(\0132\026.NavMessage.Trajectoryb\006proto3"
+  "\001(\002\022\t\n\001w\030\002 \001(\002\"K\n\005Speed\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\013"
+  "\n\003end\030\006 \001(\005\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013"
+  "\n\003max\030\002 \001(\002\"-\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\"/\n\nTrajectory\022!\n\005pose"
+  "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\223\002\n\006Action\022\014"
+  "\n\004type\030\001 \001(\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage"
+  ".Pose2d\022\"\n\006target\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\022\'\n\013target_diff\030\004 \001(\0132\022.NavMessage.Po"
+  "se2d\022.\n\016velocity_limit\030\005 \001(\0132\026.NavMessag"
+  "e.SpeedLimit\022-\n\rangular_limit\030\006 \001(\0132\026.Na"
+  "vMessage.SpeedLimit\022,\n\014horize_limit\030\007 \001("
+  "\0132\026.NavMessage.SpeedLimit\"q\n\006NavCmd\022\016\n\006a"
+  "ction\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022%\n\ttransform\030\003 "
+  "\001(\0132\022.NavMessage.Pose2d\022#\n\007actions\030\004 \003(\013"
+  "2\022.NavMessage.Action\"\263\001\n\010NavStatu\022\r\n\005sta"
+  "tu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinished_acti"
+  "ons\030\003 \003(\0132\022.NavMessage.Action\022-\n\rselecte"
+  "d_traj\030\004 \001(\0132\026.NavMessage.Trajectory\022,\n\014"
+  "predict_traj\030\005 \001(\0132\026.NavMessage.Trajecto"
+  "ryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -335,7 +340,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 880,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 930,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 9, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 9, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -943,30 +948,37 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // int32 T = 2;
+      // int32 M = 2;
       case 2:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
-          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          m_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float V = 3;
+      // int32 T = 3;
       case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 24)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float W = 4;
+      // float V = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
-          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
-      // int32 end = 5;
+      // float W = 5;
       case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 45)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // int32 end = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
           end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -1005,28 +1017,34 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_m(), target);
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_t(), target);
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_v(), target);
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(5, this->_internal_w(), target);
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(6, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1052,24 +1070,31 @@ size_t Speed::ByteSizeLong() const {
         this->_internal_h());
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_m());
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
         this->_internal_t());
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
@@ -1110,6 +1135,9 @@ void Speed::MergeFrom(const Speed& from) {
   if (from.h() != 0) {
     _internal_set_h(from._internal_h());
   }
+  if (from.m() != 0) {
+    _internal_set_m(from._internal_m());
+  }
   if (from.t() != 0) {
     _internal_set_t(from._internal_t());
   }
@@ -2296,11 +2324,18 @@ void Action::InternalSwap(Action* other) {
 // ===================================================================
 
 void NavCmd::InitAsDefaultInstance() {
+  ::NavMessage::_NavCmd_default_instance_._instance.get_mutable()->transform_ = const_cast< ::NavMessage::Pose2d*>(
+      ::NavMessage::Pose2d::internal_default_instance());
 }
 class NavCmd::_Internal {
  public:
+  static const ::NavMessage::Pose2d& transform(const NavCmd* msg);
 };
 
+const ::NavMessage::Pose2d&
+NavCmd::_Internal::transform(const NavCmd* msg) {
+  return *msg->transform_;
+}
 NavCmd::NavCmd(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena),
   actions_(arena) {
@@ -2317,6 +2352,11 @@ NavCmd::NavCmd(const NavCmd& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
+  if (from._internal_has_transform()) {
+    transform_ = new ::NavMessage::Pose2d(*from.transform_);
+  } else {
+    transform_ = nullptr;
+  }
   action_ = from.action_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavCmd)
 }
@@ -2324,7 +2364,9 @@ NavCmd::NavCmd(const NavCmd& from)
 void NavCmd::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavCmd_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  action_ = 0;
+  ::memset(&transform_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&action_) -
+      reinterpret_cast<char*>(&transform_)) + sizeof(action_));
 }
 
 NavCmd::~NavCmd() {
@@ -2336,6 +2378,7 @@ NavCmd::~NavCmd() {
 void NavCmd::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (this != internal_default_instance()) delete transform_;
 }
 
 void NavCmd::ArenaDtor(void* object) {
@@ -2361,6 +2404,10 @@ void NavCmd::Clear() {
 
   actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
   action_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
@@ -2389,16 +2436,23 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // repeated .NavMessage.Action actions = 3;
+      // .NavMessage.Pose2d transform = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
+          ptr = ctx->ParseMessage(_internal_mutable_transform(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // repeated .NavMessage.Action actions = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
           ptr -= 1;
           do {
             ptr += 1;
             ptr = ctx->ParseMessage(_internal_add_actions(), ptr);
             CHK_(ptr);
             if (!ctx->DataAvailable(ptr)) break;
-          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<34>(ptr));
         } else goto handle_unusual;
         continue;
       default: {
@@ -2445,12 +2499,20 @@ failure:
         2, this->_internal_key(), target);
   }
 
-  // repeated .NavMessage.Action actions = 3;
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        3, _Internal::transform(this), target, stream);
+  }
+
+  // repeated .NavMessage.Action actions = 4;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_actions_size()); i < n; i++) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(3, this->_internal_actions(i), target, stream);
+      InternalWriteMessage(4, this->_internal_actions(i), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2469,7 +2531,7 @@ size_t NavCmd::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   total_size += 1UL * this->_internal_actions_size();
   for (const auto& msg : this->actions_) {
     total_size +=
@@ -2483,6 +2545,13 @@ size_t NavCmd::ByteSizeLong() const {
         this->_internal_key());
   }
 
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *transform_);
+  }
+
   // int32 action = 1;
   if (this->action() != 0) {
     total_size += 1 +
@@ -2525,6 +2594,9 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
+  if (from.has_transform()) {
+    _internal_mutable_transform()->::NavMessage::Pose2d::MergeFrom(from._internal_transform());
+  }
   if (from.action() != 0) {
     _internal_set_action(from._internal_action());
   }
@@ -2553,7 +2625,12 @@ void NavCmd::InternalSwap(NavCmd* other) {
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   actions_.InternalSwap(&other->actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  swap(action_, other->action_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(NavCmd, action_)
+      + sizeof(NavCmd::action_)
+      - PROTOBUF_FIELD_OFFSET(NavCmd, transform_)>(
+          reinterpret_cast<char*>(&transform_),
+          reinterpret_cast<char*>(&other->transform_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {

projects/controllers/AGV_controller/emqx-client/message.pb.h

@@ -541,10 +541,11 @@ class Speed PROTOBUF_FINAL :
 
   enum : int {
     kHFieldNumber = 1,
-    kTFieldNumber = 2,
-    kVFieldNumber = 3,
-    kWFieldNumber = 4,
-    kEndFieldNumber = 5,
+    kMFieldNumber = 2,
+    kTFieldNumber = 3,
+    kVFieldNumber = 4,
+    kWFieldNumber = 5,
+    kEndFieldNumber = 6,
   };
   // int32 H = 1;
   void clear_h();
@@ -555,7 +556,16 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_h(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // int32 T = 2;
+  // int32 M = 2;
+  void clear_m();
+  ::PROTOBUF_NAMESPACE_ID::int32 m() const;
+  void set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_m() const;
+  void _internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
+  // int32 T = 3;
   void clear_t();
   ::PROTOBUF_NAMESPACE_ID::int32 t() const;
   void set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -564,7 +574,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // float V = 3;
+  // float V = 4;
   void clear_v();
   float v() const;
   void set_v(float value);
@@ -573,7 +583,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_v(float value);
   public:
 
-  // float W = 4;
+  // float W = 5;
   void clear_w();
   float w() const;
   void set_w(float value);
@@ -582,7 +592,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_w(float value);
   public:
 
-  // int32 end = 5;
+  // int32 end = 6;
   void clear_end();
   ::PROTOBUF_NAMESPACE_ID::int32 end() const;
   void set_end(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -599,6 +609,7 @@ class Speed PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::int32 h_;
+  ::PROTOBUF_NAMESPACE_ID::int32 m_;
   ::PROTOBUF_NAMESPACE_ID::int32 t_;
   float v_;
   float w_;
@@ -1431,11 +1442,12 @@ class NavCmd PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kActionsFieldNumber = 3,
+    kActionsFieldNumber = 4,
     kKeyFieldNumber = 2,
+    kTransformFieldNumber = 3,
     kActionFieldNumber = 1,
   };
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   int actions_size() const;
   private:
   int _internal_actions_size() const;
@@ -1469,6 +1481,24 @@ class NavCmd PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
+  // .NavMessage.Pose2d transform = 3;
+  bool has_transform() const;
+  private:
+  bool _internal_has_transform() const;
+  public:
+  void clear_transform();
+  const ::NavMessage::Pose2d& transform() const;
+  ::NavMessage::Pose2d* release_transform();
+  ::NavMessage::Pose2d* mutable_transform();
+  void set_allocated_transform(::NavMessage::Pose2d* transform);
+  private:
+  const ::NavMessage::Pose2d& _internal_transform() const;
+  ::NavMessage::Pose2d* _internal_mutable_transform();
+  public:
+  void unsafe_arena_set_allocated_transform(
+      ::NavMessage::Pose2d* transform);
+  ::NavMessage::Pose2d* unsafe_arena_release_transform();
+
   // int32 action = 1;
   void clear_action();
   ::PROTOBUF_NAMESPACE_ID::int32 action() const;
@@ -1487,6 +1517,7 @@ class NavCmd PROTOBUF_FINAL :
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action > actions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
+  ::NavMessage::Pose2d* transform_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
@@ -1885,7 +1916,27 @@ inline void Speed::set_h(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.H)
 }
 
-// int32 T = 2;
+// int32 M = 2;
+inline void Speed::clear_m() {
+  m_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_m() const {
+  return m_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::m() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.M)
+  return _internal_m();
+}
+inline void Speed::_internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  m_ = value;
+}
+inline void Speed::set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_m(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.M)
+}
+
+// int32 T = 3;
 inline void Speed::clear_t() {
   t_ = 0;
 }
@@ -1905,7 +1956,7 @@ inline void Speed::set_t(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.T)
 }
 
-// float V = 3;
+// float V = 4;
 inline void Speed::clear_v() {
   v_ = 0;
 }
@@ -1925,7 +1976,7 @@ inline void Speed::set_v(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.V)
 }
 
-// float W = 4;
+// float W = 5;
 inline void Speed::clear_w() {
   w_ = 0;
 }
@@ -1945,7 +1996,7 @@ inline void Speed::set_w(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.W)
 }
 
-// int32 end = 5;
+// int32 end = 6;
 inline void Speed::clear_end() {
   end_ = 0;
 }
@@ -2724,7 +2775,90 @@ inline void NavCmd::set_allocated_key(std::string* key) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.key)
 }
 
-// repeated .NavMessage.Action actions = 3;
+// .NavMessage.Pose2d transform = 3;
+inline bool NavCmd::_internal_has_transform() const {
+  return this != internal_default_instance() && transform_ != nullptr;
+}
+inline bool NavCmd::has_transform() const {
+  return _internal_has_transform();
+}
+inline void NavCmd::clear_transform() {
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
+}
+inline const ::NavMessage::Pose2d& NavCmd::_internal_transform() const {
+  const ::NavMessage::Pose2d* p = transform_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::Pose2d*>(
+      &::NavMessage::_Pose2d_default_instance_);
+}
+inline const ::NavMessage::Pose2d& NavCmd::transform() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.transform)
+  return _internal_transform();
+}
+inline void NavCmd::unsafe_arena_set_allocated_transform(
+    ::NavMessage::Pose2d* transform) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(transform_);
+  }
+  transform_ = transform;
+  if (transform) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.NavCmd.transform)
+}
+inline ::NavMessage::Pose2d* NavCmd::release_transform() {
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::unsafe_arena_release_transform() {
+  // @@protoc_insertion_point(field_release:NavMessage.NavCmd.transform)
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::_internal_mutable_transform() {
+  
+  if (transform_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::Pose2d>(GetArena());
+    transform_ = p;
+  }
+  return transform_;
+}
+inline ::NavMessage::Pose2d* NavCmd::mutable_transform() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavCmd.transform)
+  return _internal_mutable_transform();
+}
+inline void NavCmd::set_allocated_transform(::NavMessage::Pose2d* transform) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete transform_;
+  }
+  if (transform) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(transform);
+    if (message_arena != submessage_arena) {
+      transform = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, transform, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  transform_ = transform;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.transform)
+}
+
+// repeated .NavMessage.Action actions = 4;
 inline int NavCmd::_internal_actions_size() const {
   return actions_.size();
 }

projects/controllers/agv_child/emqx-client/message.pb.cc

@@ -111,8 +111,9 @@ static void InitDefaultsscc_info_NavCmd_message_2eproto() {
   ::NavMessage::NavCmd::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_NavCmd_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavCmd_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavCmd_message_2eproto}, {
+      &scc_info_Pose2d_message_2eproto.base,
       &scc_info_Action_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_NavStatu_message_2eproto() {
@@ -216,6 +217,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, m_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
@@ -260,6 +262,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _weak_field_map_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, action_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, key_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, transform_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, actions_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, _internal_metadata_),
@@ -276,12 +279,12 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::AGVSpeed)},
   { 17, -1, sizeof(::NavMessage::Speed)},
-  { 27, -1, sizeof(::NavMessage::SpeedLimit)},
-  { 34, -1, sizeof(::NavMessage::Pose2d)},
-  { 42, -1, sizeof(::NavMessage::Trajectory)},
-  { 48, -1, sizeof(::NavMessage::Action)},
-  { 60, -1, sizeof(::NavMessage::NavCmd)},
-  { 68, -1, sizeof(::NavMessage::NavStatu)},
+  { 28, -1, sizeof(::NavMessage::SpeedLimit)},
+  { 35, -1, sizeof(::NavMessage::Pose2d)},
+  { 43, -1, sizeof(::NavMessage::Trajectory)},
+  { 49, -1, sizeof(::NavMessage::Action)},
+  { 61, -1, sizeof(::NavMessage::NavCmd)},
+  { 70, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -300,25 +303,27 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\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\t\n\001"
   "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\" \n\010AGVSpeed\022\t\n\001v\030\001 "
-  "\001(\002\022\t\n\001w\030\002 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005\022\t\n\001T\030"
-  "\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end\030\005 \001(\005"
-  "\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013\n\003max\030\002 \001(\002"
-  "\"-\n\006Pose2d\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\"/\n\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.N"
-  "avMessage.Pose2d\"\223\002\n\006Action\022\014\n\004type\030\001 \001("
-  "\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage.Pose2d\022\"\n\006"
-  "target\030\003 \001(\0132\022.NavMessage.Pose2d\022\'\n\013targ"
-  "et_diff\030\004 \001(\0132\022.NavMessage.Pose2d\022.\n\016vel"
-  "ocity_limit\030\005 \001(\0132\026.NavMessage.SpeedLimi"
-  "t\022-\n\rangular_limit\030\006 \001(\0132\026.NavMessage.Sp"
-  "eedLimit\022,\n\014horize_limit\030\007 \001(\0132\026.NavMess"
-  "age.SpeedLimit\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005"
-  "\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\0132\022.NavMess"
-  "age.Action\"\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013"
-  "\n\003key\030\002 \001(\t\022.\n\022unfinished_actions\030\003 \003(\0132"
-  "\022.NavMessage.Action\022-\n\rselected_traj\030\004 \001"
-  "(\0132\026.NavMessage.Trajectory\022,\n\014predict_tr"
-  "aj\030\005 \001(\0132\026.NavMessage.Trajectoryb\006proto3"
+  "\001(\002\022\t\n\001w\030\002 \001(\002\"K\n\005Speed\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\013"
+  "\n\003end\030\006 \001(\005\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013"
+  "\n\003max\030\002 \001(\002\"-\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\"/\n\nTrajectory\022!\n\005pose"
+  "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\223\002\n\006Action\022\014"
+  "\n\004type\030\001 \001(\005\022!\n\005begin\030\002 \001(\0132\022.NavMessage"
+  ".Pose2d\022\"\n\006target\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\022\'\n\013target_diff\030\004 \001(\0132\022.NavMessage.Po"
+  "se2d\022.\n\016velocity_limit\030\005 \001(\0132\026.NavMessag"
+  "e.SpeedLimit\022-\n\rangular_limit\030\006 \001(\0132\026.Na"
+  "vMessage.SpeedLimit\022,\n\014horize_limit\030\007 \001("
+  "\0132\026.NavMessage.SpeedLimit\"q\n\006NavCmd\022\016\n\006a"
+  "ction\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022%\n\ttransform\030\003 "
+  "\001(\0132\022.NavMessage.Pose2d\022#\n\007actions\030\004 \003(\013"
+  "2\022.NavMessage.Action\"\263\001\n\010NavStatu\022\r\n\005sta"
+  "tu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinished_acti"
+  "ons\030\003 \003(\0132\022.NavMessage.Action\022-\n\rselecte"
+  "d_traj\030\004 \001(\0132\026.NavMessage.Trajectory\022,\n\014"
+  "predict_traj\030\005 \001(\0132\026.NavMessage.Trajecto"
+  "ryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -335,7 +340,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 880,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 930,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 9, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 9, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -943,30 +948,37 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // int32 T = 2;
+      // int32 M = 2;
       case 2:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
-          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          m_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float V = 3;
+      // int32 T = 3;
       case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 24)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float W = 4;
+      // float V = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
-          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
-      // int32 end = 5;
+      // float W = 5;
       case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 45)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // int32 end = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
           end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -1005,28 +1017,34 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_m(), target);
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_t(), target);
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_v(), target);
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(5, this->_internal_w(), target);
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(6, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1052,24 +1070,31 @@ size_t Speed::ByteSizeLong() const {
         this->_internal_h());
   }
 
-  // int32 T = 2;
+  // int32 M = 2;
+  if (this->m() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_m());
+  }
+
+  // int32 T = 3;
   if (this->t() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
         this->_internal_t());
   }
 
-  // float V = 3;
+  // float V = 4;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float W = 4;
+  // float W = 5;
   if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // int32 end = 5;
+  // int32 end = 6;
   if (this->end() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
@@ -1110,6 +1135,9 @@ void Speed::MergeFrom(const Speed& from) {
   if (from.h() != 0) {
     _internal_set_h(from._internal_h());
   }
+  if (from.m() != 0) {
+    _internal_set_m(from._internal_m());
+  }
   if (from.t() != 0) {
     _internal_set_t(from._internal_t());
   }
@@ -2296,11 +2324,18 @@ void Action::InternalSwap(Action* other) {
 // ===================================================================
 
 void NavCmd::InitAsDefaultInstance() {
+  ::NavMessage::_NavCmd_default_instance_._instance.get_mutable()->transform_ = const_cast< ::NavMessage::Pose2d*>(
+      ::NavMessage::Pose2d::internal_default_instance());
 }
 class NavCmd::_Internal {
  public:
+  static const ::NavMessage::Pose2d& transform(const NavCmd* msg);
 };
 
+const ::NavMessage::Pose2d&
+NavCmd::_Internal::transform(const NavCmd* msg) {
+  return *msg->transform_;
+}
 NavCmd::NavCmd(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   : ::PROTOBUF_NAMESPACE_ID::Message(arena),
   actions_(arena) {
@@ -2317,6 +2352,11 @@ NavCmd::NavCmd(const NavCmd& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
+  if (from._internal_has_transform()) {
+    transform_ = new ::NavMessage::Pose2d(*from.transform_);
+  } else {
+    transform_ = nullptr;
+  }
   action_ = from.action_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavCmd)
 }
@@ -2324,7 +2364,9 @@ NavCmd::NavCmd(const NavCmd& from)
 void NavCmd::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavCmd_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  action_ = 0;
+  ::memset(&transform_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&action_) -
+      reinterpret_cast<char*>(&transform_)) + sizeof(action_));
 }
 
 NavCmd::~NavCmd() {
@@ -2336,6 +2378,7 @@ NavCmd::~NavCmd() {
 void NavCmd::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (this != internal_default_instance()) delete transform_;
 }
 
 void NavCmd::ArenaDtor(void* object) {
@@ -2361,6 +2404,10 @@ void NavCmd::Clear() {
 
   actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
   action_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
@@ -2389,16 +2436,23 @@ const char* NavCmd::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::int
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // repeated .NavMessage.Action actions = 3;
+      // .NavMessage.Pose2d transform = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
+          ptr = ctx->ParseMessage(_internal_mutable_transform(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // repeated .NavMessage.Action actions = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
           ptr -= 1;
           do {
             ptr += 1;
             ptr = ctx->ParseMessage(_internal_add_actions(), ptr);
             CHK_(ptr);
             if (!ctx->DataAvailable(ptr)) break;
-          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<34>(ptr));
         } else goto handle_unusual;
         continue;
       default: {
@@ -2445,12 +2499,20 @@ failure:
         2, this->_internal_key(), target);
   }
 
-  // repeated .NavMessage.Action actions = 3;
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        3, _Internal::transform(this), target, stream);
+  }
+
+  // repeated .NavMessage.Action actions = 4;
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->_internal_actions_size()); i < n; i++) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(3, this->_internal_actions(i), target, stream);
+      InternalWriteMessage(4, this->_internal_actions(i), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2469,7 +2531,7 @@ size_t NavCmd::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   total_size += 1UL * this->_internal_actions_size();
   for (const auto& msg : this->actions_) {
     total_size +=
@@ -2483,6 +2545,13 @@ size_t NavCmd::ByteSizeLong() const {
         this->_internal_key());
   }
 
+  // .NavMessage.Pose2d transform = 3;
+  if (this->has_transform()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *transform_);
+  }
+
   // int32 action = 1;
   if (this->action() != 0) {
     total_size += 1 +
@@ -2525,6 +2594,9 @@ void NavCmd::MergeFrom(const NavCmd& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
+  if (from.has_transform()) {
+    _internal_mutable_transform()->::NavMessage::Pose2d::MergeFrom(from._internal_transform());
+  }
   if (from.action() != 0) {
     _internal_set_action(from._internal_action());
   }
@@ -2553,7 +2625,12 @@ void NavCmd::InternalSwap(NavCmd* other) {
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   actions_.InternalSwap(&other->actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  swap(action_, other->action_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(NavCmd, action_)
+      + sizeof(NavCmd::action_)
+      - PROTOBUF_FIELD_OFFSET(NavCmd, transform_)>(
+          reinterpret_cast<char*>(&transform_),
+          reinterpret_cast<char*>(&other->transform_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {

projects/controllers/agv_child/emqx-client/message.pb.h

@@ -541,10 +541,11 @@ class Speed PROTOBUF_FINAL :
 
   enum : int {
     kHFieldNumber = 1,
-    kTFieldNumber = 2,
-    kVFieldNumber = 3,
-    kWFieldNumber = 4,
-    kEndFieldNumber = 5,
+    kMFieldNumber = 2,
+    kTFieldNumber = 3,
+    kVFieldNumber = 4,
+    kWFieldNumber = 5,
+    kEndFieldNumber = 6,
   };
   // int32 H = 1;
   void clear_h();
@@ -555,7 +556,16 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_h(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // int32 T = 2;
+  // int32 M = 2;
+  void clear_m();
+  ::PROTOBUF_NAMESPACE_ID::int32 m() const;
+  void set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_m() const;
+  void _internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
+  // int32 T = 3;
   void clear_t();
   ::PROTOBUF_NAMESPACE_ID::int32 t() const;
   void set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -564,7 +574,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // float V = 3;
+  // float V = 4;
   void clear_v();
   float v() const;
   void set_v(float value);
@@ -573,7 +583,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_v(float value);
   public:
 
-  // float W = 4;
+  // float W = 5;
   void clear_w();
   float w() const;
   void set_w(float value);
@@ -582,7 +592,7 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_w(float value);
   public:
 
-  // int32 end = 5;
+  // int32 end = 6;
   void clear_end();
   ::PROTOBUF_NAMESPACE_ID::int32 end() const;
   void set_end(::PROTOBUF_NAMESPACE_ID::int32 value);
@@ -599,6 +609,7 @@ class Speed PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::int32 h_;
+  ::PROTOBUF_NAMESPACE_ID::int32 m_;
   ::PROTOBUF_NAMESPACE_ID::int32 t_;
   float v_;
   float w_;
@@ -1431,11 +1442,12 @@ class NavCmd PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kActionsFieldNumber = 3,
+    kActionsFieldNumber = 4,
     kKeyFieldNumber = 2,
+    kTransformFieldNumber = 3,
     kActionFieldNumber = 1,
   };
-  // repeated .NavMessage.Action actions = 3;
+  // repeated .NavMessage.Action actions = 4;
   int actions_size() const;
   private:
   int _internal_actions_size() const;
@@ -1469,6 +1481,24 @@ class NavCmd PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
+  // .NavMessage.Pose2d transform = 3;
+  bool has_transform() const;
+  private:
+  bool _internal_has_transform() const;
+  public:
+  void clear_transform();
+  const ::NavMessage::Pose2d& transform() const;
+  ::NavMessage::Pose2d* release_transform();
+  ::NavMessage::Pose2d* mutable_transform();
+  void set_allocated_transform(::NavMessage::Pose2d* transform);
+  private:
+  const ::NavMessage::Pose2d& _internal_transform() const;
+  ::NavMessage::Pose2d* _internal_mutable_transform();
+  public:
+  void unsafe_arena_set_allocated_transform(
+      ::NavMessage::Pose2d* transform);
+  ::NavMessage::Pose2d* unsafe_arena_release_transform();
+
   // int32 action = 1;
   void clear_action();
   ::PROTOBUF_NAMESPACE_ID::int32 action() const;
@@ -1487,6 +1517,7 @@ class NavCmd PROTOBUF_FINAL :
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action > actions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
+  ::NavMessage::Pose2d* transform_;
   ::PROTOBUF_NAMESPACE_ID::int32 action_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
@@ -1885,7 +1916,27 @@ inline void Speed::set_h(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.H)
 }
 
-// int32 T = 2;
+// int32 M = 2;
+inline void Speed::clear_m() {
+  m_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_m() const {
+  return m_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::m() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.M)
+  return _internal_m();
+}
+inline void Speed::_internal_set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  m_ = value;
+}
+inline void Speed::set_m(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_m(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.M)
+}
+
+// int32 T = 3;
 inline void Speed::clear_t() {
   t_ = 0;
 }
@@ -1905,7 +1956,7 @@ inline void Speed::set_t(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.T)
 }
 
-// float V = 3;
+// float V = 4;
 inline void Speed::clear_v() {
   v_ = 0;
 }
@@ -1925,7 +1976,7 @@ inline void Speed::set_v(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.V)
 }
 
-// float W = 4;
+// float W = 5;
 inline void Speed::clear_w() {
   w_ = 0;
 }
@@ -1945,7 +1996,7 @@ inline void Speed::set_w(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.W)
 }
 
-// int32 end = 5;
+// int32 end = 6;
 inline void Speed::clear_end() {
   end_ = 0;
 }
@@ -2724,7 +2775,90 @@ inline void NavCmd::set_allocated_key(std::string* key) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.key)
 }
 
-// repeated .NavMessage.Action actions = 3;
+// .NavMessage.Pose2d transform = 3;
+inline bool NavCmd::_internal_has_transform() const {
+  return this != internal_default_instance() && transform_ != nullptr;
+}
+inline bool NavCmd::has_transform() const {
+  return _internal_has_transform();
+}
+inline void NavCmd::clear_transform() {
+  if (GetArena() == nullptr && transform_ != nullptr) {
+    delete transform_;
+  }
+  transform_ = nullptr;
+}
+inline const ::NavMessage::Pose2d& NavCmd::_internal_transform() const {
+  const ::NavMessage::Pose2d* p = transform_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::Pose2d*>(
+      &::NavMessage::_Pose2d_default_instance_);
+}
+inline const ::NavMessage::Pose2d& NavCmd::transform() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.transform)
+  return _internal_transform();
+}
+inline void NavCmd::unsafe_arena_set_allocated_transform(
+    ::NavMessage::Pose2d* transform) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(transform_);
+  }
+  transform_ = transform;
+  if (transform) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.NavCmd.transform)
+}
+inline ::NavMessage::Pose2d* NavCmd::release_transform() {
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::unsafe_arena_release_transform() {
+  // @@protoc_insertion_point(field_release:NavMessage.NavCmd.transform)
+  
+  ::NavMessage::Pose2d* temp = transform_;
+  transform_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::Pose2d* NavCmd::_internal_mutable_transform() {
+  
+  if (transform_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::Pose2d>(GetArena());
+    transform_ = p;
+  }
+  return transform_;
+}
+inline ::NavMessage::Pose2d* NavCmd::mutable_transform() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavCmd.transform)
+  return _internal_mutable_transform();
+}
+inline void NavCmd::set_allocated_transform(::NavMessage::Pose2d* transform) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete transform_;
+  }
+  if (transform) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(transform);
+    if (message_arena != submessage_arena) {
+      transform = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, transform, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  transform_ = transform;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavCmd.transform)
+}
+
+// repeated .NavMessage.Action actions = 4;
 inline int NavCmd::_internal_actions_size() const {
   return actions_.size();
 }