增加前后车是否在库位信息判断，根据先来后到，自主选择车位1还是2点（未完）

MPC/monitor/emqx/message.pb.cc

@@ -15,6 +15,7 @@
 // @@protoc_insertion_point(includes)
 #include <google/protobuf/port_def.inc>
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_AgvStatu_message_2eproto;
+extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_LidarOdomStatu_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NewAction_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_PathNode_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Pose2d_message_2eproto;
@@ -120,8 +121,9 @@ static void InitDefaultsscc_info_NavStatu_message_2eproto() {
   ::NavMessage::NavStatu::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_NavStatu_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavStatu_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
+      &scc_info_LidarOdomStatu_message_2eproto.base,
       &scc_info_Trajectory_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_NewAction_message_2eproto() {
@@ -287,6 +289,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, l_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, w_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, theta_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, id_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Trajectory, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -326,9 +329,12 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   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, odom_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, predict_traj_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, in_space_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, space_id_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -346,11 +352,11 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 31, -1, sizeof(::NavMessage::SpeedLimit)},
   { 38, -1, sizeof(::NavMessage::Pose2d)},
   { 46, -1, sizeof(::NavMessage::PathNode)},
-  { 56, -1, sizeof(::NavMessage::Trajectory)},
-  { 62, -1, sizeof(::NavMessage::NewAction)},
-  { 79, -1, sizeof(::NavMessage::NavCmd)},
-  { 87, -1, sizeof(::NavMessage::NavStatu)},
-  { 98, -1, sizeof(::NavMessage::RobotStatu)},
+  { 57, -1, sizeof(::NavMessage::Trajectory)},
+  { 63, -1, sizeof(::NavMessage::NewAction)},
+  { 80, -1, sizeof(::NavMessage::NavCmd)},
+  { 88, -1, sizeof(::NavMessage::NavStatu)},
+  { 102, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -376,31 +382,33 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "\t\n\001M\030\002 \001(\005\022\t\n\001T\030\003 \001(\005\022\t\n\001V\030\004 \001(\002\022\t\n\001W\030\005 "
   "\001(\002\022\t\n\001L\030\006 \001(\002\022\013\n\003end\030\007 \001(\005\"&\n\nSpeedLimi"
   "t\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\"E\n\010Pa"
+  "\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"Q\n\010Pa"
   "thNode\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022"
-  "\t\n\001w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\"/\n\nTrajectory\022"
-  "!\n\005poses\030\001 \003(\0132\022.NavMessage.Pose2d\"\340\003\n\tN"
-  "ewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001("
-  "\0132\024.NavMessage.PathNode\022&\n\010passNode\030\003 \001("
-  "\0132\024.NavMessage.PathNode\022(\n\nstreetNode\030\004 "
-  "\001(\0132\024.NavMessage.PathNode\022\'\n\tpathNodes\030\005"
-  " \003(\0132\024.NavMessage.PathNode\022+\n\013InOutVLimi"
-  "t\030\006 \001(\0132\026.NavMessage.SpeedLimit\0221\n\021NodeV"
-  "elocityLimit\030\007 \001(\0132\026.NavMessage.SpeedLim"
-  "it\0220\n\020NodeAngularLimit\030\010 \001(\0132\026.NavMessag"
-  "e.SpeedLimit\0223\n\023adjustVelocitylimit\030\t \001("
-  "\0132\026.NavMessage.SpeedLimit\0222\n\022adjustHoriz"
-  "onLimit\030\n \001(\0132\026.NavMessage.SpeedLimit\022\021\n"
-  "\twheelbase\030\013 \001(\002\022\023\n\013changedMode\030\014 \001(\005\"P\n"
-  "\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\n"
-  "newActions\030\005 \003(\0132\025.NavMessage.NewAction\""
-  "\250\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\005\022\020\n\010main_agv\030"
-  "\002 \001(\010\022\021\n\tmove_mode\030\003 \001(\005\022\013\n\003key\030\004 \001(\t\022-\n"
-  "\rselected_traj\030\006 \001(\0132\026.NavMessage.Trajec"
-  "tory\022,\n\014predict_traj\030\007 \001(\0132\026.NavMessage."
-  "Trajectory\"Y\n\nRobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y"
-  "\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132"
-  "\024.NavMessage.AgvStatub\006proto3"
+  "\t\n\001w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n"
+  "\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage."
+  "Pose2d\"\340\003\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tsp"
+  "aceNode\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010p"
+  "assNode\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\ns"
+  "treetNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n"
+  "\tpathNodes\030\005 \003(\0132\024.NavMessage.PathNode\022+"
+  "\n\013InOutVLimit\030\006 \001(\0132\026.NavMessage.SpeedLi"
+  "mit\0221\n\021NodeVelocityLimit\030\007 \001(\0132\026.NavMess"
+  "age.SpeedLimit\0220\n\020NodeAngularLimit\030\010 \001(\013"
+  "2\026.NavMessage.SpeedLimit\0223\n\023adjustVeloci"
+  "tylimit\030\t \001(\0132\026.NavMessage.SpeedLimit\0222\n"
+  "\022adjustHorizonLimit\030\n \001(\0132\026.NavMessage.S"
+  "peedLimit\022\021\n\twheelbase\030\013 \001(\002\022\023\n\013changedM"
+  "ode\030\014 \001(\005\"P\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003k"
+  "ey\030\002 \001(\t\022)\n\nnewActions\030\005 \003(\0132\025.NavMessag"
+  "e.NewAction\"\366\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\005\022"
+  "\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001(\005\022(\n\004"
+  "odom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013"
+  "\n\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.Nav"
+  "Message.Trajectory\022,\n\014predict_traj\030\007 \001(\013"
+  "2\026.NavMessage.Trajectory\022\020\n\010in_space\030\010 \001"
+  "(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobotStatu\022\t\n\001x\030"
+  "\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvSt"
+  "atu\030\004 \001(\0132\024.NavMessage.AgvStatub\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -419,7 +427,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", 1309,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 1399,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 11, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 11, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -1824,6 +1832,11 @@ PathNode::PathNode(::PROTOBUF_NAMESPACE_ID::Arena* arena)
 PathNode::PathNode(const PathNode& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  id_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (!from._internal_id().empty()) {
+    id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_id(),
+      GetArena());
+  }
   ::memcpy(&x_, &from.x_,
     static_cast<size_t>(reinterpret_cast<char*>(&theta_) -
     reinterpret_cast<char*>(&x_)) + sizeof(theta_));
@@ -1831,6 +1844,8 @@ PathNode::PathNode(const PathNode& from)
 }
 
 void PathNode::SharedCtor() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_PathNode_message_2eproto.base);
+  id_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
   ::memset(&x_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&theta_) -
       reinterpret_cast<char*>(&x_)) + sizeof(theta_));
@@ -1844,6 +1859,7 @@ PathNode::~PathNode() {
 
 void PathNode::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
+  id_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
 }
 
 void PathNode::ArenaDtor(void* object) {
@@ -1867,6 +1883,7 @@ void PathNode::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
+  id_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::memset(&x_, 0, static_cast<size_t>(
       reinterpret_cast<char*>(&theta_) -
       reinterpret_cast<char*>(&x_)) + sizeof(theta_));
@@ -1916,6 +1933,15 @@ const char* PathNode::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // string id = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 50)) {
+          auto str = _internal_mutable_id();
+          ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
+          CHK_(::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "NavMessage.PathNode.id"));
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -1974,6 +2000,16 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(5, this->_internal_theta(), target);
   }
 
+  // string id = 6;
+  if (this->id().size() > 0) {
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
+      this->_internal_id().data(), static_cast<int>(this->_internal_id().length()),
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE,
+      "NavMessage.PathNode.id");
+    target = stream->WriteStringMaybeAliased(
+        6, this->_internal_id(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -1990,6 +2026,13 @@ size_t PathNode::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
+  // string id = 6;
+  if (this->id().size() > 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+        this->_internal_id());
+  }
+
   // float x = 1;
   if (!(this->x() <= 0 && this->x() >= 0)) {
     total_size += 1 + 4;
@@ -2046,6 +2089,9 @@ void PathNode::MergeFrom(const PathNode& from) {
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
+  if (from.id().size() > 0) {
+    _internal_set_id(from._internal_id());
+  }
   if (!(from.x() <= 0 && from.x() >= 0)) {
     _internal_set_x(from._internal_x());
   }
@@ -2084,6 +2130,7 @@ bool PathNode::IsInitialized() const {
 void PathNode::InternalSwap(PathNode* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
+  id_.Swap(&other->id_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
       PROTOBUF_FIELD_OFFSET(PathNode, theta_)
       + sizeof(PathNode::theta_)
@@ -3192,6 +3239,8 @@ void NavCmd::InternalSwap(NavCmd* other) {
 // ===================================================================
 
 void NavStatu::InitAsDefaultInstance() {
+  ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->odom_ = const_cast< ::NavMessage::LidarOdomStatu*>(
+      ::NavMessage::LidarOdomStatu::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->selected_traj_ = const_cast< ::NavMessage::Trajectory*>(
       ::NavMessage::Trajectory::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->predict_traj_ = const_cast< ::NavMessage::Trajectory*>(
@@ -3199,10 +3248,15 @@ void NavStatu::InitAsDefaultInstance() {
 }
 class NavStatu::_Internal {
  public:
+  static const ::NavMessage::LidarOdomStatu& odom(const NavStatu* msg);
   static const ::NavMessage::Trajectory& selected_traj(const NavStatu* msg);
   static const ::NavMessage::Trajectory& predict_traj(const NavStatu* msg);
 };
 
+const ::NavMessage::LidarOdomStatu&
+NavStatu::_Internal::odom(const NavStatu* msg) {
+  return *msg->odom_;
+}
 const ::NavMessage::Trajectory&
 NavStatu::_Internal::selected_traj(const NavStatu* msg) {
   return *msg->selected_traj_;
@@ -3225,6 +3279,16 @@ NavStatu::NavStatu(const NavStatu& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
+  space_id_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (!from._internal_space_id().empty()) {
+    space_id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_space_id(),
+      GetArena());
+  }
+  if (from._internal_has_odom()) {
+    odom_ = new ::NavMessage::LidarOdomStatu(*from.odom_);
+  } else {
+    odom_ = nullptr;
+  }
   if (from._internal_has_selected_traj()) {
     selected_traj_ = new ::NavMessage::Trajectory(*from.selected_traj_);
   } else {
@@ -3236,17 +3300,18 @@ NavStatu::NavStatu(const NavStatu& from)
     predict_traj_ = nullptr;
   }
   ::memcpy(&statu_, &from.statu_,
-    static_cast<size_t>(reinterpret_cast<char*>(&move_mode_) -
-    reinterpret_cast<char*>(&statu_)) + sizeof(move_mode_));
+    static_cast<size_t>(reinterpret_cast<char*>(&in_space_) -
+    reinterpret_cast<char*>(&statu_)) + sizeof(in_space_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavStatu)
 }
 
 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*>(&move_mode_) -
-      reinterpret_cast<char*>(&selected_traj_)) + sizeof(move_mode_));
+  space_id_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  ::memset(&odom_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&in_space_) -
+      reinterpret_cast<char*>(&odom_)) + sizeof(in_space_));
 }
 
 NavStatu::~NavStatu() {
@@ -3258,6 +3323,8 @@ NavStatu::~NavStatu() {
 void NavStatu::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  space_id_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (this != internal_default_instance()) delete odom_;
   if (this != internal_default_instance()) delete selected_traj_;
   if (this != internal_default_instance()) delete predict_traj_;
 }
@@ -3284,6 +3351,11 @@ void NavStatu::Clear() {
   (void) cached_has_bits;
 
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  space_id_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  if (GetArena() == nullptr && odom_ != nullptr) {
+    delete odom_;
+  }
+  odom_ = nullptr;
   if (GetArena() == nullptr && selected_traj_ != nullptr) {
     delete selected_traj_;
   }
@@ -3293,8 +3365,8 @@ void NavStatu::Clear() {
   }
   predict_traj_ = nullptr;
   ::memset(&statu_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&move_mode_) -
-      reinterpret_cast<char*>(&statu_)) + sizeof(move_mode_));
+      reinterpret_cast<char*>(&in_space_) -
+      reinterpret_cast<char*>(&statu_)) + sizeof(in_space_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -3327,9 +3399,16 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // string key = 4;
+      // .NavMessage.LidarOdomStatu odom = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
+          ptr = ctx->ParseMessage(_internal_mutable_odom(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // string key = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
           auto str = _internal_mutable_key();
           ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
           CHK_(::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "NavMessage.NavStatu.key"));
@@ -3350,6 +3429,22 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
+      // bool in_space = 8;
+      case 8:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 64)) {
+          in_space_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // string space_id = 9;
+      case 9:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 74)) {
+          auto str = _internal_mutable_space_id();
+          ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
+          CHK_(::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "NavMessage.NavStatu.space_id"));
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -3396,14 +3491,22 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_move_mode(), target);
   }
 
-  // string key = 4;
+  // .NavMessage.LidarOdomStatu odom = 4;
+  if (this->has_odom()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        4, _Internal::odom(this), target, stream);
+  }
+
+  // string key = 5;
   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(
-        4, this->_internal_key(), target);
+        5, this->_internal_key(), target);
   }
 
   // .NavMessage.Trajectory selected_traj = 6;
@@ -3422,6 +3525,22 @@ failure:
         7, _Internal::predict_traj(this), target, stream);
   }
 
+  // bool in_space = 8;
+  if (this->in_space() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(8, this->_internal_in_space(), target);
+  }
+
+  // string space_id = 9;
+  if (this->space_id().size() > 0) {
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
+      this->_internal_space_id().data(), static_cast<int>(this->_internal_space_id().length()),
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE,
+      "NavMessage.NavStatu.space_id");
+    target = stream->WriteStringMaybeAliased(
+        9, this->_internal_space_id(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -3438,13 +3557,27 @@ size_t NavStatu::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // string key = 4;
+  // string key = 5;
   if (this->key().size() > 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
         this->_internal_key());
   }
 
+  // string space_id = 9;
+  if (this->space_id().size() > 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+        this->_internal_space_id());
+  }
+
+  // .NavMessage.LidarOdomStatu odom = 4;
+  if (this->has_odom()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *odom_);
+  }
+
   // .NavMessage.Trajectory selected_traj = 6;
   if (this->has_selected_traj()) {
     total_size += 1 +
@@ -3466,11 +3599,6 @@ size_t NavStatu::ByteSizeLong() const {
         this->_internal_statu());
   }
 
-  // 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 +
@@ -3478,6 +3606,16 @@ size_t NavStatu::ByteSizeLong() const {
         this->_internal_move_mode());
   }
 
+  // bool main_agv = 2;
+  if (this->main_agv() != 0) {
+    total_size += 1 + 1;
+  }
+
+  // bool in_space = 8;
+  if (this->in_space() != 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_);
@@ -3512,6 +3650,12 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
+  if (from.space_id().size() > 0) {
+    _internal_set_space_id(from._internal_space_id());
+  }
+  if (from.has_odom()) {
+    _internal_mutable_odom()->::NavMessage::LidarOdomStatu::MergeFrom(from._internal_odom());
+  }
   if (from.has_selected_traj()) {
     _internal_mutable_selected_traj()->::NavMessage::Trajectory::MergeFrom(from._internal_selected_traj());
   }
@@ -3521,11 +3665,14 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.statu() != 0) {
     _internal_set_statu(from._internal_statu());
   }
+  if (from.move_mode() != 0) {
+    _internal_set_move_mode(from._internal_move_mode());
+  }
   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());
+  if (from.in_space() != 0) {
+    _internal_set_in_space(from._internal_in_space());
   }
 }
 
@@ -3551,12 +3698,13 @@ void NavStatu::InternalSwap(NavStatu* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  space_id_.Swap(&other->space_id_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      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_));
+      PROTOBUF_FIELD_OFFSET(NavStatu, in_space_)
+      + sizeof(NavStatu::in_space_)
+      - PROTOBUF_FIELD_OFFSET(NavStatu, odom_)>(
+          reinterpret_cast<char*>(&odom_),
+          reinterpret_cast<char*>(&other->odom_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavStatu::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -1080,12 +1080,29 @@ class PathNode PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
+    kIdFieldNumber = 6,
     kXFieldNumber = 1,
     kYFieldNumber = 2,
     kLFieldNumber = 3,
     kWFieldNumber = 4,
     kThetaFieldNumber = 5,
   };
+  // string id = 6;
+  void clear_id();
+  const std::string& id() const;
+  void set_id(const std::string& value);
+  void set_id(std::string&& value);
+  void set_id(const char* value);
+  void set_id(const char* value, size_t size);
+  std::string* mutable_id();
+  std::string* release_id();
+  void set_allocated_id(std::string* id);
+  private:
+  const std::string& _internal_id() const;
+  void _internal_set_id(const std::string& value);
+  std::string* _internal_mutable_id();
+  public:
+
   // float x = 1;
   void clear_x();
   float x() const;
@@ -1138,6 +1155,7 @@ class PathNode PROTOBUF_FINAL :
   template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
+  ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr id_;
   float x_;
   float y_;
   float l_;
@@ -1921,14 +1939,17 @@ class NavStatu PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kKeyFieldNumber = 4,
+    kKeyFieldNumber = 5,
+    kSpaceIdFieldNumber = 9,
+    kOdomFieldNumber = 4,
     kSelectedTrajFieldNumber = 6,
     kPredictTrajFieldNumber = 7,
     kStatuFieldNumber = 1,
-    kMainAgvFieldNumber = 2,
     kMoveModeFieldNumber = 3,
+    kMainAgvFieldNumber = 2,
+    kInSpaceFieldNumber = 8,
   };
-  // string key = 4;
+  // string key = 5;
   void clear_key();
   const std::string& key() const;
   void set_key(const std::string& value);
@@ -1944,6 +1965,40 @@ class NavStatu PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
+  // string space_id = 9;
+  void clear_space_id();
+  const std::string& space_id() const;
+  void set_space_id(const std::string& value);
+  void set_space_id(std::string&& value);
+  void set_space_id(const char* value);
+  void set_space_id(const char* value, size_t size);
+  std::string* mutable_space_id();
+  std::string* release_space_id();
+  void set_allocated_space_id(std::string* space_id);
+  private:
+  const std::string& _internal_space_id() const;
+  void _internal_set_space_id(const std::string& value);
+  std::string* _internal_mutable_space_id();
+  public:
+
+  // .NavMessage.LidarOdomStatu odom = 4;
+  bool has_odom() const;
+  private:
+  bool _internal_has_odom() const;
+  public:
+  void clear_odom();
+  const ::NavMessage::LidarOdomStatu& odom() const;
+  ::NavMessage::LidarOdomStatu* release_odom();
+  ::NavMessage::LidarOdomStatu* mutable_odom();
+  void set_allocated_odom(::NavMessage::LidarOdomStatu* odom);
+  private:
+  const ::NavMessage::LidarOdomStatu& _internal_odom() const;
+  ::NavMessage::LidarOdomStatu* _internal_mutable_odom();
+  public:
+  void unsafe_arena_set_allocated_odom(
+      ::NavMessage::LidarOdomStatu* odom);
+  ::NavMessage::LidarOdomStatu* unsafe_arena_release_odom();
+
   // .NavMessage.Trajectory selected_traj = 6;
   bool has_selected_traj() const;
   private:
@@ -1989,6 +2044,15 @@ class NavStatu PROTOBUF_FINAL :
   void _internal_set_statu(::PROTOBUF_NAMESPACE_ID::int32 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:
+
   // bool main_agv = 2;
   void clear_main_agv();
   bool main_agv() const;
@@ -1998,13 +2062,13 @@ class NavStatu PROTOBUF_FINAL :
   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);
+  // bool in_space = 8;
+  void clear_in_space();
+  bool in_space() const;
+  void set_in_space(bool value);
   private:
-  ::PROTOBUF_NAMESPACE_ID::int32 _internal_move_mode() const;
-  void _internal_set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value);
+  bool _internal_in_space() const;
+  void _internal_set_in_space(bool value);
   public:
 
   // @@protoc_insertion_point(class_scope:NavMessage.NavStatu)
@@ -2015,11 +2079,14 @@ class NavStatu PROTOBUF_FINAL :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
+  ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr space_id_;
+  ::NavMessage::LidarOdomStatu* odom_;
   ::NavMessage::Trajectory* selected_traj_;
   ::NavMessage::Trajectory* predict_traj_;
   ::PROTOBUF_NAMESPACE_ID::int32 statu_;
-  bool main_agv_;
   ::PROTOBUF_NAMESPACE_ID::int32 move_mode_;
+  bool main_agv_;
+  bool in_space_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -2753,6 +2820,68 @@ inline void PathNode::set_theta(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.PathNode.theta)
 }
 
+// string id = 6;
+inline void PathNode::clear_id() {
+  id_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline const std::string& PathNode::id() const {
+  // @@protoc_insertion_point(field_get:NavMessage.PathNode.id)
+  return _internal_id();
+}
+inline void PathNode::set_id(const std::string& value) {
+  _internal_set_id(value);
+  // @@protoc_insertion_point(field_set:NavMessage.PathNode.id)
+}
+inline std::string* PathNode::mutable_id() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.PathNode.id)
+  return _internal_mutable_id();
+}
+inline const std::string& PathNode::_internal_id() const {
+  return id_.Get();
+}
+inline void PathNode::_internal_set_id(const std::string& value) {
+  
+  id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena());
+}
+inline void PathNode::set_id(std::string&& value) {
+  
+  id_.Set(
+    &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena());
+  // @@protoc_insertion_point(field_set_rvalue:NavMessage.PathNode.id)
+}
+inline void PathNode::set_id(const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  
+  id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value),
+              GetArena());
+  // @@protoc_insertion_point(field_set_char:NavMessage.PathNode.id)
+}
+inline void PathNode::set_id(const char* value,
+    size_t size) {
+  
+  id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(
+      reinterpret_cast<const char*>(value), size), GetArena());
+  // @@protoc_insertion_point(field_set_pointer:NavMessage.PathNode.id)
+}
+inline std::string* PathNode::_internal_mutable_id() {
+  
+  return id_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline std::string* PathNode::release_id() {
+  // @@protoc_insertion_point(field_release:NavMessage.PathNode.id)
+  return id_.Release(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline void PathNode::set_allocated_id(std::string* id) {
+  if (id != nullptr) {
+    
+  } else {
+    
+  }
+  id_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), id,
+      GetArena());
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.PathNode.id)
+}
+
 // -------------------------------------------------------------------
 
 // Trajectory
@@ -3752,7 +3881,90 @@ inline void NavStatu::set_move_mode(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.NavStatu.move_mode)
 }
 
-// string key = 4;
+// .NavMessage.LidarOdomStatu odom = 4;
+inline bool NavStatu::_internal_has_odom() const {
+  return this != internal_default_instance() && odom_ != nullptr;
+}
+inline bool NavStatu::has_odom() const {
+  return _internal_has_odom();
+}
+inline void NavStatu::clear_odom() {
+  if (GetArena() == nullptr && odom_ != nullptr) {
+    delete odom_;
+  }
+  odom_ = nullptr;
+}
+inline const ::NavMessage::LidarOdomStatu& NavStatu::_internal_odom() const {
+  const ::NavMessage::LidarOdomStatu* p = odom_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::LidarOdomStatu*>(
+      &::NavMessage::_LidarOdomStatu_default_instance_);
+}
+inline const ::NavMessage::LidarOdomStatu& NavStatu::odom() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.odom)
+  return _internal_odom();
+}
+inline void NavStatu::unsafe_arena_set_allocated_odom(
+    ::NavMessage::LidarOdomStatu* odom) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(odom_);
+  }
+  odom_ = odom;
+  if (odom) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.NavStatu.odom)
+}
+inline ::NavMessage::LidarOdomStatu* NavStatu::release_odom() {
+  
+  ::NavMessage::LidarOdomStatu* temp = odom_;
+  odom_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::LidarOdomStatu* NavStatu::unsafe_arena_release_odom() {
+  // @@protoc_insertion_point(field_release:NavMessage.NavStatu.odom)
+  
+  ::NavMessage::LidarOdomStatu* temp = odom_;
+  odom_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::LidarOdomStatu* NavStatu::_internal_mutable_odom() {
+  
+  if (odom_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::LidarOdomStatu>(GetArena());
+    odom_ = p;
+  }
+  return odom_;
+}
+inline ::NavMessage::LidarOdomStatu* NavStatu::mutable_odom() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavStatu.odom)
+  return _internal_mutable_odom();
+}
+inline void NavStatu::set_allocated_odom(::NavMessage::LidarOdomStatu* odom) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete odom_;
+  }
+  if (odom) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(odom);
+    if (message_arena != submessage_arena) {
+      odom = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, odom, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  odom_ = odom;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.odom)
+}
+
+// string key = 5;
 inline void NavStatu::clear_key() {
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
 }
@@ -3980,6 +4192,88 @@ inline void NavStatu::set_allocated_predict_traj(::NavMessage::Trajectory* predi
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.predict_traj)
 }
 
+// bool in_space = 8;
+inline void NavStatu::clear_in_space() {
+  in_space_ = false;
+}
+inline bool NavStatu::_internal_in_space() const {
+  return in_space_;
+}
+inline bool NavStatu::in_space() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.in_space)
+  return _internal_in_space();
+}
+inline void NavStatu::_internal_set_in_space(bool value) {
+  
+  in_space_ = value;
+}
+inline void NavStatu::set_in_space(bool value) {
+  _internal_set_in_space(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.in_space)
+}
+
+// string space_id = 9;
+inline void NavStatu::clear_space_id() {
+  space_id_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline const std::string& NavStatu::space_id() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.space_id)
+  return _internal_space_id();
+}
+inline void NavStatu::set_space_id(const std::string& value) {
+  _internal_set_space_id(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.space_id)
+}
+inline std::string* NavStatu::mutable_space_id() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.NavStatu.space_id)
+  return _internal_mutable_space_id();
+}
+inline const std::string& NavStatu::_internal_space_id() const {
+  return space_id_.Get();
+}
+inline void NavStatu::_internal_set_space_id(const std::string& value) {
+  
+  space_id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena());
+}
+inline void NavStatu::set_space_id(std::string&& value) {
+  
+  space_id_.Set(
+    &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena());
+  // @@protoc_insertion_point(field_set_rvalue:NavMessage.NavStatu.space_id)
+}
+inline void NavStatu::set_space_id(const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  
+  space_id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value),
+              GetArena());
+  // @@protoc_insertion_point(field_set_char:NavMessage.NavStatu.space_id)
+}
+inline void NavStatu::set_space_id(const char* value,
+    size_t size) {
+  
+  space_id_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(
+      reinterpret_cast<const char*>(value), size), GetArena());
+  // @@protoc_insertion_point(field_set_pointer:NavMessage.NavStatu.space_id)
+}
+inline std::string* NavStatu::_internal_mutable_space_id() {
+  
+  return space_id_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline std::string* NavStatu::release_space_id() {
+  // @@protoc_insertion_point(field_release:NavMessage.NavStatu.space_id)
+  return space_id_.Release(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline void NavStatu::set_allocated_space_id(std::string* space_id) {
+  if (space_id != nullptr) {
+    
+  } else {
+    
+  }
+  space_id_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), space_id,
+      GetArena());
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.space_id)
+}
+
 // -------------------------------------------------------------------
 
 // RobotStatu

MPC/navigation.cpp

@@ -179,10 +179,21 @@ void Navigation::pubStatuThreadFuc(void* p) {
 }
 
 void Navigation::publish_statu(NavMessage::NavStatu& statu) {
-
+  if(timedPose_.timeout()==false){
+    NavMessage::LidarOdomStatu odom;
+    odom.set_x(timedPose_.Get().x());
+    odom.set_y(timedPose_.Get().y());
+    odom.set_theta(timedPose_.Get().theta());
+    if(timedV_.timeout()==false)
+      odom.set_v(timedV_.Get());
+    if(timedA_.timeout()==false)
+      odom.set_vth(timedA_.Get());
+    statu.mutable_odom()->CopyFrom(odom);
+  }
+  statu.set_in_space(isInSpace_);
+  if(isInSpace_) statu.set_space_id(space_id_);
     //发布nav状态
     statu.set_statu(actionType_); //
-    //printf(" statu:%d\n",actionType_);
     statu.set_move_mode(move_mode_);
     if (running_)
     {
@@ -361,29 +372,34 @@ void Navigation::Pause()
 
 Navigation::Navigation()
 {
+  isInSpace_=false; //是否在车位或者正在进入车位
   RWheel_position_=eUnknow;
   move_mode_=Monitor_emqx::eSingle;
   thread_= nullptr;
   monitor_= nullptr;
   terminator_= nullptr;
 
-  timedBrotherPose_.reset(Pose2d(-100,0,0),0.1);
+  timedBrotherPose_.reset(Pose2d(-100,0,0),0.5);
 }
 
 void Navigation::BrotherAgvStatuCallback(const MqttMsg& msg,void* context)
 {
   Navigation* navigator=(Navigation*)context;
 
-  NavMessage::LidarOdomStatu brother_statu;
+  NavMessage::NavStatu brother_statu;
   if(msg.toProtoMessage(brother_statu)==false) {
     std::cout<<" msg transform to AGVStatu failed,msg:"<<msg.data()<<std::endl;
     return;
   }
   //std::cout<<brother_nav.DebugString()<<std::endl<<std::endl;
-  Pose2d pose(brother_statu.x(),brother_statu.y(),brother_statu.theta());
-  navigator->timedBrotherPose_.reset(pose,1);
-  navigator->timedBrotherV_.reset(brother_statu.v(),1);
-  navigator->timedBrotherA_.reset(brother_statu.vth(),1);
+  if(brother_statu.has_odom()) {
+    NavMessage::LidarOdomStatu odom = brother_statu.odom();
+    Pose2d pose(odom.x(), odom.y(), odom.theta());
+    navigator->timedBrotherPose_.reset(pose, 1);
+    navigator->timedBrotherV_.reset(odom.v(), 1);
+    navigator->timedBrotherA_.reset(odom.vth(), 1);
+    navigator->timedBrotherNavStatu_.reset(brother_statu,0.1);
+  }
 }
 
 void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
@@ -494,6 +510,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
   if (IsArrived(node)) {
     return true;
   }
+  bool already_in_mpc=false;
   while (cancel_ == false) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     if (PoseTimeout()) {
@@ -514,7 +531,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
     }
     //巡线无法到达目标点
     if(enableTotarget==false) {
-      if(enable_rotate==false) {
+      if(already_in_mpc && enable_rotate==false) {
         printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
         return false;
       }
@@ -526,6 +543,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
       }
       continue;
     }
+    already_in_mpc=true;
     MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
     if (ret == eMpcSuccess) {
       printf(" MPC to target:%f %f l:%f,w:%f theta:%f  completed\n",
@@ -564,6 +582,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
       NavMessage::PathNode node = action.pathnodes(i);
       if(MoveToTarget(node,mpc_velocity,adjust_angular,anyDirect,true)==false)
         return false;
+      else
+        isInSpace_=false;
     }
     return true;
   }
@@ -587,7 +607,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
   }
 
   stLimit limit_v={action.inoutvlimit().min(),action.inoutvlimit().max()};
-  stLimit limit_w={3*M_PI/180.0,10*M_PI/180.0};
+  stLimit limit_w={2*M_PI/180.0,10*M_PI/180.0};
   //入库,起点为streetNode
   if(action.type()==1){
     if(MoveToTarget(action.streetnode(),limit_v,limit_w,true,true)==false)
@@ -595,13 +615,22 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
       printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
       return false;
     }
-    //移动到库位点， 禁止任意方向//不允许旋转
+    //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
+    printf("inout ----------------------------------------\n");
+    //计算当前车是进入车位1点还是2点
+    /*if(timedBrotherNavStatu_.timeout()){
+      printf("Enter to space failed,timedBrotherNavStatu_ timeout\n");
+      return false;
+    }
+    if(timedBrotherNavStatu_.Get().in_space()){
+
+    }*/
     if(MoveToTarget(action.spacenode(),limit_v,limit_w,false,false)==false)
     {
       printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
       return false;
     }
-  }else if(action.type()==1){
+  }else if(action.type()==2){
     Pose2d space(action.spacenode().x(),action.spacenode().y(),0);
     Pose2d diff=Pose2d::relativePose(space,timedPose_.Get());
     if(Pose2d::abs(diff)<Pose2d(0.03,0.1,2*M_PI)){
@@ -952,6 +981,8 @@ void Navigation::navigatting() {
     std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
     NavMessage::NewAction act = newUnfinished_cations_.front();
     if(act.type()==1){  //入库
+      space_id_=act.spacenode().id();
+      isInSpace_=true;
       if(!execute_InOut_space(act)){
         printf(" In space failed\n");
         break;
@@ -961,8 +992,11 @@ void Navigation::navigatting() {
       if(!execute_InOut_space(act)){
         printf(" out space failed\n");
         break;
+      }else{
+        isInSpace_=false;
       }
     }else if (act.type() == 3 || act.type() == 4) { //马路导航
+
       if (!execute_nodes(act))
         break;
     }else if(act.type()==5){

MPC/navigation.h

@@ -115,6 +115,7 @@ public:
      */
     bool execute_InOut_space(NavMessage::NewAction action);
 
+
     virtual bool clamp_close();
     virtual bool clamp_open();
     bool mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY=false);
@@ -150,6 +151,8 @@ public:
     TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
     TimedLockData<double> timedBrotherV_;              //底盘数据
     TimedLockData<double> timedBrotherA_;
+    TimedLockData<NavMessage::NavStatu> timedBrotherNavStatu_;
+
 
     NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
     std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
@@ -159,7 +162,8 @@ public:
     bool cancel_=false;
 
     Monitor_emqx::ActionMode move_mode_=Monitor_emqx::eSingle;
-
+    bool isInSpace_; //是否在车位或者正在进入车位
+    std::string space_id_;
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
 

config/webots/navigation.prototxt

@@ -4,7 +4,7 @@ main_agv:false
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"192.168.0.100"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"
@@ -14,7 +14,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-child-nav"
-       ip:"192.168.0.100"
+        ip:"127.0.0.1"
         port:1883
         pubStatuTopic:"agv_child/agv_statu"
         pubNavStatuTopic:"agv_child/nav_statu"
@@ -25,9 +25,9 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-child"
-	ip:"192.168.0.100"
+	ip:"127.0.0.1"
 	port:1883
-	subBrotherStatuTopic:"lidar_odom/main"
+	subBrotherStatuTopic:"agv_main/nav_statu"
 }
 
 x_mpc_parameter

config/webots/navigation_main.prototxt

@@ -3,7 +3,7 @@ main_agv:true
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"192.168.0.100"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -13,7 +13,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-main-nav"
-	ip:"192.168.0.100"
+	ip:"127.0.0.1"
 	port:1883
 	pubStatuTopic:"agv_main/agv_statu"
 	pubNavStatuTopic:"agv_main/nav_statu"
@@ -24,9 +24,9 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-main"
-	ip:"192.168.0.100"
+	ip:"127.0.0.1"
 	port:1883
-	subBrotherStatuTopic:"lidar_odom/child"
+	subBrotherStatuTopic:"agv_child/nav_statu"
 }
 
 x_mpc_parameter

message.proto

@@ -46,6 +46,7 @@ message PathNode  //导航路径点及精度
   float l=3;    //目标点旋转矩形方程，代表精度范围
   float w=4;
   float theta=5;
+  string id=6;
 }
 
 message Trajectory
@@ -83,10 +84,12 @@ message NavStatu
   int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，1：single，2：双车
-  string key = 4; // 任务唯一码
+  LidarOdomStatu odom=4;
+  string key = 5; // 任务唯一码
   Trajectory selected_traj = 6;
   Trajectory predict_traj = 7;
-
+  bool in_space=8;  //是否在车位/正在进入车位
+  string space_id=9;
 }
 message RobotStatu
 {

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -140,7 +140,7 @@ void Horizontal(double velocity,double wmg,double w,double l,double& R1,double&
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("192.168.0.100",1883,"webots_main","MainPLC/speedcmd"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots_main","MainPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;

projects/controllers/agv_child/AGV_controller.cpp

@@ -139,7 +139,7 @@ void Horizontal(double velocity,double wmg,double w,double l,double& R1,double&
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("192.168.0.100",1883,"webots-child","ChildPLC/speedcmd"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots-child","ChildPLC/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;