增加夹持状态反馈

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<2> scc_info_Action_message_2eproto;
+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_Pose2d_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_SpeedLimit_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Trajectory_message_2eproto;
@@ -55,6 +56,10 @@ class NavStatuDefaultTypeInternal {
  public:
   ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<NavStatu> _instance;
 } _NavStatu_default_instance_;
+class RobotStatuDefaultTypeInternal {
+ public:
+  ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<RobotStatu> _instance;
+} _RobotStatu_default_instance_;
 }  // namespace NavMessage
 static void InitDefaultsscc_info_Action_message_2eproto() {
   GOOGLE_PROTOBUF_VERIFY_VERSION;
@@ -145,6 +150,21 @@ static void InitDefaultsscc_info_Pose2d_message_2eproto() {
 ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Pose2d_message_2eproto =
     {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 0, 0, InitDefaultsscc_info_Pose2d_message_2eproto}, {}};
 
+static void InitDefaultsscc_info_RobotStatu_message_2eproto() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  {
+    void* ptr = &::NavMessage::_RobotStatu_default_instance_;
+    new (ptr) ::NavMessage::RobotStatu();
+    ::PROTOBUF_NAMESPACE_ID::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::NavMessage::RobotStatu::InitAsDefaultInstance();
+}
+
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_RobotStatu_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_RobotStatu_message_2eproto}, {
+      &scc_info_AgvStatu_message_2eproto.base,}};
+
 static void InitDefaultsscc_info_SpeedLimit_message_2eproto() {
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
@@ -188,7 +208,7 @@ static void InitDefaultsscc_info_Trajectory_message_2eproto() {
     {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_Trajectory_message_2eproto}, {
       &scc_info_Pose2d_message_2eproto.base,}};
 
-static ::PROTOBUF_NAMESPACE_ID::Metadata file_level_metadata_message_2eproto[9];
+static ::PROTOBUF_NAMESPACE_ID::Metadata file_level_metadata_message_2eproto[10];
 static constexpr ::PROTOBUF_NAMESPACE_ID::EnumDescriptor const** file_level_enum_descriptors_message_2eproto = nullptr;
 static constexpr ::PROTOBUF_NAMESPACE_ID::ServiceDescriptor const** file_level_service_descriptors_message_2eproto = nullptr;
 
@@ -278,6 +298,15 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, unfinished_actions_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, predict_traj_),
+  ~0u,  // no _has_bits_
+  PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, x_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, y_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, theta_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::RobotStatu, agvstatu_),
 };
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::NavMessage::LidarOdomStatu)},
@@ -289,6 +318,7 @@ static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOB
   { 52, -1, sizeof(::NavMessage::Action)},
   { 64, -1, sizeof(::NavMessage::NavCmd)},
   { 73, -1, sizeof(::NavMessage::NavStatu)},
+  { 85, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -301,6 +331,7 @@ static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] =
   reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::NavMessage::_Action_default_instance_),
   reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::NavMessage::_NavCmd_default_instance_),
   reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::NavMessage::_NavStatu_default_instance_),
+  reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::NavMessage::_RobotStatu_default_instance_),
 };
 
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
@@ -328,28 +359,31 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
   "\005\022\013\n\003key\030\004 \001(\t\022.\n\022unfinished_actions\030\005 \003"
   "(\0132\022.NavMessage.Action\022-\n\rselected_traj\030"
   "\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014predict"
-  "_traj\030\007 \001(\0132\026.NavMessage.Trajectoryb\006pro"
-  "to3"
+  "_traj\030\007 \001(\0132\026.NavMessage.Trajectory\"Y\n\nR"
+  "obotStatu\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.Ag"
+  "vStatub\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
-static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_message_2eproto_sccs[9] = {
+static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_message_2eproto_sccs[10] = {
   &scc_info_Action_message_2eproto.base,
   &scc_info_AgvStatu_message_2eproto.base,
   &scc_info_LidarOdomStatu_message_2eproto.base,
   &scc_info_NavCmd_message_2eproto.base,
   &scc_info_NavStatu_message_2eproto.base,
   &scc_info_Pose2d_message_2eproto.base,
+  &scc_info_RobotStatu_message_2eproto.base,
   &scc_info_SpeedLimit_message_2eproto.base,
   &scc_info_ToAgvCmd_message_2eproto.base,
   &scc_info_Trajectory_message_2eproto.base,
 };
 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", 1003,
-  &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 9, 0,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 1094,
+  &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 10, 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,
+  file_level_metadata_message_2eproto, 10, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
 };
 
 // Force running AddDescriptors() at dynamic initialization time.
@@ -3099,6 +3133,297 @@ void NavStatu::InternalSwap(NavStatu* other) {
 }
 
 
+// ===================================================================
+
+void RobotStatu::InitAsDefaultInstance() {
+  ::NavMessage::_RobotStatu_default_instance_._instance.get_mutable()->agvstatu_ = const_cast< ::NavMessage::AgvStatu*>(
+      ::NavMessage::AgvStatu::internal_default_instance());
+}
+class RobotStatu::_Internal {
+ public:
+  static const ::NavMessage::AgvStatu& agvstatu(const RobotStatu* msg);
+};
+
+const ::NavMessage::AgvStatu&
+RobotStatu::_Internal::agvstatu(const RobotStatu* msg) {
+  return *msg->agvstatu_;
+}
+RobotStatu::RobotStatu(::PROTOBUF_NAMESPACE_ID::Arena* arena)
+  : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
+  SharedCtor();
+  RegisterArenaDtor(arena);
+  // @@protoc_insertion_point(arena_constructor:NavMessage.RobotStatu)
+}
+RobotStatu::RobotStatu(const RobotStatu& from)
+  : ::PROTOBUF_NAMESPACE_ID::Message() {
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  if (from._internal_has_agvstatu()) {
+    agvstatu_ = new ::NavMessage::AgvStatu(*from.agvstatu_);
+  } else {
+    agvstatu_ = nullptr;
+  }
+  ::memcpy(&x_, &from.x_,
+    static_cast<size_t>(reinterpret_cast<char*>(&theta_) -
+    reinterpret_cast<char*>(&x_)) + sizeof(theta_));
+  // @@protoc_insertion_point(copy_constructor:NavMessage.RobotStatu)
+}
+
+void RobotStatu::SharedCtor() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_RobotStatu_message_2eproto.base);
+  ::memset(&agvstatu_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&theta_) -
+      reinterpret_cast<char*>(&agvstatu_)) + sizeof(theta_));
+}
+
+RobotStatu::~RobotStatu() {
+  // @@protoc_insertion_point(destructor:NavMessage.RobotStatu)
+  SharedDtor();
+  _internal_metadata_.Delete<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+void RobotStatu::SharedDtor() {
+  GOOGLE_DCHECK(GetArena() == nullptr);
+  if (this != internal_default_instance()) delete agvstatu_;
+}
+
+void RobotStatu::ArenaDtor(void* object) {
+  RobotStatu* _this = reinterpret_cast< RobotStatu* >(object);
+  (void)_this;
+}
+void RobotStatu::RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena*) {
+}
+void RobotStatu::SetCachedSize(int size) const {
+  _cached_size_.Set(size);
+}
+const RobotStatu& RobotStatu::default_instance() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&::scc_info_RobotStatu_message_2eproto.base);
+  return *internal_default_instance();
+}
+
+
+void RobotStatu::Clear() {
+// @@protoc_insertion_point(message_clear_start:NavMessage.RobotStatu)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  if (GetArena() == nullptr && agvstatu_ != nullptr) {
+    delete agvstatu_;
+  }
+  agvstatu_ = nullptr;
+  ::memset(&x_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&theta_) -
+      reinterpret_cast<char*>(&x_)) + sizeof(theta_));
+  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+const char* RobotStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) {
+#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  ::PROTOBUF_NAMESPACE_ID::Arena* arena = GetArena(); (void)arena;
+  while (!ctx->Done(&ptr)) {
+    ::PROTOBUF_NAMESPACE_ID::uint32 tag;
+    ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
+    CHK_(ptr);
+    switch (tag >> 3) {
+      // float x = 1;
+      case 1:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 13)) {
+          x_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // float y = 2;
+      case 2:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 21)) {
+          y_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // float theta = 3;
+      case 3:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
+          theta_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // .NavMessage.AgvStatu agvStatu = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
+          ptr = ctx->ParseMessage(_internal_mutable_agvstatu(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      default: {
+      handle_unusual:
+        if ((tag & 7) == 4 || tag == 0) {
+          ctx->SetLastTag(tag);
+          goto success;
+        }
+        ptr = UnknownFieldParse(tag,
+            _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
+            ptr, ctx);
+        CHK_(ptr != nullptr);
+        continue;
+      }
+    }  // switch
+  }  // while
+success:
+  return ptr;
+failure:
+  ptr = nullptr;
+  goto success;
+#undef CHK_
+}
+
+::PROTOBUF_NAMESPACE_ID::uint8* RobotStatu::_InternalSerialize(
+    ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
+  // @@protoc_insertion_point(serialize_to_array_start:NavMessage.RobotStatu)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  // float x = 1;
+  if (!(this->x() <= 0 && this->x() >= 0)) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(1, this->_internal_x(), target);
+  }
+
+  // float y = 2;
+  if (!(this->y() <= 0 && this->y() >= 0)) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(2, this->_internal_y(), target);
+  }
+
+  // float theta = 3;
+  if (!(this->theta() <= 0 && this->theta() >= 0)) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_theta(), target);
+  }
+
+  // .NavMessage.AgvStatu agvStatu = 4;
+  if (this->has_agvstatu()) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        4, _Internal::agvstatu(this), target, stream);
+  }
+
+  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);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:NavMessage.RobotStatu)
+  return target;
+}
+
+size_t RobotStatu::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:NavMessage.RobotStatu)
+  size_t total_size = 0;
+
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  // .NavMessage.AgvStatu agvStatu = 4;
+  if (this->has_agvstatu()) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *agvstatu_);
+  }
+
+  // float x = 1;
+  if (!(this->x() <= 0 && this->x() >= 0)) {
+    total_size += 1 + 4;
+  }
+
+  // float y = 2;
+  if (!(this->y() <= 0 && this->y() >= 0)) {
+    total_size += 1 + 4;
+  }
+
+  // float theta = 3;
+  if (!(this->theta() <= 0 && this->theta() >= 0)) {
+    total_size += 1 + 4;
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
+        _internal_metadata_, total_size, &_cached_size_);
+  }
+  int cached_size = ::PROTOBUF_NAMESPACE_ID::internal::ToCachedSize(total_size);
+  SetCachedSize(cached_size);
+  return total_size;
+}
+
+void RobotStatu::MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:NavMessage.RobotStatu)
+  GOOGLE_DCHECK_NE(&from, this);
+  const RobotStatu* source =
+      ::PROTOBUF_NAMESPACE_ID::DynamicCastToGenerated<RobotStatu>(
+          &from);
+  if (source == nullptr) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:NavMessage.RobotStatu)
+    ::PROTOBUF_NAMESPACE_ID::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:NavMessage.RobotStatu)
+    MergeFrom(*source);
+  }
+}
+
+void RobotStatu::MergeFrom(const RobotStatu& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:NavMessage.RobotStatu)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  if (from.has_agvstatu()) {
+    _internal_mutable_agvstatu()->::NavMessage::AgvStatu::MergeFrom(from._internal_agvstatu());
+  }
+  if (!(from.x() <= 0 && from.x() >= 0)) {
+    _internal_set_x(from._internal_x());
+  }
+  if (!(from.y() <= 0 && from.y() >= 0)) {
+    _internal_set_y(from._internal_y());
+  }
+  if (!(from.theta() <= 0 && from.theta() >= 0)) {
+    _internal_set_theta(from._internal_theta());
+  }
+}
+
+void RobotStatu::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:NavMessage.RobotStatu)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void RobotStatu::CopyFrom(const RobotStatu& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:NavMessage.RobotStatu)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool RobotStatu::IsInitialized() const {
+  return true;
+}
+
+void RobotStatu::InternalSwap(RobotStatu* other) {
+  using std::swap;
+  _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(RobotStatu, theta_)
+      + sizeof(RobotStatu::theta_)
+      - PROTOBUF_FIELD_OFFSET(RobotStatu, agvstatu_)>(
+          reinterpret_cast<char*>(&agvstatu_),
+          reinterpret_cast<char*>(&other->agvstatu_));
+}
+
+::PROTOBUF_NAMESPACE_ID::Metadata RobotStatu::GetMetadata() const {
+  return GetMetadataStatic();
+}
+
+
 // @@protoc_insertion_point(namespace_scope)
 }  // namespace NavMessage
 PROTOBUF_NAMESPACE_OPEN
@@ -3129,6 +3454,9 @@ template<> PROTOBUF_NOINLINE ::NavMessage::NavCmd* Arena::CreateMaybeMessage< ::
 template<> PROTOBUF_NOINLINE ::NavMessage::NavStatu* Arena::CreateMaybeMessage< ::NavMessage::NavStatu >(Arena* arena) {
   return Arena::CreateMessageInternal< ::NavMessage::NavStatu >(arena);
 }
+template<> PROTOBUF_NOINLINE ::NavMessage::RobotStatu* Arena::CreateMaybeMessage< ::NavMessage::RobotStatu >(Arena* arena) {
+  return Arena::CreateMessageInternal< ::NavMessage::RobotStatu >(arena);
+}
 PROTOBUF_NAMESPACE_CLOSE
 
 // @@protoc_insertion_point(global_scope)

MPC/monitor/emqx/message.pb.h

@@ -47,7 +47,7 @@ struct TableStruct_message_2eproto {
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[9]
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[10]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
   static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
@@ -73,6 +73,9 @@ extern NavStatuDefaultTypeInternal _NavStatu_default_instance_;
 class Pose2d;
 class Pose2dDefaultTypeInternal;
 extern Pose2dDefaultTypeInternal _Pose2d_default_instance_;
+class RobotStatu;
+class RobotStatuDefaultTypeInternal;
+extern RobotStatuDefaultTypeInternal _RobotStatu_default_instance_;
 class SpeedLimit;
 class SpeedLimitDefaultTypeInternal;
 extern SpeedLimitDefaultTypeInternal _SpeedLimit_default_instance_;
@@ -90,6 +93,7 @@ template<> ::NavMessage::LidarOdomStatu* Arena::CreateMaybeMessage<::NavMessage:
 template<> ::NavMessage::NavCmd* Arena::CreateMaybeMessage<::NavMessage::NavCmd>(Arena*);
 template<> ::NavMessage::NavStatu* Arena::CreateMaybeMessage<::NavMessage::NavStatu>(Arena*);
 template<> ::NavMessage::Pose2d* Arena::CreateMaybeMessage<::NavMessage::Pose2d>(Arena*);
+template<> ::NavMessage::RobotStatu* Arena::CreateMaybeMessage<::NavMessage::RobotStatu>(Arena*);
 template<> ::NavMessage::SpeedLimit* Arena::CreateMaybeMessage<::NavMessage::SpeedLimit>(Arena*);
 template<> ::NavMessage::ToAgvCmd* Arena::CreateMaybeMessage<::NavMessage::ToAgvCmd>(Arena*);
 template<> ::NavMessage::Trajectory* Arena::CreateMaybeMessage<::NavMessage::Trajectory>(Arena*);
@@ -1783,6 +1787,185 @@ class NavStatu PROTOBUF_FINAL :
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
+// -------------------------------------------------------------------
+
+class RobotStatu PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.RobotStatu) */ {
+ public:
+  inline RobotStatu() : RobotStatu(nullptr) {}
+  virtual ~RobotStatu();
+
+  RobotStatu(const RobotStatu& from);
+  RobotStatu(RobotStatu&& from) noexcept
+    : RobotStatu() {
+    *this = ::std::move(from);
+  }
+
+  inline RobotStatu& operator=(const RobotStatu& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline RobotStatu& operator=(RobotStatu&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const RobotStatu& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const RobotStatu* internal_default_instance() {
+    return reinterpret_cast<const RobotStatu*>(
+               &_RobotStatu_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    9;
+
+  friend void swap(RobotStatu& a, RobotStatu& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(RobotStatu* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(RobotStatu* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline RobotStatu* New() const final {
+    return CreateMaybeMessage<RobotStatu>(nullptr);
+  }
+
+  RobotStatu* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<RobotStatu>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const RobotStatu& from);
+  void MergeFrom(const RobotStatu& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(RobotStatu* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "NavMessage.RobotStatu";
+  }
+  protected:
+  explicit RobotStatu(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_message_2eproto);
+    return ::descriptor_table_message_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kAgvStatuFieldNumber = 4,
+    kXFieldNumber = 1,
+    kYFieldNumber = 2,
+    kThetaFieldNumber = 3,
+  };
+  // .NavMessage.AgvStatu agvStatu = 4;
+  bool has_agvstatu() const;
+  private:
+  bool _internal_has_agvstatu() const;
+  public:
+  void clear_agvstatu();
+  const ::NavMessage::AgvStatu& agvstatu() const;
+  ::NavMessage::AgvStatu* release_agvstatu();
+  ::NavMessage::AgvStatu* mutable_agvstatu();
+  void set_allocated_agvstatu(::NavMessage::AgvStatu* agvstatu);
+  private:
+  const ::NavMessage::AgvStatu& _internal_agvstatu() const;
+  ::NavMessage::AgvStatu* _internal_mutable_agvstatu();
+  public:
+  void unsafe_arena_set_allocated_agvstatu(
+      ::NavMessage::AgvStatu* agvstatu);
+  ::NavMessage::AgvStatu* unsafe_arena_release_agvstatu();
+
+  // float x = 1;
+  void clear_x();
+  float x() const;
+  void set_x(float value);
+  private:
+  float _internal_x() const;
+  void _internal_set_x(float value);
+  public:
+
+  // float y = 2;
+  void clear_y();
+  float y() const;
+  void set_y(float value);
+  private:
+  float _internal_y() const;
+  void _internal_set_y(float value);
+  public:
+
+  // float theta = 3;
+  void clear_theta();
+  float theta() const;
+  void set_theta(float value);
+  private:
+  float _internal_theta() const;
+  void _internal_set_theta(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:NavMessage.RobotStatu)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  ::NavMessage::AgvStatu* agvstatu_;
+  float x_;
+  float y_;
+  float theta_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  friend struct ::TableStruct_message_2eproto;
+};
 // ===================================================================
 
 
@@ -3271,6 +3454,153 @@ inline void NavStatu::set_allocated_predict_traj(::NavMessage::Trajectory* predi
   // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.predict_traj)
 }
 
+// -------------------------------------------------------------------
+
+// RobotStatu
+
+// float x = 1;
+inline void RobotStatu::clear_x() {
+  x_ = 0;
+}
+inline float RobotStatu::_internal_x() const {
+  return x_;
+}
+inline float RobotStatu::x() const {
+  // @@protoc_insertion_point(field_get:NavMessage.RobotStatu.x)
+  return _internal_x();
+}
+inline void RobotStatu::_internal_set_x(float value) {
+  
+  x_ = value;
+}
+inline void RobotStatu::set_x(float value) {
+  _internal_set_x(value);
+  // @@protoc_insertion_point(field_set:NavMessage.RobotStatu.x)
+}
+
+// float y = 2;
+inline void RobotStatu::clear_y() {
+  y_ = 0;
+}
+inline float RobotStatu::_internal_y() const {
+  return y_;
+}
+inline float RobotStatu::y() const {
+  // @@protoc_insertion_point(field_get:NavMessage.RobotStatu.y)
+  return _internal_y();
+}
+inline void RobotStatu::_internal_set_y(float value) {
+  
+  y_ = value;
+}
+inline void RobotStatu::set_y(float value) {
+  _internal_set_y(value);
+  // @@protoc_insertion_point(field_set:NavMessage.RobotStatu.y)
+}
+
+// float theta = 3;
+inline void RobotStatu::clear_theta() {
+  theta_ = 0;
+}
+inline float RobotStatu::_internal_theta() const {
+  return theta_;
+}
+inline float RobotStatu::theta() const {
+  // @@protoc_insertion_point(field_get:NavMessage.RobotStatu.theta)
+  return _internal_theta();
+}
+inline void RobotStatu::_internal_set_theta(float value) {
+  
+  theta_ = value;
+}
+inline void RobotStatu::set_theta(float value) {
+  _internal_set_theta(value);
+  // @@protoc_insertion_point(field_set:NavMessage.RobotStatu.theta)
+}
+
+// .NavMessage.AgvStatu agvStatu = 4;
+inline bool RobotStatu::_internal_has_agvstatu() const {
+  return this != internal_default_instance() && agvstatu_ != nullptr;
+}
+inline bool RobotStatu::has_agvstatu() const {
+  return _internal_has_agvstatu();
+}
+inline void RobotStatu::clear_agvstatu() {
+  if (GetArena() == nullptr && agvstatu_ != nullptr) {
+    delete agvstatu_;
+  }
+  agvstatu_ = nullptr;
+}
+inline const ::NavMessage::AgvStatu& RobotStatu::_internal_agvstatu() const {
+  const ::NavMessage::AgvStatu* p = agvstatu_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::AgvStatu*>(
+      &::NavMessage::_AgvStatu_default_instance_);
+}
+inline const ::NavMessage::AgvStatu& RobotStatu::agvstatu() const {
+  // @@protoc_insertion_point(field_get:NavMessage.RobotStatu.agvStatu)
+  return _internal_agvstatu();
+}
+inline void RobotStatu::unsafe_arena_set_allocated_agvstatu(
+    ::NavMessage::AgvStatu* agvstatu) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(agvstatu_);
+  }
+  agvstatu_ = agvstatu;
+  if (agvstatu) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.RobotStatu.agvStatu)
+}
+inline ::NavMessage::AgvStatu* RobotStatu::release_agvstatu() {
+  
+  ::NavMessage::AgvStatu* temp = agvstatu_;
+  agvstatu_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::AgvStatu* RobotStatu::unsafe_arena_release_agvstatu() {
+  // @@protoc_insertion_point(field_release:NavMessage.RobotStatu.agvStatu)
+  
+  ::NavMessage::AgvStatu* temp = agvstatu_;
+  agvstatu_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::AgvStatu* RobotStatu::_internal_mutable_agvstatu() {
+  
+  if (agvstatu_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::AgvStatu>(GetArena());
+    agvstatu_ = p;
+  }
+  return agvstatu_;
+}
+inline ::NavMessage::AgvStatu* RobotStatu::mutable_agvstatu() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.RobotStatu.agvStatu)
+  return _internal_mutable_agvstatu();
+}
+inline void RobotStatu::set_allocated_agvstatu(::NavMessage::AgvStatu* agvstatu) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete agvstatu_;
+  }
+  if (agvstatu) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(agvstatu);
+    if (message_arena != submessage_arena) {
+      agvstatu = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, agvstatu, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  agvstatu_ = agvstatu;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.RobotStatu.agvStatu)
+}
+
 #ifdef __GNUC__
   #pragma GCC diagnostic pop
 #endif  // __GNUC__
@@ -3290,6 +3620,8 @@ inline void NavStatu::set_allocated_predict_traj(::NavMessage::Trajectory* predi
 
 // -------------------------------------------------------------------
 
+// -------------------------------------------------------------------
+
 
 // @@protoc_insertion_point(namespace_scope)
 

MPC/navigation.cpp

@@ -187,22 +187,32 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
     if(terminator_)
       terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);
 
-    //发布位姿  --------------------------------
-    if (timedPose_.timeout() == false &&
-        (timedV_.timeout() == false && timedA_.timeout() == false)) {
-      NavMessage::LidarOdomStatu agvStatu;
-      Pose2d pose = timedPose_.Get();
-      agvStatu.set_x(pose.x());
-      agvStatu.set_y(pose.y());
-      agvStatu.set_theta(pose.theta());
-      agvStatu.set_v(timedV_.Get());
-      agvStatu.set_vth(timedA_.Get());
+    //发布位姿  ------robot状态--------------------------
+    NavMessage::RobotStatu robot=CreateRobotStatuMsg();
       if(terminator_) {
         MqttMsg msg;
-        msg.fromProtoMessage(agvStatu);
+        msg.fromProtoMessage(robot);
         terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
       }
-    }
+}
+
+NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
+{
+  NavMessage::RobotStatu robotStatu;
+  if (timedPose_.timeout() == false &&
+      (timedV_.timeout() == false && timedA_.timeout() == false)) {
+
+    Pose2d pose = timedPose_.Get();
+    robotStatu.set_x(pose.x());
+    robotStatu.set_y(pose.y());
+    robotStatu.set_theta(pose.theta());
+    NavMessage::AgvStatu plc;
+    plc.set_v(timedV_.Get());
+    plc.set_w(timedA_.Get());
+    plc.set_clamp(timed_clamp_.Get());
+    robotStatu.mutable_agvstatu()->CopyFrom(plc);
+  }
+  return robotStatu;
 }
 
 void Navigation::ResetStatu(double v,double a)

MPC/navigation.h

@@ -76,7 +76,7 @@ public:
     bool mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out);
     void navigatting();
 
-
+    virtual NavMessage::RobotStatu CreateRobotStatuMsg();
 
     /*
      * 发布导航模块状态

MPC/navigation_main.cpp

@@ -75,6 +75,13 @@ void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::Act
 
 }
 
+NavMessage::RobotStatu NavigationMain::CreateRobotStatuMsg()
+{
+  NavMessage::RobotStatu agvStatu=Navigation::CreateRobotStatuMsg();
+  agvStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
+  //std::cout<<agvStatu.DebugString()<<std::endl;
+  return agvStatu;
+}
 
 void NavigationMain::ResetOtherClamp(ClampStatu statu)
 {

MPC/navigation_main.h

@@ -33,6 +33,7 @@ protected:
     virtual void HandleNavCmd(const NavMessage::NavCmd& cmd);
     virtual bool clamp_close();
     virtual bool clamp_open();
+    virtual NavMessage::RobotStatu CreateRobotStatuMsg();
 protected:
     float  wheelBase_;   //两节agv的位姿与单节的位姿变换
     TimedLockData<int> timed_other_clamp_;

message.proto

@@ -75,5 +75,12 @@ message NavStatu
   Trajectory predict_traj = 7;
 
 }
+message RobotStatu
+{
+  float x=1;
+  float y=2;
+  float theta=3;
+  AgvStatu agvStatu=4;
+}