修改成订阅对象节点，Speed消息格式修改

MPC/monitor/emqx/message.pb.cc

@@ -14,7 +14,6 @@
 #include <google/protobuf/wire_format.h>
 // @@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<1> scc_info_Action_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<1> scc_info_Trajectory_message_2eproto;
@@ -103,10 +102,9 @@ static void InitDefaultsscc_info_NavStatu_message_2eproto() {
   ::NavMessage::NavStatu::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<3> scc_info_NavStatu_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 3, 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_Action_message_2eproto.base,
-      &scc_info_AGVStatu_message_2eproto.base,
       &scc_info_Trajectory_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_Pose2d_message_2eproto() {
@@ -172,9 +170,10 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, type_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, vth_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -213,20 +212,17 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, statu_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, unfinished_actions_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, current_pose_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, pose_timeout_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, twist_timeout_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, predict_traj_),
 };
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::Speed)},
-  { 18, -1, sizeof(::NavMessage::Pose2d)},
-  { 26, -1, sizeof(::NavMessage::Trajectory)},
-  { 32, -1, sizeof(::NavMessage::Action)},
-  { 40, -1, sizeof(::NavMessage::NavCmd)},
-  { 48, -1, sizeof(::NavMessage::NavStatu)},
+  { 19, -1, sizeof(::NavMessage::Pose2d)},
+  { 27, -1, sizeof(::NavMessage::Trajectory)},
+  { 33, -1, sizeof(::NavMessage::Action)},
+  { 41, -1, sizeof(::NavMessage::NavCmd)},
+  { 49, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -242,22 +238,20 @@ static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] =
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
   "\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\005Speed\022\014\n\004type\030\001 "
-  "\001(\005\022\t\n\001v\030\002 \001(\002\022\013\n\003vth\030\003 \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\nTr"
-  "ajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pos"
-  "e2d\"c\n\006Action\022\014\n\004type\030\001 \001(\005\022\"\n\006target\030\002 "
-  "\001(\0132\022.NavMessage.Pose2d\022\'\n\013target_diff\030\003"
-  " \001(\0132\022.NavMessage.Pose2d\"J\n\006NavCmd\022\016\n\006ac"
-  "tion\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\013"
-  "2\022.NavMessage.Action\"\214\002\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\014current"
-  "_pose\030\004 \001(\0132\024.NavMessage.AGVStatu\022\024\n\014pos"
-  "e_timeout\030\005 \001(\010\022\025\n\rtwist_timeout\030\006 \001(\010\022-"
-  "\n\rselected_traj\030\007 \001(\0132\026.NavMessage.Traje"
-  "ctory\022,\n\014predict_traj\030\010 \001(\0132\026.NavMessage"
-  ".Trajectoryb\006proto3"
+  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"3\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\"-\n\006Pos"
+  "e2d\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\005poses\030\001 \003(\0132\022.NavMessa"
+  "ge.Pose2d\"c\n\006Action\022\014\n\004type\030\001 \001(\005\022\"\n\006tar"
+  "get\030\002 \001(\0132\022.NavMessage.Pose2d\022\'\n\013target_"
+  "diff\030\003 \001(\0132\022.NavMessage.Pose2d\"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.NavMessage.Action\"\263\001\n\010NavStatu\022"
+  "\r\n\005statu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinishe"
+  "d_actions\030\003 \003(\0132\022.NavMessage.Action\022-\n\rs"
+  "elected_traj\030\004 \001(\0132\026.NavMessage.Trajecto"
+  "ry\022,\n\014predict_traj\030\005 \001(\0132\026.NavMessage.Tr"
+  "ajectoryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -272,7 +266,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", 699,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 616,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 7, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 7, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -589,16 +583,16 @@ Speed::Speed(::PROTOBUF_NAMESPACE_ID::Arena* arena)
 Speed::Speed(const Speed& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
-  ::memcpy(&type_, &from.type_,
-    static_cast<size_t>(reinterpret_cast<char*>(&vth_) -
-    reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memcpy(&h_, &from.h_,
+    static_cast<size_t>(reinterpret_cast<char*>(&w_) -
+    reinterpret_cast<char*>(&h_)) + sizeof(w_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.Speed)
 }
 
 void Speed::SharedCtor() {
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&w_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(w_));
 }
 
 Speed::~Speed() {
@@ -632,9 +626,9 @@ void Speed::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&w_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(w_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -646,24 +640,31 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // int32 type = 1;
+      // int32 H = 1;
       case 1:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
-          type_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          h_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float v = 2;
+      // int32 T = 2;
       case 2:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 21)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float vth = 3;
+      // float V = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          vth_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // float W = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
@@ -695,22 +696,28 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_type(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
   }
 
-  // float v = 2;
+  // int32 T = 2;
+  if (this->t() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
+  }
+
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(2, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_vth(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -729,20 +736,27 @@ size_t Speed::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-        this->_internal_type());
+        this->_internal_h());
   }
 
-  // float v = 2;
+  // int32 T = 2;
+  if (this->t() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_t());
+  }
+
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
@@ -777,14 +791,17 @@ void Speed::MergeFrom(const Speed& from) {
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (from.type() != 0) {
-    _internal_set_type(from._internal_type());
+  if (from.h() != 0) {
+    _internal_set_h(from._internal_h());
+  }
+  if (from.t() != 0) {
+    _internal_set_t(from._internal_t());
   }
   if (!(from.v() <= 0 && from.v() >= 0)) {
     _internal_set_v(from._internal_v());
   }
-  if (!(from.vth() <= 0 && from.vth() >= 0)) {
-    _internal_set_vth(from._internal_vth());
+  if (!(from.w() <= 0 && from.w() >= 0)) {
+    _internal_set_w(from._internal_w());
   }
 }
 
@@ -810,11 +827,11 @@ void Speed::InternalSwap(Speed* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(Speed, vth_)
-      + sizeof(Speed::vth_)
-      - PROTOBUF_FIELD_OFFSET(Speed, type_)>(
-          reinterpret_cast<char*>(&type_),
-          reinterpret_cast<char*>(&other->type_));
+      PROTOBUF_FIELD_OFFSET(Speed, w_)
+      + sizeof(Speed::w_)
+      - PROTOBUF_FIELD_OFFSET(Speed, h_)>(
+          reinterpret_cast<char*>(&h_),
+          reinterpret_cast<char*>(&other->h_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata Speed::GetMetadata() const {
@@ -1833,8 +1850,6 @@ void NavCmd::InternalSwap(NavCmd* other) {
 // ===================================================================
 
 void NavStatu::InitAsDefaultInstance() {
-  ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->current_pose_ = const_cast< ::NavMessage::AGVStatu*>(
-      ::NavMessage::AGVStatu::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*>(
@@ -1842,15 +1857,10 @@ void NavStatu::InitAsDefaultInstance() {
 }
 class NavStatu::_Internal {
  public:
-  static const ::NavMessage::AGVStatu& current_pose(const NavStatu* msg);
   static const ::NavMessage::Trajectory& selected_traj(const NavStatu* msg);
   static const ::NavMessage::Trajectory& predict_traj(const NavStatu* msg);
 };
 
-const ::NavMessage::AGVStatu&
-NavStatu::_Internal::current_pose(const NavStatu* msg) {
-  return *msg->current_pose_;
-}
 const ::NavMessage::Trajectory&
 NavStatu::_Internal::selected_traj(const NavStatu* msg) {
   return *msg->selected_traj_;
@@ -1875,11 +1885,6 @@ NavStatu::NavStatu(const NavStatu& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
-  if (from._internal_has_current_pose()) {
-    current_pose_ = new ::NavMessage::AGVStatu(*from.current_pose_);
-  } else {
-    current_pose_ = nullptr;
-  }
   if (from._internal_has_selected_traj()) {
     selected_traj_ = new ::NavMessage::Trajectory(*from.selected_traj_);
   } else {
@@ -1890,18 +1895,16 @@ NavStatu::NavStatu(const NavStatu& from)
   } else {
     predict_traj_ = nullptr;
   }
-  ::memcpy(&statu_, &from.statu_,
-    static_cast<size_t>(reinterpret_cast<char*>(&twist_timeout_) -
-    reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = from.statu_;
   // @@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(&current_pose_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&current_pose_)) + sizeof(twist_timeout_));
+  ::memset(&selected_traj_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&statu_) -
+      reinterpret_cast<char*>(&selected_traj_)) + sizeof(statu_));
 }
 
 NavStatu::~NavStatu() {
@@ -1913,7 +1916,6 @@ NavStatu::~NavStatu() {
 void NavStatu::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  if (this != internal_default_instance()) delete current_pose_;
   if (this != internal_default_instance()) delete selected_traj_;
   if (this != internal_default_instance()) delete predict_traj_;
 }
@@ -1941,10 +1943,6 @@ void NavStatu::Clear() {
 
   unfinished_actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  if (GetArena() == nullptr && current_pose_ != nullptr) {
-    delete current_pose_;
-  }
-  current_pose_ = nullptr;
   if (GetArena() == nullptr && selected_traj_ != nullptr) {
     delete selected_traj_;
   }
@@ -1953,9 +1951,7 @@ void NavStatu::Clear() {
     delete predict_traj_;
   }
   predict_traj_ = nullptr;
-  ::memset(&statu_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = false;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1995,37 +1991,16 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
         } else goto handle_unusual;
         continue;
-      // .NavMessage.AGVStatu current_pose = 4;
+      // .NavMessage.Trajectory selected_traj = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
-          ptr = ctx->ParseMessage(_internal_mutable_current_pose(), ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool pose_timeout = 5;
-      case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
-          pose_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool twist_timeout = 6;
-      case 6:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
-          twist_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // .NavMessage.Trajectory selected_traj = 7;
-      case 7:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 58)) {
           ptr = ctx->ParseMessage(_internal_mutable_selected_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // .NavMessage.Trajectory predict_traj = 8;
-      case 8:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 66)) {
+      // .NavMessage.Trajectory predict_traj = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
           ptr = ctx->ParseMessage(_internal_mutable_predict_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -2082,40 +2057,20 @@ failure:
       InternalWriteMessage(3, this->_internal_unfinished_actions(i), target, stream);
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(
-        4, _Internal::current_pose(this), target, stream);
-  }
-
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(5, this->_internal_pose_timeout(), target);
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(6, this->_internal_twist_timeout(), target);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        7, _Internal::selected_traj(this), target, stream);
+        4, _Internal::selected_traj(this), target, stream);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        8, _Internal::predict_traj(this), target, stream);
+        5, _Internal::predict_traj(this), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2148,21 +2103,14 @@ size_t NavStatu::ByteSizeLong() const {
         this->_internal_key());
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    total_size += 1 +
-      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
-        *current_pose_);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *selected_traj_);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
@@ -2174,16 +2122,6 @@ size_t NavStatu::ByteSizeLong() const {
     total_size += 1 + 1;
   }
 
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    total_size += 1 + 1;
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 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_);
@@ -2219,9 +2157,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
-  if (from.has_current_pose()) {
-    _internal_mutable_current_pose()->::NavMessage::AGVStatu::MergeFrom(from._internal_current_pose());
-  }
   if (from.has_selected_traj()) {
     _internal_mutable_selected_traj()->::NavMessage::Trajectory::MergeFrom(from._internal_selected_traj());
   }
@@ -2231,12 +2166,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.statu() != 0) {
     _internal_set_statu(from._internal_statu());
   }
-  if (from.pose_timeout() != 0) {
-    _internal_set_pose_timeout(from._internal_pose_timeout());
-  }
-  if (from.twist_timeout() != 0) {
-    _internal_set_twist_timeout(from._internal_twist_timeout());
-  }
 }
 
 void NavStatu::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -2263,11 +2192,11 @@ 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, twist_timeout_)
-      + sizeof(NavStatu::twist_timeout_)
-      - PROTOBUF_FIELD_OFFSET(NavStatu, current_pose_)>(
-          reinterpret_cast<char*>(&current_pose_),
-          reinterpret_cast<char*>(&other->current_pose_));
+      PROTOBUF_FIELD_OFFSET(NavStatu, statu_)
+      + sizeof(NavStatu::statu_)
+      - PROTOBUF_FIELD_OFFSET(NavStatu, selected_traj_)>(
+          reinterpret_cast<char*>(&selected_traj_),
+          reinterpret_cast<char*>(&other->selected_traj_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavStatu::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -384,20 +384,30 @@ class Speed PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kTypeFieldNumber = 1,
-    kVFieldNumber = 2,
-    kVthFieldNumber = 3,
+    kHFieldNumber = 1,
+    kTFieldNumber = 2,
+    kVFieldNumber = 3,
+    kWFieldNumber = 4,
   };
-  // int32 type = 1;
-  void clear_type();
-  ::PROTOBUF_NAMESPACE_ID::int32 type() const;
-  void set_type(::PROTOBUF_NAMESPACE_ID::int32 value);
+  // int32 H = 1;
+  void clear_h();
+  ::PROTOBUF_NAMESPACE_ID::int32 h() const;
+  void set_h(::PROTOBUF_NAMESPACE_ID::int32 value);
   private:
-  ::PROTOBUF_NAMESPACE_ID::int32 _internal_type() const;
-  void _internal_set_type(::PROTOBUF_NAMESPACE_ID::int32 value);
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_h() const;
+  void _internal_set_h(::PROTOBUF_NAMESPACE_ID::int32 value);
   public:
 
-  // float v = 2;
+  // int32 T = 2;
+  void clear_t();
+  ::PROTOBUF_NAMESPACE_ID::int32 t() const;
+  void set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_t() const;
+  void _internal_set_t(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
+  // float V = 3;
   void clear_v();
   float v() const;
   void set_v(float value);
@@ -406,13 +416,13 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_v(float value);
   public:
 
-  // float vth = 3;
-  void clear_vth();
-  float vth() const;
-  void set_vth(float value);
+  // float W = 4;
+  void clear_w();
+  float w() const;
+  void set_w(float value);
   private:
-  float _internal_vth() const;
-  void _internal_set_vth(float value);
+  float _internal_w() const;
+  void _internal_set_w(float value);
   public:
 
   // @@protoc_insertion_point(class_scope:NavMessage.Speed)
@@ -422,9 +432,10 @@ class Speed PROTOBUF_FINAL :
   template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
-  ::PROTOBUF_NAMESPACE_ID::int32 type_;
+  ::PROTOBUF_NAMESPACE_ID::int32 h_;
+  ::PROTOBUF_NAMESPACE_ID::int32 t_;
   float v_;
-  float vth_;
+  float w_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -1202,12 +1213,9 @@ class NavStatu PROTOBUF_FINAL :
   enum : int {
     kUnfinishedActionsFieldNumber = 3,
     kKeyFieldNumber = 2,
-    kCurrentPoseFieldNumber = 4,
-    kSelectedTrajFieldNumber = 7,
-    kPredictTrajFieldNumber = 8,
+    kSelectedTrajFieldNumber = 4,
+    kPredictTrajFieldNumber = 5,
     kStatuFieldNumber = 1,
-    kPoseTimeoutFieldNumber = 5,
-    kTwistTimeoutFieldNumber = 6,
   };
   // repeated .NavMessage.Action unfinished_actions = 3;
   int unfinished_actions_size() const;
@@ -1243,25 +1251,7 @@ class NavStatu PROTOBUF_FINAL :
   std::string* _internal_mutable_key();
   public:
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  bool has_current_pose() const;
-  private:
-  bool _internal_has_current_pose() const;
-  public:
-  void clear_current_pose();
-  const ::NavMessage::AGVStatu& current_pose() const;
-  ::NavMessage::AGVStatu* release_current_pose();
-  ::NavMessage::AGVStatu* mutable_current_pose();
-  void set_allocated_current_pose(::NavMessage::AGVStatu* current_pose);
-  private:
-  const ::NavMessage::AGVStatu& _internal_current_pose() const;
-  ::NavMessage::AGVStatu* _internal_mutable_current_pose();
-  public:
-  void unsafe_arena_set_allocated_current_pose(
-      ::NavMessage::AGVStatu* current_pose);
-  ::NavMessage::AGVStatu* unsafe_arena_release_current_pose();
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   bool has_selected_traj() const;
   private:
   bool _internal_has_selected_traj() const;
@@ -1279,7 +1269,7 @@ class NavStatu PROTOBUF_FINAL :
       ::NavMessage::Trajectory* selected_traj);
   ::NavMessage::Trajectory* unsafe_arena_release_selected_traj();
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   bool has_predict_traj() const;
   private:
   bool _internal_has_predict_traj() const;
@@ -1306,24 +1296,6 @@ class NavStatu PROTOBUF_FINAL :
   void _internal_set_statu(bool value);
   public:
 
-  // bool pose_timeout = 5;
-  void clear_pose_timeout();
-  bool pose_timeout() const;
-  void set_pose_timeout(bool value);
-  private:
-  bool _internal_pose_timeout() const;
-  void _internal_set_pose_timeout(bool value);
-  public:
-
-  // bool twist_timeout = 6;
-  void clear_twist_timeout();
-  bool twist_timeout() const;
-  void set_twist_timeout(bool value);
-  private:
-  bool _internal_twist_timeout() const;
-  void _internal_set_twist_timeout(bool value);
-  public:
-
   // @@protoc_insertion_point(class_scope:NavMessage.NavStatu)
  private:
   class _Internal;
@@ -1333,12 +1305,9 @@ class NavStatu PROTOBUF_FINAL :
   typedef void DestructorSkippable_;
   ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::Action > unfinished_actions_;
   ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
-  ::NavMessage::AGVStatu* current_pose_;
   ::NavMessage::Trajectory* selected_traj_;
   ::NavMessage::Trajectory* predict_traj_;
   bool statu_;
-  bool pose_timeout_;
-  bool twist_timeout_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -1457,27 +1426,47 @@ inline void AGVStatu::set_vth(float value) {
 
 // Speed
 
-// int32 type = 1;
-inline void Speed::clear_type() {
-  type_ = 0;
+// int32 H = 1;
+inline void Speed::clear_h() {
+  h_ = 0;
 }
-inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_type() const {
-  return type_;
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_h() const {
+  return h_;
 }
-inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::type() const {
-  // @@protoc_insertion_point(field_get:NavMessage.Speed.type)
-  return _internal_type();
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::h() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.H)
+  return _internal_h();
 }
-inline void Speed::_internal_set_type(::PROTOBUF_NAMESPACE_ID::int32 value) {
+inline void Speed::_internal_set_h(::PROTOBUF_NAMESPACE_ID::int32 value) {
   
-  type_ = value;
+  h_ = value;
 }
-inline void Speed::set_type(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _internal_set_type(value);
-  // @@protoc_insertion_point(field_set:NavMessage.Speed.type)
+inline void Speed::set_h(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_h(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.H)
+}
+
+// int32 T = 2;
+inline void Speed::clear_t() {
+  t_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_t() const {
+  return t_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::t() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.T)
+  return _internal_t();
+}
+inline void Speed::_internal_set_t(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  t_ = value;
+}
+inline void Speed::set_t(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_t(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.T)
 }
 
-// float v = 2;
+// float V = 3;
 inline void Speed::clear_v() {
   v_ = 0;
 }
@@ -1485,7 +1474,7 @@ inline float Speed::_internal_v() const {
   return v_;
 }
 inline float Speed::v() const {
-  // @@protoc_insertion_point(field_get:NavMessage.Speed.v)
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.V)
   return _internal_v();
 }
 inline void Speed::_internal_set_v(float value) {
@@ -1494,27 +1483,27 @@ inline void Speed::_internal_set_v(float value) {
 }
 inline void Speed::set_v(float value) {
   _internal_set_v(value);
-  // @@protoc_insertion_point(field_set:NavMessage.Speed.v)
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.V)
 }
 
-// float vth = 3;
-inline void Speed::clear_vth() {
-  vth_ = 0;
+// float W = 4;
+inline void Speed::clear_w() {
+  w_ = 0;
 }
-inline float Speed::_internal_vth() const {
-  return vth_;
+inline float Speed::_internal_w() const {
+  return w_;
 }
-inline float Speed::vth() const {
-  // @@protoc_insertion_point(field_get:NavMessage.Speed.vth)
-  return _internal_vth();
+inline float Speed::w() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.W)
+  return _internal_w();
 }
-inline void Speed::_internal_set_vth(float value) {
+inline void Speed::_internal_set_w(float value) {
   
-  vth_ = value;
+  w_ = value;
 }
-inline void Speed::set_vth(float value) {
-  _internal_set_vth(value);
-  // @@protoc_insertion_point(field_set:NavMessage.Speed.vth)
+inline void Speed::set_w(float value) {
+  _internal_set_w(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.W)
 }
 
 // -------------------------------------------------------------------
@@ -2064,130 +2053,7 @@ NavStatu::unfinished_actions() const {
   return unfinished_actions_;
 }
 
-// .NavMessage.AGVStatu current_pose = 4;
-inline bool NavStatu::_internal_has_current_pose() const {
-  return this != internal_default_instance() && current_pose_ != nullptr;
-}
-inline bool NavStatu::has_current_pose() const {
-  return _internal_has_current_pose();
-}
-inline void NavStatu::clear_current_pose() {
-  if (GetArena() == nullptr && current_pose_ != nullptr) {
-    delete current_pose_;
-  }
-  current_pose_ = nullptr;
-}
-inline const ::NavMessage::AGVStatu& NavStatu::_internal_current_pose() const {
-  const ::NavMessage::AGVStatu* p = current_pose_;
-  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::AGVStatu*>(
-      &::NavMessage::_AGVStatu_default_instance_);
-}
-inline const ::NavMessage::AGVStatu& NavStatu::current_pose() const {
-  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.current_pose)
-  return _internal_current_pose();
-}
-inline void NavStatu::unsafe_arena_set_allocated_current_pose(
-    ::NavMessage::AGVStatu* current_pose) {
-  if (GetArena() == nullptr) {
-    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(current_pose_);
-  }
-  current_pose_ = current_pose;
-  if (current_pose) {
-    
-  } else {
-    
-  }
-  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.NavStatu.current_pose)
-}
-inline ::NavMessage::AGVStatu* NavStatu::release_current_pose() {
-  
-  ::NavMessage::AGVStatu* temp = current_pose_;
-  current_pose_ = nullptr;
-  if (GetArena() != nullptr) {
-    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
-  }
-  return temp;
-}
-inline ::NavMessage::AGVStatu* NavStatu::unsafe_arena_release_current_pose() {
-  // @@protoc_insertion_point(field_release:NavMessage.NavStatu.current_pose)
-  
-  ::NavMessage::AGVStatu* temp = current_pose_;
-  current_pose_ = nullptr;
-  return temp;
-}
-inline ::NavMessage::AGVStatu* NavStatu::_internal_mutable_current_pose() {
-  
-  if (current_pose_ == nullptr) {
-    auto* p = CreateMaybeMessage<::NavMessage::AGVStatu>(GetArena());
-    current_pose_ = p;
-  }
-  return current_pose_;
-}
-inline ::NavMessage::AGVStatu* NavStatu::mutable_current_pose() {
-  // @@protoc_insertion_point(field_mutable:NavMessage.NavStatu.current_pose)
-  return _internal_mutable_current_pose();
-}
-inline void NavStatu::set_allocated_current_pose(::NavMessage::AGVStatu* current_pose) {
-  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
-  if (message_arena == nullptr) {
-    delete current_pose_;
-  }
-  if (current_pose) {
-    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
-      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(current_pose);
-    if (message_arena != submessage_arena) {
-      current_pose = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
-          message_arena, current_pose, submessage_arena);
-    }
-    
-  } else {
-    
-  }
-  current_pose_ = current_pose;
-  // @@protoc_insertion_point(field_set_allocated:NavMessage.NavStatu.current_pose)
-}
-
-// bool pose_timeout = 5;
-inline void NavStatu::clear_pose_timeout() {
-  pose_timeout_ = false;
-}
-inline bool NavStatu::_internal_pose_timeout() const {
-  return pose_timeout_;
-}
-inline bool NavStatu::pose_timeout() const {
-  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.pose_timeout)
-  return _internal_pose_timeout();
-}
-inline void NavStatu::_internal_set_pose_timeout(bool value) {
-  
-  pose_timeout_ = value;
-}
-inline void NavStatu::set_pose_timeout(bool value) {
-  _internal_set_pose_timeout(value);
-  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.pose_timeout)
-}
-
-// bool twist_timeout = 6;
-inline void NavStatu::clear_twist_timeout() {
-  twist_timeout_ = false;
-}
-inline bool NavStatu::_internal_twist_timeout() const {
-  return twist_timeout_;
-}
-inline bool NavStatu::twist_timeout() const {
-  // @@protoc_insertion_point(field_get:NavMessage.NavStatu.twist_timeout)
-  return _internal_twist_timeout();
-}
-inline void NavStatu::_internal_set_twist_timeout(bool value) {
-  
-  twist_timeout_ = value;
-}
-inline void NavStatu::set_twist_timeout(bool value) {
-  _internal_set_twist_timeout(value);
-  // @@protoc_insertion_point(field_set:NavMessage.NavStatu.twist_timeout)
-}
-
-// .NavMessage.Trajectory selected_traj = 7;
+// .NavMessage.Trajectory selected_traj = 4;
 inline bool NavStatu::_internal_has_selected_traj() const {
   return this != internal_default_instance() && selected_traj_ != nullptr;
 }
@@ -2270,7 +2136,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 = 8;
+// .NavMessage.Trajectory predict_traj = 5;
 inline bool NavStatu::_internal_has_predict_traj() const {
   return this != internal_default_instance() && predict_traj_ != nullptr;
 }

MPC/monitor/emqx/mqttmsg.cpp

@@ -265,13 +265,11 @@ void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
   data_=(char*)malloc(length_);
   memcpy(data_,json_str.c_str(),length_);
 }
-bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
   if(data_== nullptr)
     return false;
 
-
   util::Status ret;
   char* buf=(char*)malloc(length_+1);
   memset(buf,0,length_+1);

MPC/monitor/emqx/mqttmsg.h

@@ -31,7 +31,7 @@ class MqttMsg
 
 
     void fromProtoMessage(const google::protobuf::Message& messge);
-    bool toProtoMessage(google::protobuf::Message& message);
+    bool toProtoMessage(google::protobuf::Message& message)const;
 
     /*void fromStatu(double x,double y,double theta,double v,double vth);
     void fromNavSpeed(const NavMessage::Speed& speed);

MPC/monitor/monitor_emqx.cpp

@@ -42,9 +42,11 @@ void Monitor_emqx::set_speed(SpeedType type,double v,double a)
 {
   MqttMsg msg;
   NavMessage::Speed speed;
-  speed.set_type(type);
+  heat_=(heat_++)%255;
+  speed.set_h(heat_);
+  speed.set_t(type);
   speed.set_v(v);
-  speed.set_vth(a);
+  speed.set_w(a);
   msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);
@@ -56,9 +58,11 @@ void Monitor_emqx::stop()
 {
   MqttMsg msg;
   NavMessage::Speed speed;
-  speed.set_type(eStop);
+  heat_=(heat_++)%255;
+  speed.set_h(heat_);
+  speed.set_t(eStop);
   speed.set_v(0);
-  speed.set_vth(0);
+  speed.set_w(0);
   msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);
@@ -69,6 +73,7 @@ void Monitor_emqx::stop()
 Monitor_emqx::Monitor_emqx(std::string nodeId,std::string pubTopic,std::string subTopic)
   :client_(nullptr),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic)
 {
+  heat_=0;
   StatuArrivedCallback_= nullptr;
   context_= nullptr;
 }

MPC/monitor/monitor_emqx.h

@@ -42,6 +42,7 @@ public:
     std::string pubTopic_;
     std::string subTopic_;
  public:
+    int heat_=0;
     MqttMsg msg_;
 
 };

MPC/monitor/terminator_emqx.cpp

@@ -0,0 +1,73 @@
+//
+// Created by zx on 23-4-11.
+//
+
+#include "terminator_emqx.h"
+#include "unistd.h"
+Terminator_emqx::~Terminator_emqx()
+{
+  if(client_!= nullptr){
+    client_->disconnect();
+    delete client_;
+    client_= nullptr;
+  }
+}
+
+bool Terminator_emqx::Connect(std::string ip,int port)
+{
+  if(client_!= nullptr)
+  {
+    client_->disconnect();
+    delete client_;
+  }
+  client_=new Paho_client(nodeId_);
+  connected_= client_->connect(ip,port);
+
+  return connected_;
+
+}
+
+void Terminator_emqx::Publish(std::string topic,const MqttMsg& msg)
+{
+  if(client_)
+    client_->publish(topic,1,msg);
+  else
+    printf("publish  failed : emqx client maybe disconnected...\n");
+}
+
+bool Terminator_emqx::AddCallback(std::string topic,Callback callback,void* context)
+{
+  CallBackInfo info;
+  info.func=callback;
+  info.contextx=context;
+  sub_topic_callbacks_[topic]=info;
+  if(connected_)
+  {
+    while(!client_->isconnected()) usleep(1000);
+    client_->subcribe(topic,1,StatuArrivedCallback,this);
+    return true;
+  }
+  else
+  {
+    printf(" emqx has not connected ,subcribe topic:%s  failed ...!!\n",topic.c_str());
+    return false;
+  }
+}
+
+
+Terminator_emqx::Terminator_emqx(std::string nodeId)
+    :client_(nullptr),nodeId_(nodeId),connected_(false)
+{
+}
+
+void Terminator_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
+{
+  Terminator_emqx* terminator=(Terminator_emqx*)context;
+
+  if(terminator->sub_topic_callbacks_.find(topic)!=terminator->sub_topic_callbacks_.end()) {
+    CallBackInfo info = terminator->sub_topic_callbacks_[topic];
+    if (info.func)
+      info.func(msg, info.contextx);
+  }
+
+}

MPC/monitor/terminator_emqx.h

@@ -0,0 +1,38 @@
+//
+// Created by zx on 23-4-11.
+//
+
+#ifndef NAVIGATION_TERMINATOR_EMQX_H
+#define NAVIGATION_TERMINATOR_EMQX_H
+#include <mutex>
+#include "emqx/paho_client.h"
+
+typedef void (*Callback)(const MqttMsg& msg,void* context);
+
+typedef struct{
+    Callback func;
+    void* contextx;
+}CallBackInfo;
+
+class Terminator_emqx {
+public:
+    Terminator_emqx(std::string nodeId);
+    ~Terminator_emqx();
+    bool Connect(std::string ip,int port);
+    bool AddCallback(std::string topic,Callback callback,void* context);
+    void Publish(std::string topic,const MqttMsg& statu);
+
+protected:
+
+    static void StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context);
+
+    std::mutex mtx_;
+
+    Paho_client* client_;
+    bool connected_;
+    std::string nodeId_;
+public:
+    std::map<std::string,CallBackInfo> sub_topic_callbacks_;
+};
+
+#endif //NAVIGATION_TERMINATOR_EMQX_H

MPC/navigation.cpp

@@ -39,7 +39,8 @@ double limit(double x,double min,double max){
 
 bool Navigation::Init(const Navigation_parameter& parameter)
 {
-  Emqx_parameter agv_p=parameter.agv_emqx();
+  parameter_=parameter;
+  AgvEmqx_parameter agv_p=parameter.agv_emqx();
   if(monitor_== nullptr) {
     monitor_ = new Monitor_emqx(agv_p.nodeid(), agv_p.pubtopic(), agv_p.subtopic());
     monitor_->set_statu_arrived_callback(Navigation::RobotStatuCallback, this);
@@ -52,13 +53,17 @@ bool Navigation::Init(const Navigation_parameter& parameter)
 
   Emqx_parameter terminal_p=parameter.terminal_emqx();
   if(terminator_== nullptr) {
-    terminator_ = new Terminator_emqx(terminal_p.nodeid(), terminal_p.pubtopic(), terminal_p.subtopic());
-    terminator_->set_cmd_arrived(Navigation::NavCmdCallback,this);
+    terminator_ = new Terminator_emqx(terminal_p.nodeid());
+
     if(terminator_->Connect(terminal_p.ip(),terminal_p.port())==false)
     {
       printf(" terminator emqx connected failed\n");
       return false;
     }
+
+    terminator_->AddCallback(terminal_p.subnavcmdtopic(),Navigation::NavCmdCallback,this);
+    terminator_->AddCallback(terminal_p.subbrotherstatutopic(),Navigation::BrotherAgvStatuCallback,this);
+
   }
   inited_=true;
 
@@ -80,10 +85,13 @@ void Navigation::publish_statu() {
   while (exit_ == false)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+    //发布nav状态
     NavMessage::NavStatu statu;
     statu.set_statu(eReady); //默认ready
     NavMessage::Action *current_action = nullptr;
-    if (running_) {
+    if (running_)
+    {
       statu.set_statu(eRunning);
       statu.set_key(global_navCmd_.key());
       std::queue<NavMessage::Action> actios = unfinished_cations_;
@@ -97,23 +105,7 @@ void Navigation::publish_statu() {
 
       }
     }
-    statu.set_pose_timeout(true);    //默认设置位姿超时
-    statu.set_twist_timeout(true);
-    NavMessage::AGVStatu agvStatu;
-    if (timedPose_.timeout() == false) {
-      statu.set_pose_timeout(false);
-      Pose2d pose = timedPose_.Get();
-      agvStatu.set_x(pose.x());
-      agvStatu.set_y(pose.y());
-      agvStatu.set_theta(pose.theta());
-    }
-    if (timedV_.timeout() == false && timedA_.timeout() == false) {
-      statu.set_twist_timeout(false);
-      agvStatu.set_v(timedV_.Get());
-      agvStatu.set_vth(timedA_.Get());
-    }
-    statu.mutable_current_pose()->CopyFrom(agvStatu);
-
+    //发布MPC信息
     if (current_action != nullptr) {
       delete current_action;
       //发布mpc 预选点
@@ -137,7 +129,29 @@ void Navigation::publish_statu() {
         }
       }
     }
-    terminator_->publish_nav_statu(statu);
+    MqttMsg msg;
+    msg.fromProtoMessage(statu);
+    if(terminator_)
+      terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);
+
+    //发布位姿  --------------------------------
+    if (timedPose_.timeout() == false&&
+        timedV_.timeout() == false && timedA_.timeout() == false) {
+      NavMessage::AGVStatu 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());
+      if(terminator_) {
+        MqttMsg msg;
+        msg.fromProtoMessage(agvStatu);
+        terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
+      }
+    }
+
+
   }
 }
 
@@ -197,11 +211,33 @@ Navigation::Navigation()
   terminator_= nullptr;
 }
 
+void Navigation::BrotherAgvStatuCallback(const MqttMsg& msg,void* context)
+{
+  Navigation* navigator=(Navigation*)context;
+
+  NavMessage::AGVStatu brother_statu;
+  if(msg.toProtoMessage(brother_statu)==false) {
+    printf(" msg transform to AGVStatu failed ..!!\n");
+    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);
+}
 
-void Navigation::NavCmdCallback(const NavMessage::NavCmd& cmd,void* context)
+void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
 {
   Navigation* navigator=(Navigation*)context;
 
+  NavMessage::NavCmd cmd;
+  if(msg.toProtoMessage(cmd)==false) {
+    printf(" msg transform to NavCmd failed ..!!\n");
+    return;
+  }
+
+
   if(cmd.action()==3)
   {
     navigator->Cancel();

MPC/navigation.h

@@ -45,7 +45,8 @@ public:
  protected:
 
     static void RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context);
-    static void NavCmdCallback(const NavMessage::NavCmd& cmd,void* context);
+    static void NavCmdCallback(const MqttMsg& msg,void* context);
+    static void BrotherAgvStatuCallback(const MqttMsg& msg,void* context);
 
     static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,const Pose2d& diff);
     bool execute_along_action(const Pose2d& target,const Pose2d& target_diff);
@@ -69,6 +70,11 @@ public:
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
+
+    TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
+    TimedLockData<double> timedBrotherV_;              //底盘数据
+    TimedLockData<double> timedBrotherA_;
+
     NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
     std::queue<NavMessage::Action> unfinished_cations_; //未完成的动作
     std::thread* thread_= nullptr;
@@ -78,6 +84,8 @@ public:
 
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
+
+    Navigation_parameter parameter_;
 };
 
 

config/navigation.prototxt

@@ -0,0 +1,19 @@
+Agv_emqx
+{
+	NodeId:"agv1"
+	ip:"127.0.0.1"
+	port:1883
+	pubTopic:"monitor/speedcmd"
+	subTopic:"monitor/statu"
+}
+
+Terminal_emqx
+{
+	NodeId:"agv1-nav"
+	ip:"127.0.0.1"
+	port:1883
+	pubStatuTopic:"agv1/agv_statu"
+	pubNavStatuTopic:"agv1/nav_statu"
+	subNavCmdTopic:"agv1/nav_cmd"
+	subBrotherStatuTopic:"agv1_child/agv_statu"
+}

config/navigation_child.prototxt

@@ -0,0 +1,19 @@
+Agv_emqx
+{
+	NodeId:"agv1-child"
+	ip:"127.0.0.1"
+	port:1883
+	pubTopic:"monitor_child/speedcmd"
+	subTopic:"monitor_child/statu"
+}
+
+Terminal_emqx
+{
+	NodeId:"agv1-child-nav"
+	ip:"127.0.0.1"
+	port:1883
+	pubStatuTopic:"agv1_child/agv_statu"
+	pubNavStatuTopic:"agv1_child/nav_statu"
+	subNavCmdTopic:"agv1_child/nav_cmd"
+	subBrotherStatuTopic:"agv1/agv_statu"
+}

controller.cpp

@@ -10,11 +10,15 @@
 
 int main(int argc,char* argv[])
 {
+  if(argc<2)
+  {
+    printf(" argc must == 2, exe parameter_file\n");
+    return -1;
+  }
+  std::string parameter_file=argv[1];
   Navigation* g_navigator=new Navigation();
-
   Navigation_parameter parameter;
 
-  std::string parameter_file="../config/navigation.prototxt";
   if(proto_tool::get_instance_pointer()->read_proto_param(parameter_file,parameter)==false)
   {
     printf(" read proto parameter failed:%s\n",parameter_file.c_str());

message.proto

@@ -9,9 +9,10 @@ message AGVStatu {
 }
 
 message Speed {
-  int32 type=1; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
-  float v=2;
-  float vth=3;
+  int32 H=1;  //心跳
+  int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
+  float V=3;  //角速度
+  float W=4;  //线速度
 }
 
 
@@ -44,14 +45,11 @@ message NavCmd
 
 message NavStatu
 {
-    bool statu=1; //0:ready  1:running
-  string key=2; // 任务唯一码
-  repeated Action unfinished_actions=3;  //未完成的动作，第一个即为当前动作
-  AGVStatu current_pose=4; //当前位姿
-  bool pose_timeout=5;
-  bool twist_timeout=6;   //速度是否超时
-    Trajectory selected_traj=7;
-    Trajectory predict_traj=8;
+  bool statu = 1; //0:ready  1:running
+  string key = 2; // 任务唯一码
+  repeated Action unfinished_actions = 3;  //未完成的动作，第一个即为当前动作
+  Trajectory selected_traj = 4;
+  Trajectory predict_traj = 5;
 
 }
 

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -150,8 +150,8 @@ int main(int argc, char **argv) {
   gps->enable(timeStep);
   imu->enable(timeStep);
 
-  double w=2.450;
-  double l=1.274;
+  double w=2.5;
+  double l=1.3;
   double radius=0.1915;
   double K=1.0/radius;
 
@@ -234,7 +234,7 @@ int main(int argc, char **argv) {
     {
       Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else {    //巡线/前进
+    else if(g_speed.Get().type()==3) {    //巡线/前进
 
       if (fabs(cmd_vth) > 0.4) {
         cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
@@ -262,7 +262,7 @@ int main(int argc, char **argv) {
 
             if(BaseR<0.05)
               BaseR=0.05;
-            printf("BaseR : %f \n",BaseR);
+            //printf("BaseR : %f \n",BaseR);
 
             if (BaseR > 0) {
               if (cmd_vth * cmd_v >= 0) {
@@ -291,6 +291,8 @@ int main(int argc, char **argv) {
 
         }
       }
+    }else{
+    L1=L2=L3=L4=0;
     }
 
     printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",

projects/controllers/AGV_controller/timedlockdata.hpp

@@ -0,0 +1,59 @@
+//
+// Created by zx on 22-12-1.
+//
+
+#ifndef LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#define LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#include <chrono>
+#include <mutex>
+
+template <typename T>
+class TimedLockData
+{
+ public:
+    TimedLockData();
+    void reset(const T& tdata,double timeout=0.1);
+    bool timeout();
+    T Get();
+
+ protected:
+    T data_;
+    std::chrono::steady_clock::time_point tp_;
+    std::mutex mutex_;
+    double timeout_;
+
+};
+
+template <typename T>
+TimedLockData<T>::TimedLockData()
+{
+  timeout_=0.1;
+}
+
+template <typename T>
+void TimedLockData<T>::reset(const T& tdata,double timeout)
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  data_=tdata;
+  timeout_=timeout;
+  tp_=std::chrono::steady_clock::now();
+}
+
+template <typename T>
+bool TimedLockData<T>::timeout()
+{
+  auto now=std::chrono::steady_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - tp_);
+  double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+  return time>timeout_;
+}
+
+template <typename T>
+T TimedLockData<T>::Get()
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  return data_;
+}
+
+
+#endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

projects/controllers/agv_child/AGV_controller.cpp

@@ -0,0 +1,316 @@
+
+#include <webots/Robot.hpp>
+#include <webots/Motor.hpp>
+#include <webots/PositionSensor.hpp>
+#include <webots/GPS.hpp>
+#include <webots/Gyro.hpp>
+#include <webots/InertialUnit.hpp>
+#include "emqx-client/mqttmsg.h"
+#include "emqx-client/paho_client.h"
+#include <unistd.h>
+#include <chrono>
+#include <random>
+#include "timedlockdata.hpp"
+// All the webots classes are defined in the "webots" namespace
+using namespace webots;
+
+Paho_client* client_= nullptr;
+TimedLockData<NavMessage::Speed> g_speed;
+
+
+void SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
+{
+  NavMessage::Speed speed;
+  msg.toProtoMessage(speed);
+  g_speed.reset(speed,0.3);
+}
+
+double generae_random(std::default_random_engine& generator,double mean,double sigma)
+{
+  std::normal_distribution<double> distribution(mean, sigma);
+  return distribution(generator);
+}
+
+bool init_mqtt(std::string ip,int port,std::string nodeId,std::string subtopic)
+{
+  if(client_!= nullptr)
+  {
+    client_->disconnect();
+    delete client_;
+  }
+  client_=new Paho_client(nodeId);
+  bool ret= client_->connect(ip,port);
+  if(ret)
+  {
+    while(!client_->isconnected()) usleep(1000);
+    client_->subcribe(subtopic,1,SpeedChangeCallback, nullptr);
+  }
+  return ret;
+}
+
+void Rotating(double wmg,double w,double l,
+        double& R1,double& R2,double& R3,double& R4,
+              double& L1,double& L2,double& L3,double& L4)
+{
+  double theta=atan(l/w);
+  R1=(-theta);
+  R2=(theta);
+  R3=(theta);
+  R4=(-theta);
+
+  double velocity=wmg*sqrt(w*w+l*l);
+  L1=(-velocity);
+  L2=(velocity);
+  L3=(-velocity);
+  L4=(velocity);
+}
+
+void Horizontal(double velocity,double& R1,double& R2,double& R3,double& R4,
+                double& L1,double& L2,double& L3,double& L4)
+{
+  R1=-M_PI/2.0;
+  R2=M_PI/2.0;
+  R3=M_PI/2.0;
+  R4=-M_PI/2.0;
+  L1=-velocity;
+  L2=velocity;
+  L3=velocity;
+  L4=-velocity;
+}
+
+void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
+                double& L1,double& L2,double& L3,double& L4)
+{
+  R1=0;
+  R2=0;
+  R3=0;
+  R4=0;
+  L1=velocity;
+  L2=velocity;
+  L3=velocity;
+  L4=velocity;
+}
+
+
+int main(int argc, char **argv) {
+  //init mqtt
+  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV-child","monitor_child/speedcmd"))
+  {
+    printf(" Init mqtt failed...\n");
+    return -1;
+  }
+  //随机数
+
+  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
+  std::default_random_engine generator(seed);
+
+  // create the Robot instance.
+  Robot *robot = new Robot();
+
+  // get the time step of the current world.
+  int timeStep = (int)robot->getBasicTimeStep();
+
+  webots::Motor* RM1=robot->getMotor("R1_motor");
+  webots::Motor* RM2=robot->getMotor("R2_motor");
+  webots::Motor* RM3=robot->getMotor("R3_motor");
+  webots::Motor* RM4=robot->getMotor("R4_motor");
+
+  //R1->getPositionSensor()->enable(timeStep);
+
+  webots::Motor* R1_l=robot->getMotor("L1_motor");
+  webots::Motor* R2_l=robot->getMotor("L2_motor");
+  webots::Motor* R3_l=robot->getMotor("L3_motor");
+  webots::Motor* R4_l=robot->getMotor("L4_motor");
+
+
+  RM1->setPosition(0);
+  RM2->setPosition(0);
+  RM3->setPosition(0);
+  RM4->setPosition(0);
+
+  R1_l->setPosition(INFINITY);
+  R2_l->setPosition(INFINITY);
+  R3_l->setPosition(INFINITY);
+  R4_l->setPosition(INFINITY);
+
+  /*RM1->setTorque(100);
+  RM2->setTorque(100);
+  RM3->setTorque(100);
+  RM4->setTorque(100);*/
+
+  R1_l->setVelocity(0.0);
+  R2_l->setVelocity(0.0);
+  R3_l->setVelocity(0.0);
+  R4_l->setVelocity(0.0);
+
+  webots::GPS* gps=robot->getGPS("gps");
+  webots::InertialUnit* imu=robot->getInertialUnit("imu");
+  webots::Gyro* gyr=robot->getGyro("gyro");
+  gyr->enable(timeStep);
+  gps->enable(timeStep);
+  imu->enable(timeStep);
+
+  double w=2.5;
+  double l=1.3;
+  double radius=0.1915;
+  double K=1.0/radius;
+
+  auto last=std::chrono::steady_clock::now();
+  while (robot->step(timeStep) != -1) {
+
+    auto now=std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - last);
+    double time = double(duration.count()) * std::chrono::microseconds::period::num /
+            std::chrono::microseconds::period::den;
+
+    ////发布：
+    //增加高斯分布
+    double x=gps->getValues()[2] + generae_random(generator,0,0.02);
+    double y=gps->getValues()[0] + generae_random(generator,0,0.02);
+    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.3*M_PI/180.0);
+
+    //获取gps速度，判断朝向
+    double vm1=R3_l->getVelocity()*radius;
+    double vm2=-R4_l->getVelocity()*radius;
+    double v=gps->getSpeed();
+    if(vm1+vm2<0.0)
+      v=-v;
+    //陀螺仪角速度
+    double vth=gyr->getValues()[1];
+
+    if(time>0.2)
+    {
+      printf(" publish delay................  time:%f -------------------------------------\n",time);
+    }
+    /*printf("dt:%f x y:%f  %f,  yaw:%f   velocity:%f vth:%f  %f  %f\n",time,
+            x,y,theta,
+           v,gyr->getValues()[0],gyr->getValues()[1],gyr->getValues()[2]);*/
+    last=now;
+
+    if(client_)
+    {
+      MqttMsg msg;
+      NavMessage::AGVStatu statu;
+      statu.set_x(x);
+      statu.set_y(y);
+      statu.set_theta(theta);
+      statu.set_v(v);
+      statu.set_vth(vth);
+      msg.fromProtoMessage(statu);
+      client_->publish("monitor_child/statu",1,msg);
+    }
+
+    if(g_speed.timeout())
+    {
+      RM1->setPosition(0);
+      RM2->setPosition(0);
+      RM3->setPosition(0);
+      RM4->setPosition(0);
+
+      R1_l->setVelocity(0);
+      R2_l->setVelocity(0);
+      R3_l->setVelocity(0);
+      R4_l->setVelocity(0);
+      continue;
+    }
+
+    //变速----------------------------------
+    double cmd_v=g_speed.Get().v();
+    double cmd_vth=g_speed.Get().vth();
+
+    //初始值
+    double R1=0,R2=0,R3=0,R4=0;
+    double L1=cmd_v;
+    double L2=cmd_v;
+    double L3=cmd_v;
+    double L4=cmd_v;
+
+    if(g_speed.Get().type()==1) //原地旋转
+    {
+      if (fabs(cmd_vth) > 0.0001)
+        Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+    }
+    else if(g_speed.Get().type()==2)    //横移
+    {
+      Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
+    }
+    else {    //巡线/前进
+
+      if (fabs(cmd_vth) > 0.4) {
+        cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
+      }
+
+
+      //------------------------------------------------
+      //无角速度，直线
+      if (fabs(cmd_vth) < 0.0001) {
+        R1 = R2 = R3 = R4 = 0;
+        L1 = L2 = L3 = L4 = cmd_v;
+      } else  //有角速度
+      {
+        if (fabs(cmd_v) < 0.00001) {
+          //原地旋转
+          if (fabs(cmd_vth) > 0.0001)
+            Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+        } else {
+          //默认原地旋转
+          Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
+          double base = pow(cmd_v / cmd_vth, 2) - (l * l / 4);
+          //满足运动方程
+          if (base >= 0.0001) {
+            double BaseR = sqrt(base) - w / 2;
+
+            if(BaseR<0.05)
+              BaseR=0.05;
+            printf("BaseR : %f \n",BaseR);
+
+            if (BaseR > 0) {
+              if (cmd_vth * cmd_v >= 0) {
+                R1 = atan(l / BaseR);
+                R2 = atan(l / (BaseR + w));
+                L3 = BaseR * cmd_vth;
+                L4 = (BaseR + w) * cmd_vth;
+                R3 = 0;
+                R4 = 0;
+                L1 = L3 / cos(R1);
+                L2 = L4 / cos(R2);
+
+              } else {
+                R1 = -atan(l / (BaseR + w));
+                R2 = -atan(l / BaseR);
+                L3 = -(BaseR + w) * cmd_vth;
+                L4 = -BaseR * cmd_vth;
+                R3 = 0;
+                R4 = 0;
+                L1 = L3 / cos(R1);
+                L2 = L4 / cos(R2);
+              }
+
+            }
+          }
+
+        }
+      }
+    }
+
+    printf(" R1-R4:%.5f %.5f %.5f %.5f  L1-L4:%.5f %.5f %.5f %.5f  V:%.5f  Vth:%.5f\n",
+            R1,R2,R3,R4,L1,L2,L3,L4,v,vth);
+     
+     
+    RM1->setPosition(R1);
+    RM2->setPosition(R2);
+    RM3->setPosition(R3);
+    RM4->setPosition(R4);
+
+    R1_l->setVelocity(L1*K);
+    R2_l->setVelocity(-L2*K);
+    R3_l->setVelocity(L3*K);
+    R4_l->setVelocity(-L4*K);
+
+  };
+
+  // Enter here exit cleanup code.
+
+  delete robot;
+  return 0;
+}

projects/controllers/agv_child/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0)
+
+# Setup the project.
+# Its name is defined to be the controller directory name.
+get_filename_component(PROJECT ${CMAKE_SOURCE_DIR} NAME)
+project(${PROJECT})
+
+# Get C or C++ sources in the current directory (only).
+file(GLOB C_SOURCES *.c)
+file(GLOB CPP_SOURCES *.cpp)
+set(SOURCES ${C_SOURCES} ${CPP_SOURCES})
+
+# Set the  Webots home path (change it according to your installation method)
+set(WEBOTS_HOME "/usr/local/webots")
+#set(WEBOTS_HOME "/snap/webots/current/usr/share/webots")
+FIND_PACKAGE(Protobuf REQUIRED)
+# Link with the Webots controller library.
+link_directories($ENV{WEBOTS_HOME}/lib/controller)
+set (LIBRARIES m ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX})
+include_directories(
+        /usr/local/webots/include/controller/cpp
+)
+
+# Setup the target executable.
+add_executable(${PROJECT} ${SOURCES}
+        timedlockdata.hpp
+        emqx-client/paho_client.cpp
+        emqx-client/mqttmsg.cpp
+        emqx-client/message.pb.cc)
+target_link_libraries(${PROJECT} ${LIBRARIES}
+        paho-mqtt3a
+        ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES}
+        libglog.a libgflags.a
+        )
+
+# Copy the target executable at the right location.
+add_custom_command(TARGET ${PROJECT} POST_BUILD COMMAND ${CMAKE_COMMAND} -E
+  copy ${CMAKE_BINARY_DIR}/${PROJECT} ${CMAKE_SOURCE_DIR}
+)

projects/controllers/agv_child/emqx-client/mqttmsg.cpp

@@ -0,0 +1,289 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#include "mqttmsg.h"
+#include <string.h>
+
+#include <google/protobuf/util/json_util.h>
+using namespace google::protobuf;
+MqttMsg::MqttMsg()
+{
+  data_= nullptr;
+  length_=0;
+}
+MqttMsg::MqttMsg(char* data,const int length)
+{
+  data_=(char*)malloc(length);
+  memcpy(data_,data,length);
+  length_=length;
+}
+MqttMsg::MqttMsg(const MqttMsg& msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+  data_=(char*)malloc(msg.length_);
+  memcpy(data_,msg.data_,msg.length_);
+  length_=msg.length_;
+}
+MqttMsg& MqttMsg::operator=(const MqttMsg& msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+  data_=(char*)malloc(msg.length_);
+  memcpy(data_,msg.data_,msg.length_);
+  length_=msg.length_;
+  return *this;
+}
+
+char* MqttMsg::data()const{
+  return data_;
+}
+int MqttMsg::length()const{
+  return length_;
+}
+
+
+
+
+MqttMsg::~MqttMsg(){
+  if(data_!= nullptr)
+  {
+    free(data_);
+    data_ = nullptr;
+  }
+  length_=0;
+}
+
+/*
+void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  AGVStatu statu;
+  statu.set_x(x);
+  statu.set_y(y);
+  statu.set_theta(theta);
+  statu.set_v(v);
+  statu.set_vth(vth);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(statu,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vth)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+  AGVStatu statu;
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &statu);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+
+  if(ret.ok())
+  {
+    x=statu.x();
+    y=statu.y();
+    theta=statu.theta();
+    v=statu.v();
+    vth=statu.vth();
+
+  }
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavSpeed(const NavMessage::Speed& speed)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(speed,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+
+}
+
+bool MqttMsg::toNavSpeed(NavMessage::Speed& speed)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+
+    //printf("%s\n",buf);
+    ret = util::JsonStringToMessage(buf, &speed);
+    printf("change speed v:%f,  vth:%f\n",speed.v(),speed.vth());
+
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+
+  free(buf);
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavCmd(const NavMessage::NavCmd& cmd)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavCmd(NavMessage::NavCmd& cmd)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &cmd);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavStatu(const NavMessage::NavStatu &statu)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(statu,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavStatu(NavMessage::NavStatu &statu)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &statu);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
+
+  return ret.ok();
+}
+*/
+
+void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(message,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+}
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &message);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+  return ret.ok();
+}

projects/controllers/agv_child/emqx-client/mqttmsg.h

@@ -0,0 +1,55 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#ifndef PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
+#define PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
+#include "message.pb.h"
+#include <mutex>
+
+class MqttMsg
+{
+ public:
+    enum Dtype
+    {
+      ePointCloudXYZ,
+      ePointCloudXYZI,
+      eString,
+      eImage,
+      eAGVStatu,
+      eAGVSpeed
+    };
+ public:
+    MqttMsg();
+    MqttMsg(char* data,const int length);
+    MqttMsg(const MqttMsg& msg);
+    MqttMsg& operator=(const MqttMsg& msg);
+    ~MqttMsg();
+
+    char* data()const;
+    int length()const;
+
+
+    void fromProtoMessage(const google::protobuf::Message& messge);
+    bool toProtoMessage(google::protobuf::Message& message);
+
+    /*void fromStatu(double x,double y,double theta,double v,double vth);
+    void fromNavSpeed(const NavMessage::Speed& speed);
+    void fromNavCmd(const NavMessage::NavCmd& cmd);
+    void fromNavStatu(const NavMessage::NavStatu& statu);
+
+    bool toStatu(double& x,double& y,double& theta,double& v,double& vth);
+    bool toNavSpeed(NavMessage::Speed& speed);
+    bool toNavCmd(NavMessage::NavCmd& cmd);
+    bool toNavStatu(NavMessage::NavStatu& statu);*/
+
+
+ protected:
+    char* data_=nullptr;
+    int length_=0;
+    std::mutex mutex_;
+
+};
+
+
+#endif //PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_

projects/controllers/agv_child/emqx-client/paho_client.cpp

@@ -0,0 +1,192 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#include "paho_client.h"
+
+
+Paho_client::Paho_client(std::string clientid){
+  clientid_=clientid;
+  context_= nullptr;
+  connected_=false;
+}
+Paho_client::~Paho_client(){}
+void Paho_client::set_maxbuf(int size)
+{
+  create_opts_.maxBufferedMessages=size+256;
+}
+bool Paho_client::connect(std::string address,int port){
+  char connstr[255]={0};
+  sprintf(connstr,"mqtt://%s:%d",address.c_str(),port);
+  int rc;
+
+  if ((rc=MQTTAsync_createWithOptions(&client_,connstr,clientid_.c_str(),MQTTCLIENT_PERSISTENCE_NONE,NULL,&create_opts_)
+          != MQTTASYNC_SUCCESS))
+  {
+    printf("Failed to create client object, return code %d\n", rc);
+    return false;
+  }
+
+  if ((rc = MQTTAsync_setCallbacks(client_, this, connlost, messageArrived, NULL)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to set callback, return code %d\n", rc);
+    return false;
+  }
+
+  conn_opts_.keepAliveInterval = 20;
+  conn_opts_.cleansession = 1;
+  conn_opts_.onSuccess = onConnect;
+  conn_opts_.onFailure = onConnectFailure;
+  conn_opts_.context = this;
+
+  if ((rc = MQTTAsync_connect(client_, &conn_opts_)) == MQTTASYNC_SUCCESS)
+  {
+    printf("Connected success \n");
+  }
+
+  address_=address;
+  port_=port;
+  return true;
+}
+bool Paho_client::disconnect(){
+  return true;
+}
+void Paho_client::publish(const std::string& topic,int QOS,const MqttMsg& msg){
+  if(connected_==false)
+  {
+    printf(" pls connect before publish\n");
+    return;
+  }
+  pub_opts_.onSuccess = onSend;
+  pub_opts_.onFailure = onSendFailure;
+  pub_opts_.context = client_;
+
+  pubmsg_.payload = msg.data();
+  pubmsg_.payloadlen = msg.length();
+  pubmsg_.qos = QOS;
+  pubmsg_.retained = 0;
+  int rc=-1;
+  if ((rc = MQTTAsync_sendMessage(client_, topic.c_str(), &pubmsg_, &pub_opts_)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start sendMessage, return code %d\n", rc);
+  }
+
+}
+void Paho_client::subcribe(std::string topic,int QOS,ArrivedCallback callback,void* context){
+  arrivedCallback_=callback;
+  context_=context;
+  MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer;
+  printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n"
+         "\n", topic.c_str(), clientid_.c_str(), QOS);
+  opts.onSuccess = onSubscribe;
+  opts.onFailure = onSubscribeFailure;
+  opts.context = this;
+  int rc=-1;
+
+  if ((rc = MQTTAsync_subscribe(client_, topic.c_str(), QOS, &opts)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start subscribe, return code %d\n", rc);
+    return;
+  }
+
+}
+
+int Paho_client::messageArrived(void* context, char* topicName, int topicLen, MQTTAsync_message* message)
+{
+  Paho_client* cl=(Paho_client*)context;
+  MqttMsg msg((char*)message->payload,message->payloadlen);
+  if(cl->arrivedCallback_!= nullptr)
+    cl->arrivedCallback_(topicName,message->qos,msg,cl->context_);
+  MQTTAsync_freeMessage(&message);
+  MQTTAsync_free(topicName);
+  return 1;
+}
+
+
+void Paho_client::connlost(void *context, char *cause)
+{
+  Paho_client* cl=(Paho_client*)context;
+  MQTTAsync client = cl->client_;
+  MQTTAsync_connectOptions conn_opts = cl->conn_opts_;
+  int rc;
+
+  printf("\nConnection lost\n");
+  printf("     cause: %s\n", cause);
+  printf("Reconnecting\n");
+  conn_opts.keepAliveInterval = 20;
+  conn_opts.cleansession = 1;
+  conn_opts.onSuccess = onConnect;
+  conn_opts.onFailure = onConnectFailure;
+  conn_opts.context = context;
+  cl->connected_=false;
+  if ((rc = MQTTAsync_connect(client, &conn_opts)) == MQTTASYNC_SUCCESS)
+  {
+    printf(" Reconnected success\n");
+    cl->connected_=true;
+  }
+}
+
+
+void Paho_client::onConnectFailure(void* context, MQTTAsync_failureData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s,Connect failed, rc %d\n", cl->clientid_.c_str(),response ? response->code : 0);
+
+  int rc=-1;
+  if ((rc = MQTTAsync_connect(cl->client_, &(cl->conn_opts_))) == MQTTASYNC_SUCCESS)
+  {
+    printf(" Reconnected success\n");
+  }
+
+}
+
+void Paho_client::onSubscribe(void* context, MQTTAsync_successData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s  Subscribe succeeded\n",cl->clientid_.c_str());
+
+}
+
+void Paho_client::onSubscribeFailure(void* context, MQTTAsync_failureData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s Subscribe failed, rc %d\n",cl->clientid_.c_str(), response->code);
+}
+
+
+void Paho_client::onConnect(void* context, MQTTAsync_successData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  cl->connected_=true;
+}
+
+
+void Paho_client::onSendFailure(void* context, MQTTAsync_failureData* response)
+{
+  MQTTAsync client = (MQTTAsync)context;
+  MQTTAsync_disconnectOptions opts = MQTTAsync_disconnectOptions_initializer;
+  int rc;
+
+  printf("Message send failed token %d error code %d\n", response->token, response->code);
+  opts.onSuccess = onDisconnect;
+  opts.onFailure = onDisconnectFailure;
+  opts.context = client;
+  if ((rc = MQTTAsync_disconnect(client, &opts)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start disconnect, return code %d\n", rc);
+  }
+}
+
+void Paho_client::onSend(void* context, MQTTAsync_successData* response)
+{
+  // This gets called when a message is acknowledged successfully.
+}
+void Paho_client::onDisconnectFailure(void* context, MQTTAsync_failureData* response)
+{
+  printf("Disconnect failed\n");
+}
+
+void Paho_client::onDisconnect(void* context, MQTTAsync_successData* response)
+{
+  printf("Successful disconnection\n");
+}

projects/controllers/agv_child/emqx-client/paho_client.h

@@ -0,0 +1,58 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#ifndef PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_
+#define PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_
+#include "MQTTAsync.h"
+#include <string>
+#include "mqttmsg.h"
+
+typedef void (*ArrivedCallback)(std::string topic,int QOS,MqttMsg& msg,void* context);
+
+class Paho_client
+{
+ public:
+    Paho_client(std::string clientid);
+    ~Paho_client();
+    void set_maxbuf(int size);
+    bool connect(std::string address,int port);
+    bool disconnect();
+    void subcribe(std::string topic,int QOS,ArrivedCallback callback,void* context);
+    void publish(const std::string& topic,int QOS,const MqttMsg& msg);
+
+    bool isconnected(){return connected_;}
+
+ protected:
+    static int messageArrived(void* context, char* topicName, int topicLen, MQTTAsync_message* message);
+    static void connlost(void *context, char *cause);
+    static void onConnectFailure(void* context, MQTTAsync_failureData* response);
+    static void onSubscribe(void* context, MQTTAsync_successData* response);
+    static void onSubscribeFailure(void* context, MQTTAsync_failureData* response);
+    static void onConnect(void* context, MQTTAsync_successData* response);
+    static void onSendFailure(void* context, MQTTAsync_failureData* response);
+    static void onSend(void* context, MQTTAsync_successData* response);
+    static void onDisconnectFailure(void* context, MQTTAsync_failureData* response);
+    static void onDisconnect(void* context, MQTTAsync_successData* response);
+
+ protected:
+
+    bool connected_;
+    std::string clientid_;
+    std::string address_;
+    int port_;
+
+    MQTTAsync client_;
+    MQTTAsync_connectOptions conn_opts_ = MQTTAsync_connectOptions_initializer;
+
+    MQTTAsync_message pubmsg_ = MQTTAsync_message_initializer;
+    MQTTAsync_responseOptions pub_opts_ = MQTTAsync_responseOptions_initializer;
+    MQTTAsync_createOptions create_opts_=MQTTAsync_createOptions_initializer;
+
+    ArrivedCallback arrivedCallback_= nullptr;
+    void* context_;
+
+};
+
+
+#endif //PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_

projects/controllers/agv_child/timedlockdata.hpp

@@ -0,0 +1,59 @@
+//
+// Created by zx on 22-12-1.
+//
+
+#ifndef LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#define LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#include <chrono>
+#include <mutex>
+
+template <typename T>
+class TimedLockData
+{
+ public:
+    TimedLockData();
+    void reset(const T& tdata,double timeout=0.1);
+    bool timeout();
+    T Get();
+
+ protected:
+    T data_;
+    std::chrono::steady_clock::time_point tp_;
+    std::mutex mutex_;
+    double timeout_;
+
+};
+
+template <typename T>
+TimedLockData<T>::TimedLockData()
+{
+  timeout_=0.1;
+}
+
+template <typename T>
+void TimedLockData<T>::reset(const T& tdata,double timeout)
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  data_=tdata;
+  timeout_=timeout;
+  tp_=std::chrono::steady_clock::now();
+}
+
+template <typename T>
+bool TimedLockData<T>::timeout()
+{
+  auto now=std::chrono::steady_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - tp_);
+  double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+  return time>timeout_;
+}
+
+template <typename T>
+T TimedLockData<T>::Get()
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  return data_;
+}
+
+
+#endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

tool/proto_tool.cpp

@@ -0,0 +1,42 @@
+
+
+
+#include "proto_tool.h"
+#include <fcntl.h>
+#include<unistd.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+
+//读取protobuf 配置文件，转化为protobuf参数形式
+//input：	prototxt_path ：prototxt文件路径
+//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+bool proto_tool::read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter)
+{
+	int fd = open(prototxt_path.c_str(), O_RDONLY);
+	if (fd == -1) return false;
+	FileInputStream* input = new FileInputStream(fd);
+	bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+	delete input;
+	close(fd);
+	return success;
+}
+
+
+
+
+
+
+
+
+
+
+
+

tool/proto_tool.h

@@ -0,0 +1,56 @@
+
+
+
+
+
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
+
+#include "../tool/singleton.h"
+#include <istream>
+#include <google/protobuf/message.h>
+
+class proto_tool:public Singleton<proto_tool>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<proto_tool>;
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	proto_tool(const proto_tool&)=delete;
+	proto_tool& operator =(const proto_tool&)= delete;
+	~proto_tool()=default;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	proto_tool()=default;
+
+
+public:
+	//读取protobuf 配置文件，转化为protobuf参数形式
+	//input：	prototxt_path ：prototxt文件路径
+	//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+	static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter);
+};
+
+
+
+
+#endif //__PROTO_TOOL_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

tool/singleton.cpp

@@ -0,0 +1,4 @@
+
+#include "singleton.h"
+
+

tool/singleton.h

@@ -0,0 +1,81 @@
+
+/* Singleton 是单例类的模板。
+ * https://www.cnblogs.com/sunchaothu/p/10389842.html
+ * 单例 Singleton 是设计模式的一种，其特点是只提供唯一一个类的实例,具有全局变量的特点，在任何位置都可以通过接口获取到那个唯一实例;
+ * 全局只有一个实例：static 特性，同时禁止用户自己声明并定义实例（把构造函数设为 private 或者 protected）
+ * Singleton 模板类对这种方法进行了一层封装。
+ * 单例类需要从Singleton继承。
+ * 子类需要将自己作为模板参数T 传递给 Singleton<T> 模板;
+ * 同时需要将基类声明为友元，这样Singleton才能调用子类的私有构造函数。
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来操作 唯一的实例。
+ * */
+
+#ifndef __SINGLIETON_H
+#define __SINGLIETON_H
+
+//#include <iostream>
+
+template<typename T>
+class Singleton
+{
+public:
+	//获取单例的引用
+	static T& get_instance_references()
+	{
+		static T instance;
+		return instance;
+	}
+	//获取单例的指针
+	static T* get_instance_pointer()
+	{
+		return &(get_instance_references());
+	}
+
+	virtual ~Singleton()
+	{
+//		std::cout<<"destructor called!"<<std::endl;
+	}
+
+	Singleton(const Singleton&)=delete;					//关闭拷贝构造函数
+	Singleton& operator =(const Singleton&)=delete;		//关闭赋值函数
+
+protected:
+	//构造函数需要是 protected，这样子类才能继承；
+	Singleton()
+	{
+//		std::cout<<"constructor called!"<<std::endl;
+	}
+
+};
+
+/*
+// 如下是 使用样例：
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+
+class DerivedSingle:public Singleton<DerivedSingle>
+{
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<DerivedSingle>;
+public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	DerivedSingle(const DerivedSingle&)=delete;
+	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+	DerivedSingle()=default;
+};
+
+int main(int argc, char* argv[]){
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
+	return 0;
+}
+
+ */
+
+#endif