1.添加打印

MPC/monitor/emqx/message.pb.cc

@@ -212,14 +212,17 @@ PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
 template <typename>
 PROTOBUF_CONSTEXPR NavCmd::NavCmd(
     ::_pbi::ConstantInitialized): _impl_{
-    /*decltype(_impl_.newactions_)*/{}
+    /*decltype(_impl_._has_bits_)*/{}
+  , /*decltype(_impl_._cached_size_)*/{}
+  , /*decltype(_impl_.newactions_)*/{}
   , /*decltype(_impl_.key_)*/ {
     &::_pbi::fixed_address_empty_string, ::_pbi::ConstantInitialized {}
   }
 
   , /*decltype(_impl_.action_)*/ 0
 
-  , /*decltype(_impl_._cached_size_)*/{}} {}
+  , /*decltype(_impl_.sequence_)*/ 0
+} {}
 struct NavCmdDefaultTypeInternal {
   PROTOBUF_CONSTEXPR NavCmdDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
   ~NavCmdDefaultTypeInternal() {}
@@ -451,7 +454,7 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     ~0u,
     ~0u,
     ~0u,
-    ~0u,  // no _has_bits_
+    PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _impl_._has_bits_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _internal_metadata_),
     ~0u,  // no _extensions_
     ~0u,  // no _oneof_case_
@@ -462,6 +465,11 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _impl_.action_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _impl_.key_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _impl_.newactions_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::NavCmd, _impl_.sequence_),
+    ~0u,
+    ~0u,
+    ~0u,
+    0,
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::NavResponse, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -536,11 +544,11 @@ static const ::_pbi::MigrationSchema
         { 64, 78, -1, sizeof(::NavMessage::PathNode)},
         { 84, -1, -1, sizeof(::NavMessage::Trajectory)},
         { 93, 108, -1, sizeof(::NavMessage::NewAction)},
-        { 115, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 126, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 136, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 147, 164, -1, sizeof(::NavMessage::NavStatu)},
-        { 173, 185, -1, sizeof(::NavMessage::RobotStatu)},
+        { 115, 127, -1, sizeof(::NavMessage::NavCmd)},
+        { 131, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 141, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 152, 169, -1, sizeof(::NavMessage::NavStatu)},
+        { 178, 190, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -579,32 +587,33 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
     "\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstreetNo"
     "de\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpathNo"
     "des\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\twheel"
-    "base\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006NavCm"
+    "base\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"t\n\006NavCm"
     "d\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewAct"
-    "ions\030\005 \003(\0132\025.NavMessage.NewAction\"(\n\013Nav"
-    "Response\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tM"
-    "anualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_type\030"
-    "\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005"
-    "statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mo"
-    "de\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage.Lida"
-    "rOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected_traj"
-    "\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014predic"
-    "t_traj\030\007 \001(\0132\026.NavMessage.Trajectory\022\020\n\010"
-    "in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobo"
-    "tStatu\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.AgvSt"
-    "atu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMessage"
-    ".NavCmd\032\027.NavMessage.NavResponse\"\000\0227\n\006Ca"
-    "ncel\022\022.NavMessage.NavCmd\032\027.NavMessage.Na"
-    "vResponse\"\000\022C\n\017ManualOperation\022\025.NavMess"
-    "age.ManualCmd\032\027.NavMessage.NavResponse\"\000"
-    "b\006proto3"
+    "ions\030\005 \003(\0132\025.NavMessage.NewAction\022\025\n\010seq"
+    "uence\030\006 \001(\005H\000\210\001\001B\013\n\t_sequence\"(\n\013NavResp"
+    "onse\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManua"
+    "lCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_type\030\002 \001("
+    "\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005stat"
+    "u\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003"
+    " \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage.LidarOdo"
+    "mStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001"
+    "(\0132\026.NavMessage.Trajectory\022,\n\014predict_tr"
+    "aj\030\007 \001(\0132\026.NavMessage.Trajectory\022\020\n\010in_s"
+    "pace\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobotSta"
+    "tu\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.AgvStatu2"
+    "\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMessage.Nav"
+    "Cmd\032\027.NavMessage.NavResponse\"\000\0227\n\006Cancel"
+    "\022\022.NavMessage.NavCmd\032\027.NavMessage.NavRes"
+    "ponse\"\000\022C\n\017ManualOperation\022\025.NavMessage."
+    "ManualCmd\032\027.NavMessage.NavResponse\"\000b\006pr"
+    "oto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1608,
+    1644,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -3428,6 +3437,12 @@ void NewAction::InternalSwap(NewAction* other) {
 
 class NavCmd::_Internal {
  public:
+  using HasBits = decltype(std::declval<NavCmd>()._impl_._has_bits_);
+  static constexpr ::int32_t kHasBitsOffset =
+    8 * PROTOBUF_FIELD_OFFSET(NavCmd, _impl_._has_bits_);
+  static void set_has_sequence(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
 };
 
 NavCmd::NavCmd(::PROTOBUF_NAMESPACE_ID::Arena* arena)
@@ -3439,12 +3454,15 @@ NavCmd::NavCmd(const NavCmd& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   NavCmd* const _this = this; (void)_this;
   new (&_impl_) Impl_{
-      decltype(_impl_.newactions_){from._impl_.newactions_}
+      decltype(_impl_._has_bits_){from._impl_._has_bits_}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.newactions_){from._impl_.newactions_}
     , decltype(_impl_.key_) {}
 
     , decltype(_impl_.action_) {}
 
-    , /*decltype(_impl_._cached_size_)*/{}};
+    , decltype(_impl_.sequence_) {}
+  };
 
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
   _impl_.key_.InitDefault();
@@ -3454,19 +3472,24 @@ NavCmd::NavCmd(const NavCmd& from)
   if (!from._internal_key().empty()) {
     _this->_impl_.key_.Set(from._internal_key(), _this->GetArenaForAllocation());
   }
-  _this->_impl_.action_ = from._impl_.action_;
+  ::memcpy(&_impl_.action_, &from._impl_.action_,
+    static_cast<::size_t>(reinterpret_cast<char*>(&_impl_.sequence_) -
+    reinterpret_cast<char*>(&_impl_.action_)) + sizeof(_impl_.sequence_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavCmd)
 }
 
 inline void NavCmd::SharedCtor(::_pb::Arena* arena) {
   (void)arena;
   new (&_impl_) Impl_{
-      decltype(_impl_.newactions_){arena}
+      decltype(_impl_._has_bits_){}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.newactions_){arena}
     , decltype(_impl_.key_) {}
 
     , decltype(_impl_.action_) { 0 }
 
-    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.sequence_) { 0 }
+
   };
   _impl_.key_.InitDefault();
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
@@ -3502,11 +3525,14 @@ void NavCmd::Clear() {
   _internal_mutable_newactions()->Clear();
   _impl_.key_.ClearToEmpty();
   _impl_.action_ = 0;
+  _impl_.sequence_ = 0;
+  _impl_._has_bits_.Clear();
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
 const char* NavCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
 #define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
   while (!ctx->Done(&ptr)) {
     ::uint32_t tag;
     ptr = ::_pbi::ReadTag(ptr, &tag);
@@ -3545,6 +3571,16 @@ const char* NavCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
           goto handle_unusual;
         }
         continue;
+      // optional int32 sequence = 6;
+      case 6:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 48)) {
+          _Internal::set_has_sequence(&has_bits);
+          _impl_.sequence_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
       default:
         goto handle_unusual;
     }  // switch
@@ -3561,6 +3597,7 @@ const char* NavCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
     CHK_(ptr != nullptr);
   }  // while
 message_done:
+  _impl_._has_bits_.Or(has_bits);
   return ptr;
 failure:
   ptr = nullptr;
@@ -3597,6 +3634,14 @@ failure:
         InternalWriteMessage(5, repfield, repfield.GetCachedSize(), target, stream);
   }
 
+  cached_has_bits = _impl_._has_bits_[0];
+  // optional int32 sequence = 6;
+  if (cached_has_bits & 0x00000001u) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        6, this->_internal_sequence(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -3632,6 +3677,13 @@ failure:
         this->_internal_action());
   }
 
+  // optional int32 sequence = 6;
+  cached_has_bits = _impl_._has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
+        this->_internal_sequence());
+  }
+
   return MaybeComputeUnknownFieldsSize(total_size, &_impl_._cached_size_);
 }
 
@@ -3657,6 +3709,9 @@ void NavCmd::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBU
   if (from._internal_action() != 0) {
     _this->_internal_set_action(from._internal_action());
   }
+  if ((from._impl_._has_bits_[0] & 0x00000001u) != 0) {
+    _this->_internal_set_sequence(from._internal_sequence());
+  }
   _this->_internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
 }
 
@@ -3676,11 +3731,16 @@ void NavCmd::InternalSwap(NavCmd* other) {
   auto* lhs_arena = GetArenaForAllocation();
   auto* rhs_arena = other->GetArenaForAllocation();
   _internal_metadata_.InternalSwap(&other->_internal_metadata_);
+  swap(_impl_._has_bits_[0], other->_impl_._has_bits_[0]);
   _internal_mutable_newactions()->InternalSwap(other->_internal_mutable_newactions());
   ::_pbi::ArenaStringPtr::InternalSwap(&_impl_.key_, lhs_arena,
                                        &other->_impl_.key_, rhs_arena);
-
-  swap(_impl_.action_, other->_impl_.action_);
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(NavCmd, _impl_.sequence_)
+      + sizeof(NavCmd::_impl_.sequence_)
+      - PROTOBUF_FIELD_OFFSET(NavCmd, _impl_.action_)>(
+          reinterpret_cast<char*>(&_impl_.action_),
+          reinterpret_cast<char*>(&other->_impl_.action_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavCmd::GetMetadata() const {

MPC/monitor/emqx/message.pb.h

@@ -1862,6 +1862,7 @@ class NavCmd final :
     kNewActionsFieldNumber = 5,
     kKeyFieldNumber = 2,
     kActionFieldNumber = 1,
+    kSequenceFieldNumber = 6,
   };
   // repeated .NavMessage.NewAction newActions = 5;
   int newactions_size() const;
@@ -1912,6 +1913,17 @@ class NavCmd final :
   ::int32_t _internal_action() const;
   void _internal_set_action(::int32_t value);
 
+  public:
+  // optional int32 sequence = 6;
+  bool has_sequence() const;
+  void clear_sequence() ;
+  ::int32_t sequence() const;
+  void set_sequence(::int32_t value);
+
+  private:
+  ::int32_t _internal_sequence() const;
+  void _internal_set_sequence(::int32_t value);
+
   public:
   // @@protoc_insertion_point(class_scope:NavMessage.NavCmd)
  private:
@@ -1921,10 +1933,12 @@ class NavCmd final :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   struct Impl_ {
+    ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
     ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::NavMessage::NewAction > newactions_;
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr key_;
     ::int32_t action_;
-    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+    ::int32_t sequence_;
   };
   union { Impl_ _impl_; };
   friend struct ::TableStruct_message_2eproto;
@@ -4181,6 +4195,31 @@ NavCmd::_internal_mutable_newactions() {
   return &_impl_.newactions_;
 }
 
+// optional int32 sequence = 6;
+inline bool NavCmd::has_sequence() const {
+  bool value = (_impl_._has_bits_[0] & 0x00000001u) != 0;
+  return value;
+}
+inline void NavCmd::clear_sequence() {
+  _impl_.sequence_ = 0;
+  _impl_._has_bits_[0] &= ~0x00000001u;
+}
+inline ::int32_t NavCmd::sequence() const {
+  // @@protoc_insertion_point(field_get:NavMessage.NavCmd.sequence)
+  return _internal_sequence();
+}
+inline void NavCmd::set_sequence(::int32_t value) {
+  _internal_set_sequence(value);
+  // @@protoc_insertion_point(field_set:NavMessage.NavCmd.sequence)
+}
+inline ::int32_t NavCmd::_internal_sequence() const {
+  return _impl_.sequence_;
+}
+inline void NavCmd::_internal_set_sequence(::int32_t value) {
+  _impl_._has_bits_[0] |= 0x00000001u;
+  _impl_.sequence_ = value;
+}
+
 // -------------------------------------------------------------------
 
 // NavResponse

MPC/navigation.cpp

@@ -1214,12 +1214,13 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         isInSpace_ = false;
 
         // 存车流程离开入口，掉头
+        printf("SequenceType: %d\n",GetSequenceType());
         if (GetSequenceType() == eSequencePark) {
             if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
                 Pose2d current_pose = timedPose_.Get();
                 double current_theta = current_pose.theta();
                 if (RotateAfterOutExport() != eMpcSuccess) {
-                    printf("Rotate before in entrance failed\n");
+                    printf("Rotate after out entrance failed\n");
                     return false;
                 }
             }