浏览代码

修改障碍物限制距离到2m

zx 2 年之前
父节点
当前提交
b9ba0c2147

+ 2 - 2
MPC/loaded_mpc.cpp

@@ -250,10 +250,10 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     constraints_upperbound[i] = max_acc_dlt;
   }
   // 与障碍物保持距离
-  double dobs=3.0;
+  double dobs=2.0;
   for(int i=nobs;i<nobs+N;++i)
   {
-    constraints_lowerbound[i] = 9;//pow(float(nobs+N-i)/float(N)*dobs,2);
+    constraints_lowerbound[i] = dobs*dobs;//pow(float(nobs+N-i)/float(N)*dobs,2);
     constraints_upperbound[i] = 1e19;
   }
   //限制最小转弯半径,

+ 2 - 2
MPC/monitor/emqx/paho_client.cpp

@@ -33,13 +33,13 @@ bool Paho_client::connect(std::string address,int port){
     return false;
   }
 
-  conn_opts_.keepAliveInterval = 20;
+  conn_opts_.keepAliveInterval = 500;
   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)
+  if (MQTTAsync_connect(client_, &conn_opts_) == MQTTASYNC_SUCCESS)
   {
     printf("Connected success \n");
   }

+ 6 - 2
MPC/navigation.cpp

@@ -265,24 +265,27 @@ void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
 
   if(cmd.action()==3)
   {
+    printf(" Nav cancel");
     navigator->Cancel();
     return ;
   }
   if(cmd.action()==2) {
+    printf(" Nav continue");
     navigator->pause_=false;
     return ;
   }
   if(cmd.action()==1) {
+    printf(" Nav pause");
     navigator->Pause();
     return ;
   }
-
   navigator->Start(cmd);
 }
 
 bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
                                     stLimit limit_v,stLimit limit_h,stLimit limit_rotate)
 {
+  std::cout<<" adjust tarhet:"<<target<<"  diff:"<<target_diff<<std::endl;
   if(inited_==false) {
     printf(" navigation has not inited\n");
     return false;
@@ -307,7 +310,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       printf("adjust completed\n");
       return true;
     }
-
+    
     //计算目标点在当前pose下的pose
     Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
 
@@ -402,6 +405,7 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
         if(fabs(velocity)<0.1 && fabs(angular)< 10*M_PI/180.0)
             return true;
     }
+  //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
     return false;
 }
 bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v)

+ 6 - 5
config/navigation.prototxt

@@ -1,16 +1,17 @@
 Agv_emqx
 {
-	NodeId:"agv1"
-	ip:"192.168.0.70"
+	NodeId:"agv"
+	ip:"127.0.0.1"
 	port:1883
-	pubTopic:"monitor/speedcmd"
-	subTopic:"monitor/statu"
+	pubSpeedTopic:"monitor/speedcmd"
+	subPoseTopic:"monitor/statu"
+	subSpeedTopic:"monitor/speed"
 }
 
 Terminal_emqx
 {
 	NodeId:"agv1-nav"
-	ip:"192.168.0.70"
+	ip:"127.0.0.1"
 	port:1883
 	pubStatuTopic:"agv1/agv_statu"
 	pubNavStatuTopic:"agv1/nav_statu"

+ 3 - 3
config/navigation_child.prototxt

@@ -1,7 +1,7 @@
 Agv_emqx
 {
-	NodeId:"agv1-child"
-	ip:"192.168.0.70"
+	NodeId:"agv-child"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
 	subPoseTopic:"monitor_child/statu"
@@ -11,7 +11,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv1-child-nav"
-	ip:"192.168.0.70"
+	ip:"127.0.0.1"
 	port:1883
 	pubStatuTopic:"agv1_child/agv_statu"
 	pubNavStatuTopic:"agv1_child/nav_statu"

+ 12 - 10
projects/controllers/AGV_controller/AGV_controller.cpp

@@ -17,7 +17,6 @@ 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;
@@ -94,7 +93,7 @@ void Vrtical(double velocity,double& R1,double& R2,double& R3,double& R4,
 
 int main(int argc, char **argv) {
   //init mqtt
-  if(false==init_mqtt("127.0.0.1",1883,"webots-AGV","monitor/speedcmd"))
+  if(false==init_mqtt("127.0.0.1",1883,"webots","monitor/speedcmd"))
   {
     printf(" Init mqtt failed...\n");
     return -1;
@@ -165,9 +164,9 @@ int main(int argc, char **argv) {
 
     ////发布:
     //增加高斯分布
-    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);
+    double x=gps->getValues()[2]+ generae_random(generator,0,0.007);
+    double y=gps->getValues()[0] + generae_random(generator,0,0.007);
+    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.1*M_PI/180.0);
 
     //获取gps速度,判断朝向
     double vm1=R3_l->getVelocity()*radius;
@@ -198,6 +197,12 @@ int main(int argc, char **argv) {
       statu.set_vth(vth);
       msg.fromProtoMessage(statu);
       client_->publish("monitor/statu",1,msg);
+
+      NavMessage::AGVSpeed speed;
+      speed.set_v(v);
+      speed.set_w(vth);
+      msg.fromProtoMessage(speed);
+      client_->publish("monitor/speed",1,msg);
     }
 
     if(g_speed.timeout())
@@ -234,7 +239,7 @@ int main(int argc, char **argv) {
     {
       Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else if(g_speed.Get().t()==3) {    //巡线/前进
+    else {    //巡线/前进
 
       if (fabs(cmd_vth) > 0.4) {
         cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;
@@ -291,14 +296,11 @@ 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",
+    printf("Child 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);

文件差异内容过多而无法显示
+ 739 - 43
projects/controllers/AGV_controller/emqx-client/message.pb.cc


+ 821 - 13
projects/controllers/AGV_controller/emqx-client/message.pb.h

@@ -47,7 +47,7 @@ struct TableStruct_message_2eproto {
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[7]
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[9]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
   static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
@@ -55,6 +55,9 @@ struct TableStruct_message_2eproto {
 };
 extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto;
 namespace NavMessage {
+class AGVSpeed;
+class AGVSpeedDefaultTypeInternal;
+extern AGVSpeedDefaultTypeInternal _AGVSpeed_default_instance_;
 class AGVStatu;
 class AGVStatuDefaultTypeInternal;
 extern AGVStatuDefaultTypeInternal _AGVStatu_default_instance_;
@@ -73,17 +76,22 @@ extern Pose2dDefaultTypeInternal _Pose2d_default_instance_;
 class Speed;
 class SpeedDefaultTypeInternal;
 extern SpeedDefaultTypeInternal _Speed_default_instance_;
+class SpeedLimit;
+class SpeedLimitDefaultTypeInternal;
+extern SpeedLimitDefaultTypeInternal _SpeedLimit_default_instance_;
 class Trajectory;
 class TrajectoryDefaultTypeInternal;
 extern TrajectoryDefaultTypeInternal _Trajectory_default_instance_;
 }  // namespace NavMessage
 PROTOBUF_NAMESPACE_OPEN
+template<> ::NavMessage::AGVSpeed* Arena::CreateMaybeMessage<::NavMessage::AGVSpeed>(Arena*);
 template<> ::NavMessage::AGVStatu* Arena::CreateMaybeMessage<::NavMessage::AGVStatu>(Arena*);
 template<> ::NavMessage::Action* Arena::CreateMaybeMessage<::NavMessage::Action>(Arena*);
 template<> ::NavMessage::NavCmd* Arena::CreateMaybeMessage<::NavMessage::NavCmd>(Arena*);
 template<> ::NavMessage::NavStatu* Arena::CreateMaybeMessage<::NavMessage::NavStatu>(Arena*);
 template<> ::NavMessage::Pose2d* Arena::CreateMaybeMessage<::NavMessage::Pose2d>(Arena*);
 template<> ::NavMessage::Speed* Arena::CreateMaybeMessage<::NavMessage::Speed>(Arena*);
+template<> ::NavMessage::SpeedLimit* Arena::CreateMaybeMessage<::NavMessage::SpeedLimit>(Arena*);
 template<> ::NavMessage::Trajectory* Arena::CreateMaybeMessage<::NavMessage::Trajectory>(Arena*);
 PROTOBUF_NAMESPACE_CLOSE
 namespace NavMessage {
@@ -271,6 +279,154 @@ class AGVStatu PROTOBUF_FINAL :
 };
 // -------------------------------------------------------------------
 
+class AGVSpeed PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.AGVSpeed) */ {
+ public:
+  inline AGVSpeed() : AGVSpeed(nullptr) {}
+  virtual ~AGVSpeed();
+
+  AGVSpeed(const AGVSpeed& from);
+  AGVSpeed(AGVSpeed&& from) noexcept
+    : AGVSpeed() {
+    *this = ::std::move(from);
+  }
+
+  inline AGVSpeed& operator=(const AGVSpeed& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline AGVSpeed& operator=(AGVSpeed&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const AGVSpeed& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const AGVSpeed* internal_default_instance() {
+    return reinterpret_cast<const AGVSpeed*>(
+               &_AGVSpeed_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    1;
+
+  friend void swap(AGVSpeed& a, AGVSpeed& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(AGVSpeed* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(AGVSpeed* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline AGVSpeed* New() const final {
+    return CreateMaybeMessage<AGVSpeed>(nullptr);
+  }
+
+  AGVSpeed* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<AGVSpeed>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const AGVSpeed& from);
+  void MergeFrom(const AGVSpeed& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(AGVSpeed* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "NavMessage.AGVSpeed";
+  }
+  protected:
+  explicit AGVSpeed(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_message_2eproto);
+    return ::descriptor_table_message_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kVFieldNumber = 1,
+    kWFieldNumber = 2,
+  };
+  // float v = 1;
+  void clear_v();
+  float v() const;
+  void set_v(float value);
+  private:
+  float _internal_v() const;
+  void _internal_set_v(float value);
+  public:
+
+  // float w = 2;
+  void clear_w();
+  float w() const;
+  void set_w(float value);
+  private:
+  float _internal_w() const;
+  void _internal_set_w(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:NavMessage.AGVSpeed)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  float v_;
+  float w_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  friend struct ::TableStruct_message_2eproto;
+};
+// -------------------------------------------------------------------
+
 class Speed PROTOBUF_FINAL :
     public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.Speed) */ {
  public:
@@ -313,7 +469,7 @@ class Speed PROTOBUF_FINAL :
                &_Speed_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    1;
+    2;
 
   friend void swap(Speed& a, Speed& b) {
     a.Swap(&b);
@@ -452,6 +608,154 @@ class Speed PROTOBUF_FINAL :
 };
 // -------------------------------------------------------------------
 
+class SpeedLimit PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.SpeedLimit) */ {
+ public:
+  inline SpeedLimit() : SpeedLimit(nullptr) {}
+  virtual ~SpeedLimit();
+
+  SpeedLimit(const SpeedLimit& from);
+  SpeedLimit(SpeedLimit&& from) noexcept
+    : SpeedLimit() {
+    *this = ::std::move(from);
+  }
+
+  inline SpeedLimit& operator=(const SpeedLimit& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline SpeedLimit& operator=(SpeedLimit&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const SpeedLimit& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const SpeedLimit* internal_default_instance() {
+    return reinterpret_cast<const SpeedLimit*>(
+               &_SpeedLimit_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    3;
+
+  friend void swap(SpeedLimit& a, SpeedLimit& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(SpeedLimit* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(SpeedLimit* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline SpeedLimit* New() const final {
+    return CreateMaybeMessage<SpeedLimit>(nullptr);
+  }
+
+  SpeedLimit* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<SpeedLimit>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const SpeedLimit& from);
+  void MergeFrom(const SpeedLimit& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(SpeedLimit* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "NavMessage.SpeedLimit";
+  }
+  protected:
+  explicit SpeedLimit(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_message_2eproto);
+    return ::descriptor_table_message_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kMinFieldNumber = 1,
+    kMaxFieldNumber = 2,
+  };
+  // float min = 1;
+  void clear_min();
+  float min() const;
+  void set_min(float value);
+  private:
+  float _internal_min() const;
+  void _internal_set_min(float value);
+  public:
+
+  // float max = 2;
+  void clear_max();
+  float max() const;
+  void set_max(float value);
+  private:
+  float _internal_max() const;
+  void _internal_set_max(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:NavMessage.SpeedLimit)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  float min_;
+  float max_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  friend struct ::TableStruct_message_2eproto;
+};
+// -------------------------------------------------------------------
+
 class Pose2d PROTOBUF_FINAL :
     public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.Pose2d) */ {
  public:
@@ -494,7 +798,7 @@ class Pose2d PROTOBUF_FINAL :
                &_Pose2d_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    2;
+    4;
 
   friend void swap(Pose2d& a, Pose2d& b) {
     a.Swap(&b);
@@ -653,7 +957,7 @@ class Trajectory PROTOBUF_FINAL :
                &_Trajectory_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    3;
+    5;
 
   friend void swap(Trajectory& a, Trajectory& b) {
     a.Swap(&b);
@@ -799,7 +1103,7 @@ class Action PROTOBUF_FINAL :
                &_Action_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    4;
+    6;
 
   friend void swap(Action& a, Action& b) {
     a.Swap(&b);
@@ -870,11 +1174,33 @@ class Action PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kTargetFieldNumber = 2,
-    kTargetDiffFieldNumber = 3,
+    kBeginFieldNumber = 2,
+    kTargetFieldNumber = 3,
+    kTargetDiffFieldNumber = 4,
+    kVelocityLimitFieldNumber = 5,
+    kAngularLimitFieldNumber = 6,
+    kHorizeLimitFieldNumber = 7,
     kTypeFieldNumber = 1,
   };
-  // .NavMessage.Pose2d target = 2;
+  // .NavMessage.Pose2d begin = 2;
+  bool has_begin() const;
+  private:
+  bool _internal_has_begin() const;
+  public:
+  void clear_begin();
+  const ::NavMessage::Pose2d& begin() const;
+  ::NavMessage::Pose2d* release_begin();
+  ::NavMessage::Pose2d* mutable_begin();
+  void set_allocated_begin(::NavMessage::Pose2d* begin);
+  private:
+  const ::NavMessage::Pose2d& _internal_begin() const;
+  ::NavMessage::Pose2d* _internal_mutable_begin();
+  public:
+  void unsafe_arena_set_allocated_begin(
+      ::NavMessage::Pose2d* begin);
+  ::NavMessage::Pose2d* unsafe_arena_release_begin();
+
+  // .NavMessage.Pose2d target = 3;
   bool has_target() const;
   private:
   bool _internal_has_target() const;
@@ -892,7 +1218,7 @@ class Action PROTOBUF_FINAL :
       ::NavMessage::Pose2d* target);
   ::NavMessage::Pose2d* unsafe_arena_release_target();
 
-  // .NavMessage.Pose2d target_diff = 3;
+  // .NavMessage.Pose2d target_diff = 4;
   bool has_target_diff() const;
   private:
   bool _internal_has_target_diff() const;
@@ -910,6 +1236,60 @@ class Action PROTOBUF_FINAL :
       ::NavMessage::Pose2d* target_diff);
   ::NavMessage::Pose2d* unsafe_arena_release_target_diff();
 
+  // .NavMessage.SpeedLimit velocity_limit = 5;
+  bool has_velocity_limit() const;
+  private:
+  bool _internal_has_velocity_limit() const;
+  public:
+  void clear_velocity_limit();
+  const ::NavMessage::SpeedLimit& velocity_limit() const;
+  ::NavMessage::SpeedLimit* release_velocity_limit();
+  ::NavMessage::SpeedLimit* mutable_velocity_limit();
+  void set_allocated_velocity_limit(::NavMessage::SpeedLimit* velocity_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_velocity_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_velocity_limit();
+  public:
+  void unsafe_arena_set_allocated_velocity_limit(
+      ::NavMessage::SpeedLimit* velocity_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_velocity_limit();
+
+  // .NavMessage.SpeedLimit angular_limit = 6;
+  bool has_angular_limit() const;
+  private:
+  bool _internal_has_angular_limit() const;
+  public:
+  void clear_angular_limit();
+  const ::NavMessage::SpeedLimit& angular_limit() const;
+  ::NavMessage::SpeedLimit* release_angular_limit();
+  ::NavMessage::SpeedLimit* mutable_angular_limit();
+  void set_allocated_angular_limit(::NavMessage::SpeedLimit* angular_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_angular_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_angular_limit();
+  public:
+  void unsafe_arena_set_allocated_angular_limit(
+      ::NavMessage::SpeedLimit* angular_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_angular_limit();
+
+  // .NavMessage.SpeedLimit horize_limit = 7;
+  bool has_horize_limit() const;
+  private:
+  bool _internal_has_horize_limit() const;
+  public:
+  void clear_horize_limit();
+  const ::NavMessage::SpeedLimit& horize_limit() const;
+  ::NavMessage::SpeedLimit* release_horize_limit();
+  ::NavMessage::SpeedLimit* mutable_horize_limit();
+  void set_allocated_horize_limit(::NavMessage::SpeedLimit* horize_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_horize_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_horize_limit();
+  public:
+  void unsafe_arena_set_allocated_horize_limit(
+      ::NavMessage::SpeedLimit* horize_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_horize_limit();
+
   // int32 type = 1;
   void clear_type();
   ::PROTOBUF_NAMESPACE_ID::int32 type() const;
@@ -926,8 +1306,12 @@ class Action PROTOBUF_FINAL :
   template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
+  ::NavMessage::Pose2d* begin_;
   ::NavMessage::Pose2d* target_;
   ::NavMessage::Pose2d* target_diff_;
+  ::NavMessage::SpeedLimit* velocity_limit_;
+  ::NavMessage::SpeedLimit* angular_limit_;
+  ::NavMessage::SpeedLimit* horize_limit_;
   ::PROTOBUF_NAMESPACE_ID::int32 type_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
@@ -976,7 +1360,7 @@ class NavCmd PROTOBUF_FINAL :
                &_NavCmd_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    5;
+    7;
 
   friend void swap(NavCmd& a, NavCmd& b) {
     a.Swap(&b);
@@ -1151,7 +1535,7 @@ class NavStatu PROTOBUF_FINAL :
                &_NavStatu_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    6;
+    8;
 
   friend void swap(NavStatu& a, NavStatu& b) {
     a.Swap(&b);
@@ -1435,6 +1819,50 @@ inline void AGVStatu::set_vth(float value) {
 
 // -------------------------------------------------------------------
 
+// AGVSpeed
+
+// float v = 1;
+inline void AGVSpeed::clear_v() {
+  v_ = 0;
+}
+inline float AGVSpeed::_internal_v() const {
+  return v_;
+}
+inline float AGVSpeed::v() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AGVSpeed.v)
+  return _internal_v();
+}
+inline void AGVSpeed::_internal_set_v(float value) {
+  
+  v_ = value;
+}
+inline void AGVSpeed::set_v(float value) {
+  _internal_set_v(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AGVSpeed.v)
+}
+
+// float w = 2;
+inline void AGVSpeed::clear_w() {
+  w_ = 0;
+}
+inline float AGVSpeed::_internal_w() const {
+  return w_;
+}
+inline float AGVSpeed::w() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AGVSpeed.w)
+  return _internal_w();
+}
+inline void AGVSpeed::_internal_set_w(float value) {
+  
+  w_ = value;
+}
+inline void AGVSpeed::set_w(float value) {
+  _internal_set_w(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AGVSpeed.w)
+}
+
+// -------------------------------------------------------------------
+
 // Speed
 
 // int32 H = 1;
@@ -1539,6 +1967,50 @@ inline void Speed::set_end(::PROTOBUF_NAMESPACE_ID::int32 value) {
 
 // -------------------------------------------------------------------
 
+// SpeedLimit
+
+// float min = 1;
+inline void SpeedLimit::clear_min() {
+  min_ = 0;
+}
+inline float SpeedLimit::_internal_min() const {
+  return min_;
+}
+inline float SpeedLimit::min() const {
+  // @@protoc_insertion_point(field_get:NavMessage.SpeedLimit.min)
+  return _internal_min();
+}
+inline void SpeedLimit::_internal_set_min(float value) {
+  
+  min_ = value;
+}
+inline void SpeedLimit::set_min(float value) {
+  _internal_set_min(value);
+  // @@protoc_insertion_point(field_set:NavMessage.SpeedLimit.min)
+}
+
+// float max = 2;
+inline void SpeedLimit::clear_max() {
+  max_ = 0;
+}
+inline float SpeedLimit::_internal_max() const {
+  return max_;
+}
+inline float SpeedLimit::max() const {
+  // @@protoc_insertion_point(field_get:NavMessage.SpeedLimit.max)
+  return _internal_max();
+}
+inline void SpeedLimit::_internal_set_max(float value) {
+  
+  max_ = value;
+}
+inline void SpeedLimit::set_max(float value) {
+  _internal_set_max(value);
+  // @@protoc_insertion_point(field_set:NavMessage.SpeedLimit.max)
+}
+
+// -------------------------------------------------------------------
+
 // Pose2d
 
 // float x = 1;
@@ -1668,7 +2140,90 @@ inline void Action::set_type(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Action.type)
 }
 
-// .NavMessage.Pose2d target = 2;
+// .NavMessage.Pose2d begin = 2;
+inline bool Action::_internal_has_begin() const {
+  return this != internal_default_instance() && begin_ != nullptr;
+}
+inline bool Action::has_begin() const {
+  return _internal_has_begin();
+}
+inline void Action::clear_begin() {
+  if (GetArena() == nullptr && begin_ != nullptr) {
+    delete begin_;
+  }
+  begin_ = nullptr;
+}
+inline const ::NavMessage::Pose2d& Action::_internal_begin() const {
+  const ::NavMessage::Pose2d* p = begin_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::Pose2d*>(
+      &::NavMessage::_Pose2d_default_instance_);
+}
+inline const ::NavMessage::Pose2d& Action::begin() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.begin)
+  return _internal_begin();
+}
+inline void Action::unsafe_arena_set_allocated_begin(
+    ::NavMessage::Pose2d* begin) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(begin_);
+  }
+  begin_ = begin;
+  if (begin) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.begin)
+}
+inline ::NavMessage::Pose2d* Action::release_begin() {
+  
+  ::NavMessage::Pose2d* temp = begin_;
+  begin_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::Pose2d* Action::unsafe_arena_release_begin() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.begin)
+  
+  ::NavMessage::Pose2d* temp = begin_;
+  begin_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::Pose2d* Action::_internal_mutable_begin() {
+  
+  if (begin_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::Pose2d>(GetArena());
+    begin_ = p;
+  }
+  return begin_;
+}
+inline ::NavMessage::Pose2d* Action::mutable_begin() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.begin)
+  return _internal_mutable_begin();
+}
+inline void Action::set_allocated_begin(::NavMessage::Pose2d* begin) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete begin_;
+  }
+  if (begin) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(begin);
+    if (message_arena != submessage_arena) {
+      begin = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, begin, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  begin_ = begin;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.begin)
+}
+
+// .NavMessage.Pose2d target = 3;
 inline bool Action::_internal_has_target() const {
   return this != internal_default_instance() && target_ != nullptr;
 }
@@ -1751,7 +2306,7 @@ inline void Action::set_allocated_target(::NavMessage::Pose2d* target) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.target)
 }
 
-// .NavMessage.Pose2d target_diff = 3;
+// .NavMessage.Pose2d target_diff = 4;
 inline bool Action::_internal_has_target_diff() const {
   return this != internal_default_instance() && target_diff_ != nullptr;
 }
@@ -1834,6 +2389,255 @@ inline void Action::set_allocated_target_diff(::NavMessage::Pose2d* target_diff)
   // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.target_diff)
 }
 
+// .NavMessage.SpeedLimit velocity_limit = 5;
+inline bool Action::_internal_has_velocity_limit() const {
+  return this != internal_default_instance() && velocity_limit_ != nullptr;
+}
+inline bool Action::has_velocity_limit() const {
+  return _internal_has_velocity_limit();
+}
+inline void Action::clear_velocity_limit() {
+  if (GetArena() == nullptr && velocity_limit_ != nullptr) {
+    delete velocity_limit_;
+  }
+  velocity_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_velocity_limit() const {
+  const ::NavMessage::SpeedLimit* p = velocity_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::velocity_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.velocity_limit)
+  return _internal_velocity_limit();
+}
+inline void Action::unsafe_arena_set_allocated_velocity_limit(
+    ::NavMessage::SpeedLimit* velocity_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(velocity_limit_);
+  }
+  velocity_limit_ = velocity_limit;
+  if (velocity_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.velocity_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_velocity_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = velocity_limit_;
+  velocity_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_velocity_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.velocity_limit)
+  
+  ::NavMessage::SpeedLimit* temp = velocity_limit_;
+  velocity_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_velocity_limit() {
+  
+  if (velocity_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    velocity_limit_ = p;
+  }
+  return velocity_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_velocity_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.velocity_limit)
+  return _internal_mutable_velocity_limit();
+}
+inline void Action::set_allocated_velocity_limit(::NavMessage::SpeedLimit* velocity_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete velocity_limit_;
+  }
+  if (velocity_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(velocity_limit);
+    if (message_arena != submessage_arena) {
+      velocity_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, velocity_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  velocity_limit_ = velocity_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.velocity_limit)
+}
+
+// .NavMessage.SpeedLimit angular_limit = 6;
+inline bool Action::_internal_has_angular_limit() const {
+  return this != internal_default_instance() && angular_limit_ != nullptr;
+}
+inline bool Action::has_angular_limit() const {
+  return _internal_has_angular_limit();
+}
+inline void Action::clear_angular_limit() {
+  if (GetArena() == nullptr && angular_limit_ != nullptr) {
+    delete angular_limit_;
+  }
+  angular_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_angular_limit() const {
+  const ::NavMessage::SpeedLimit* p = angular_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::angular_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.angular_limit)
+  return _internal_angular_limit();
+}
+inline void Action::unsafe_arena_set_allocated_angular_limit(
+    ::NavMessage::SpeedLimit* angular_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(angular_limit_);
+  }
+  angular_limit_ = angular_limit;
+  if (angular_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.angular_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_angular_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = angular_limit_;
+  angular_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_angular_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.angular_limit)
+  
+  ::NavMessage::SpeedLimit* temp = angular_limit_;
+  angular_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_angular_limit() {
+  
+  if (angular_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    angular_limit_ = p;
+  }
+  return angular_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_angular_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.angular_limit)
+  return _internal_mutable_angular_limit();
+}
+inline void Action::set_allocated_angular_limit(::NavMessage::SpeedLimit* angular_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete angular_limit_;
+  }
+  if (angular_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(angular_limit);
+    if (message_arena != submessage_arena) {
+      angular_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, angular_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  angular_limit_ = angular_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.angular_limit)
+}
+
+// .NavMessage.SpeedLimit horize_limit = 7;
+inline bool Action::_internal_has_horize_limit() const {
+  return this != internal_default_instance() && horize_limit_ != nullptr;
+}
+inline bool Action::has_horize_limit() const {
+  return _internal_has_horize_limit();
+}
+inline void Action::clear_horize_limit() {
+  if (GetArena() == nullptr && horize_limit_ != nullptr) {
+    delete horize_limit_;
+  }
+  horize_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_horize_limit() const {
+  const ::NavMessage::SpeedLimit* p = horize_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::horize_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.horize_limit)
+  return _internal_horize_limit();
+}
+inline void Action::unsafe_arena_set_allocated_horize_limit(
+    ::NavMessage::SpeedLimit* horize_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(horize_limit_);
+  }
+  horize_limit_ = horize_limit;
+  if (horize_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.horize_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_horize_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = horize_limit_;
+  horize_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_horize_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.horize_limit)
+  
+  ::NavMessage::SpeedLimit* temp = horize_limit_;
+  horize_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_horize_limit() {
+  
+  if (horize_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    horize_limit_ = p;
+  }
+  return horize_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_horize_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.horize_limit)
+  return _internal_mutable_horize_limit();
+}
+inline void Action::set_allocated_horize_limit(::NavMessage::SpeedLimit* horize_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete horize_limit_;
+  }
+  if (horize_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(horize_limit);
+    if (message_arena != submessage_arena) {
+      horize_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, horize_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  horize_limit_ = horize_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.horize_limit)
+}
+
 // -------------------------------------------------------------------
 
 // NavCmd
@@ -2265,6 +3069,10 @@ inline void NavStatu::set_allocated_predict_traj(::NavMessage::Trajectory* predi
 
 // -------------------------------------------------------------------
 
+// -------------------------------------------------------------------
+
+// -------------------------------------------------------------------
+
 
 // @@protoc_insertion_point(namespace_scope)
 

+ 1 - 3
projects/controllers/AGV_controller/emqx-client/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);

+ 1 - 1
projects/controllers/AGV_controller/emqx-client/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);

+ 9 - 4
projects/controllers/agv_child/AGV_controller.cpp

@@ -17,7 +17,6 @@ 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;
@@ -165,9 +164,9 @@ int main(int argc, char **argv) {
 
     ////发布:
     //增加高斯分布
-    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);
+    double x=gps->getValues()[2]+ generae_random(generator,0,0.007);
+    double y=gps->getValues()[0] + generae_random(generator,0,0.007);
+    double theta=imu->getRollPitchYaw()[2]+generae_random(generator,0,0.1*M_PI/180.0);
 
     //获取gps速度,判断朝向
     double vm1=R3_l->getVelocity()*radius;
@@ -198,6 +197,12 @@ int main(int argc, char **argv) {
       statu.set_vth(vth);
       msg.fromProtoMessage(statu);
       client_->publish("monitor_child/statu",1,msg);
+
+      NavMessage::AGVSpeed speed;
+      speed.set_v(v);
+      speed.set_w(vth);
+      msg.fromProtoMessage(speed);
+      client_->publish("monitor_child/speed",1,msg);
     }
 
     if(g_speed.timeout())

文件差异内容过多而无法显示
+ 739 - 43
projects/controllers/agv_child/emqx-client/message.pb.cc


+ 821 - 13
projects/controllers/agv_child/emqx-client/message.pb.h

@@ -47,7 +47,7 @@ struct TableStruct_message_2eproto {
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[7]
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[9]
     PROTOBUF_SECTION_VARIABLE(protodesc_cold);
   static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
   static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
@@ -55,6 +55,9 @@ struct TableStruct_message_2eproto {
 };
 extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto;
 namespace NavMessage {
+class AGVSpeed;
+class AGVSpeedDefaultTypeInternal;
+extern AGVSpeedDefaultTypeInternal _AGVSpeed_default_instance_;
 class AGVStatu;
 class AGVStatuDefaultTypeInternal;
 extern AGVStatuDefaultTypeInternal _AGVStatu_default_instance_;
@@ -73,17 +76,22 @@ extern Pose2dDefaultTypeInternal _Pose2d_default_instance_;
 class Speed;
 class SpeedDefaultTypeInternal;
 extern SpeedDefaultTypeInternal _Speed_default_instance_;
+class SpeedLimit;
+class SpeedLimitDefaultTypeInternal;
+extern SpeedLimitDefaultTypeInternal _SpeedLimit_default_instance_;
 class Trajectory;
 class TrajectoryDefaultTypeInternal;
 extern TrajectoryDefaultTypeInternal _Trajectory_default_instance_;
 }  // namespace NavMessage
 PROTOBUF_NAMESPACE_OPEN
+template<> ::NavMessage::AGVSpeed* Arena::CreateMaybeMessage<::NavMessage::AGVSpeed>(Arena*);
 template<> ::NavMessage::AGVStatu* Arena::CreateMaybeMessage<::NavMessage::AGVStatu>(Arena*);
 template<> ::NavMessage::Action* Arena::CreateMaybeMessage<::NavMessage::Action>(Arena*);
 template<> ::NavMessage::NavCmd* Arena::CreateMaybeMessage<::NavMessage::NavCmd>(Arena*);
 template<> ::NavMessage::NavStatu* Arena::CreateMaybeMessage<::NavMessage::NavStatu>(Arena*);
 template<> ::NavMessage::Pose2d* Arena::CreateMaybeMessage<::NavMessage::Pose2d>(Arena*);
 template<> ::NavMessage::Speed* Arena::CreateMaybeMessage<::NavMessage::Speed>(Arena*);
+template<> ::NavMessage::SpeedLimit* Arena::CreateMaybeMessage<::NavMessage::SpeedLimit>(Arena*);
 template<> ::NavMessage::Trajectory* Arena::CreateMaybeMessage<::NavMessage::Trajectory>(Arena*);
 PROTOBUF_NAMESPACE_CLOSE
 namespace NavMessage {
@@ -271,6 +279,154 @@ class AGVStatu PROTOBUF_FINAL :
 };
 // -------------------------------------------------------------------
 
+class AGVSpeed PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.AGVSpeed) */ {
+ public:
+  inline AGVSpeed() : AGVSpeed(nullptr) {}
+  virtual ~AGVSpeed();
+
+  AGVSpeed(const AGVSpeed& from);
+  AGVSpeed(AGVSpeed&& from) noexcept
+    : AGVSpeed() {
+    *this = ::std::move(from);
+  }
+
+  inline AGVSpeed& operator=(const AGVSpeed& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline AGVSpeed& operator=(AGVSpeed&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const AGVSpeed& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const AGVSpeed* internal_default_instance() {
+    return reinterpret_cast<const AGVSpeed*>(
+               &_AGVSpeed_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    1;
+
+  friend void swap(AGVSpeed& a, AGVSpeed& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(AGVSpeed* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(AGVSpeed* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline AGVSpeed* New() const final {
+    return CreateMaybeMessage<AGVSpeed>(nullptr);
+  }
+
+  AGVSpeed* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<AGVSpeed>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const AGVSpeed& from);
+  void MergeFrom(const AGVSpeed& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(AGVSpeed* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "NavMessage.AGVSpeed";
+  }
+  protected:
+  explicit AGVSpeed(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_message_2eproto);
+    return ::descriptor_table_message_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kVFieldNumber = 1,
+    kWFieldNumber = 2,
+  };
+  // float v = 1;
+  void clear_v();
+  float v() const;
+  void set_v(float value);
+  private:
+  float _internal_v() const;
+  void _internal_set_v(float value);
+  public:
+
+  // float w = 2;
+  void clear_w();
+  float w() const;
+  void set_w(float value);
+  private:
+  float _internal_w() const;
+  void _internal_set_w(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:NavMessage.AGVSpeed)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  float v_;
+  float w_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  friend struct ::TableStruct_message_2eproto;
+};
+// -------------------------------------------------------------------
+
 class Speed PROTOBUF_FINAL :
     public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.Speed) */ {
  public:
@@ -313,7 +469,7 @@ class Speed PROTOBUF_FINAL :
                &_Speed_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    1;
+    2;
 
   friend void swap(Speed& a, Speed& b) {
     a.Swap(&b);
@@ -452,6 +608,154 @@ class Speed PROTOBUF_FINAL :
 };
 // -------------------------------------------------------------------
 
+class SpeedLimit PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.SpeedLimit) */ {
+ public:
+  inline SpeedLimit() : SpeedLimit(nullptr) {}
+  virtual ~SpeedLimit();
+
+  SpeedLimit(const SpeedLimit& from);
+  SpeedLimit(SpeedLimit&& from) noexcept
+    : SpeedLimit() {
+    *this = ::std::move(from);
+  }
+
+  inline SpeedLimit& operator=(const SpeedLimit& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline SpeedLimit& operator=(SpeedLimit&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const SpeedLimit& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const SpeedLimit* internal_default_instance() {
+    return reinterpret_cast<const SpeedLimit*>(
+               &_SpeedLimit_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    3;
+
+  friend void swap(SpeedLimit& a, SpeedLimit& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(SpeedLimit* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(SpeedLimit* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline SpeedLimit* New() const final {
+    return CreateMaybeMessage<SpeedLimit>(nullptr);
+  }
+
+  SpeedLimit* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<SpeedLimit>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const SpeedLimit& from);
+  void MergeFrom(const SpeedLimit& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(SpeedLimit* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "NavMessage.SpeedLimit";
+  }
+  protected:
+  explicit SpeedLimit(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_message_2eproto);
+    return ::descriptor_table_message_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kMinFieldNumber = 1,
+    kMaxFieldNumber = 2,
+  };
+  // float min = 1;
+  void clear_min();
+  float min() const;
+  void set_min(float value);
+  private:
+  float _internal_min() const;
+  void _internal_set_min(float value);
+  public:
+
+  // float max = 2;
+  void clear_max();
+  float max() const;
+  void set_max(float value);
+  private:
+  float _internal_max() const;
+  void _internal_set_max(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:NavMessage.SpeedLimit)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  float min_;
+  float max_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  friend struct ::TableStruct_message_2eproto;
+};
+// -------------------------------------------------------------------
+
 class Pose2d PROTOBUF_FINAL :
     public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavMessage.Pose2d) */ {
  public:
@@ -494,7 +798,7 @@ class Pose2d PROTOBUF_FINAL :
                &_Pose2d_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    2;
+    4;
 
   friend void swap(Pose2d& a, Pose2d& b) {
     a.Swap(&b);
@@ -653,7 +957,7 @@ class Trajectory PROTOBUF_FINAL :
                &_Trajectory_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    3;
+    5;
 
   friend void swap(Trajectory& a, Trajectory& b) {
     a.Swap(&b);
@@ -799,7 +1103,7 @@ class Action PROTOBUF_FINAL :
                &_Action_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    4;
+    6;
 
   friend void swap(Action& a, Action& b) {
     a.Swap(&b);
@@ -870,11 +1174,33 @@ class Action PROTOBUF_FINAL :
   // accessors -------------------------------------------------------
 
   enum : int {
-    kTargetFieldNumber = 2,
-    kTargetDiffFieldNumber = 3,
+    kBeginFieldNumber = 2,
+    kTargetFieldNumber = 3,
+    kTargetDiffFieldNumber = 4,
+    kVelocityLimitFieldNumber = 5,
+    kAngularLimitFieldNumber = 6,
+    kHorizeLimitFieldNumber = 7,
     kTypeFieldNumber = 1,
   };
-  // .NavMessage.Pose2d target = 2;
+  // .NavMessage.Pose2d begin = 2;
+  bool has_begin() const;
+  private:
+  bool _internal_has_begin() const;
+  public:
+  void clear_begin();
+  const ::NavMessage::Pose2d& begin() const;
+  ::NavMessage::Pose2d* release_begin();
+  ::NavMessage::Pose2d* mutable_begin();
+  void set_allocated_begin(::NavMessage::Pose2d* begin);
+  private:
+  const ::NavMessage::Pose2d& _internal_begin() const;
+  ::NavMessage::Pose2d* _internal_mutable_begin();
+  public:
+  void unsafe_arena_set_allocated_begin(
+      ::NavMessage::Pose2d* begin);
+  ::NavMessage::Pose2d* unsafe_arena_release_begin();
+
+  // .NavMessage.Pose2d target = 3;
   bool has_target() const;
   private:
   bool _internal_has_target() const;
@@ -892,7 +1218,7 @@ class Action PROTOBUF_FINAL :
       ::NavMessage::Pose2d* target);
   ::NavMessage::Pose2d* unsafe_arena_release_target();
 
-  // .NavMessage.Pose2d target_diff = 3;
+  // .NavMessage.Pose2d target_diff = 4;
   bool has_target_diff() const;
   private:
   bool _internal_has_target_diff() const;
@@ -910,6 +1236,60 @@ class Action PROTOBUF_FINAL :
       ::NavMessage::Pose2d* target_diff);
   ::NavMessage::Pose2d* unsafe_arena_release_target_diff();
 
+  // .NavMessage.SpeedLimit velocity_limit = 5;
+  bool has_velocity_limit() const;
+  private:
+  bool _internal_has_velocity_limit() const;
+  public:
+  void clear_velocity_limit();
+  const ::NavMessage::SpeedLimit& velocity_limit() const;
+  ::NavMessage::SpeedLimit* release_velocity_limit();
+  ::NavMessage::SpeedLimit* mutable_velocity_limit();
+  void set_allocated_velocity_limit(::NavMessage::SpeedLimit* velocity_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_velocity_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_velocity_limit();
+  public:
+  void unsafe_arena_set_allocated_velocity_limit(
+      ::NavMessage::SpeedLimit* velocity_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_velocity_limit();
+
+  // .NavMessage.SpeedLimit angular_limit = 6;
+  bool has_angular_limit() const;
+  private:
+  bool _internal_has_angular_limit() const;
+  public:
+  void clear_angular_limit();
+  const ::NavMessage::SpeedLimit& angular_limit() const;
+  ::NavMessage::SpeedLimit* release_angular_limit();
+  ::NavMessage::SpeedLimit* mutable_angular_limit();
+  void set_allocated_angular_limit(::NavMessage::SpeedLimit* angular_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_angular_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_angular_limit();
+  public:
+  void unsafe_arena_set_allocated_angular_limit(
+      ::NavMessage::SpeedLimit* angular_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_angular_limit();
+
+  // .NavMessage.SpeedLimit horize_limit = 7;
+  bool has_horize_limit() const;
+  private:
+  bool _internal_has_horize_limit() const;
+  public:
+  void clear_horize_limit();
+  const ::NavMessage::SpeedLimit& horize_limit() const;
+  ::NavMessage::SpeedLimit* release_horize_limit();
+  ::NavMessage::SpeedLimit* mutable_horize_limit();
+  void set_allocated_horize_limit(::NavMessage::SpeedLimit* horize_limit);
+  private:
+  const ::NavMessage::SpeedLimit& _internal_horize_limit() const;
+  ::NavMessage::SpeedLimit* _internal_mutable_horize_limit();
+  public:
+  void unsafe_arena_set_allocated_horize_limit(
+      ::NavMessage::SpeedLimit* horize_limit);
+  ::NavMessage::SpeedLimit* unsafe_arena_release_horize_limit();
+
   // int32 type = 1;
   void clear_type();
   ::PROTOBUF_NAMESPACE_ID::int32 type() const;
@@ -926,8 +1306,12 @@ class Action PROTOBUF_FINAL :
   template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
+  ::NavMessage::Pose2d* begin_;
   ::NavMessage::Pose2d* target_;
   ::NavMessage::Pose2d* target_diff_;
+  ::NavMessage::SpeedLimit* velocity_limit_;
+  ::NavMessage::SpeedLimit* angular_limit_;
+  ::NavMessage::SpeedLimit* horize_limit_;
   ::PROTOBUF_NAMESPACE_ID::int32 type_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
@@ -976,7 +1360,7 @@ class NavCmd PROTOBUF_FINAL :
                &_NavCmd_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    5;
+    7;
 
   friend void swap(NavCmd& a, NavCmd& b) {
     a.Swap(&b);
@@ -1151,7 +1535,7 @@ class NavStatu PROTOBUF_FINAL :
                &_NavStatu_default_instance_);
   }
   static constexpr int kIndexInFileMessages =
-    6;
+    8;
 
   friend void swap(NavStatu& a, NavStatu& b) {
     a.Swap(&b);
@@ -1435,6 +1819,50 @@ inline void AGVStatu::set_vth(float value) {
 
 // -------------------------------------------------------------------
 
+// AGVSpeed
+
+// float v = 1;
+inline void AGVSpeed::clear_v() {
+  v_ = 0;
+}
+inline float AGVSpeed::_internal_v() const {
+  return v_;
+}
+inline float AGVSpeed::v() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AGVSpeed.v)
+  return _internal_v();
+}
+inline void AGVSpeed::_internal_set_v(float value) {
+  
+  v_ = value;
+}
+inline void AGVSpeed::set_v(float value) {
+  _internal_set_v(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AGVSpeed.v)
+}
+
+// float w = 2;
+inline void AGVSpeed::clear_w() {
+  w_ = 0;
+}
+inline float AGVSpeed::_internal_w() const {
+  return w_;
+}
+inline float AGVSpeed::w() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AGVSpeed.w)
+  return _internal_w();
+}
+inline void AGVSpeed::_internal_set_w(float value) {
+  
+  w_ = value;
+}
+inline void AGVSpeed::set_w(float value) {
+  _internal_set_w(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AGVSpeed.w)
+}
+
+// -------------------------------------------------------------------
+
 // Speed
 
 // int32 H = 1;
@@ -1539,6 +1967,50 @@ inline void Speed::set_end(::PROTOBUF_NAMESPACE_ID::int32 value) {
 
 // -------------------------------------------------------------------
 
+// SpeedLimit
+
+// float min = 1;
+inline void SpeedLimit::clear_min() {
+  min_ = 0;
+}
+inline float SpeedLimit::_internal_min() const {
+  return min_;
+}
+inline float SpeedLimit::min() const {
+  // @@protoc_insertion_point(field_get:NavMessage.SpeedLimit.min)
+  return _internal_min();
+}
+inline void SpeedLimit::_internal_set_min(float value) {
+  
+  min_ = value;
+}
+inline void SpeedLimit::set_min(float value) {
+  _internal_set_min(value);
+  // @@protoc_insertion_point(field_set:NavMessage.SpeedLimit.min)
+}
+
+// float max = 2;
+inline void SpeedLimit::clear_max() {
+  max_ = 0;
+}
+inline float SpeedLimit::_internal_max() const {
+  return max_;
+}
+inline float SpeedLimit::max() const {
+  // @@protoc_insertion_point(field_get:NavMessage.SpeedLimit.max)
+  return _internal_max();
+}
+inline void SpeedLimit::_internal_set_max(float value) {
+  
+  max_ = value;
+}
+inline void SpeedLimit::set_max(float value) {
+  _internal_set_max(value);
+  // @@protoc_insertion_point(field_set:NavMessage.SpeedLimit.max)
+}
+
+// -------------------------------------------------------------------
+
 // Pose2d
 
 // float x = 1;
@@ -1668,7 +2140,90 @@ inline void Action::set_type(::PROTOBUF_NAMESPACE_ID::int32 value) {
   // @@protoc_insertion_point(field_set:NavMessage.Action.type)
 }
 
-// .NavMessage.Pose2d target = 2;
+// .NavMessage.Pose2d begin = 2;
+inline bool Action::_internal_has_begin() const {
+  return this != internal_default_instance() && begin_ != nullptr;
+}
+inline bool Action::has_begin() const {
+  return _internal_has_begin();
+}
+inline void Action::clear_begin() {
+  if (GetArena() == nullptr && begin_ != nullptr) {
+    delete begin_;
+  }
+  begin_ = nullptr;
+}
+inline const ::NavMessage::Pose2d& Action::_internal_begin() const {
+  const ::NavMessage::Pose2d* p = begin_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::Pose2d*>(
+      &::NavMessage::_Pose2d_default_instance_);
+}
+inline const ::NavMessage::Pose2d& Action::begin() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.begin)
+  return _internal_begin();
+}
+inline void Action::unsafe_arena_set_allocated_begin(
+    ::NavMessage::Pose2d* begin) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(begin_);
+  }
+  begin_ = begin;
+  if (begin) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.begin)
+}
+inline ::NavMessage::Pose2d* Action::release_begin() {
+  
+  ::NavMessage::Pose2d* temp = begin_;
+  begin_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::Pose2d* Action::unsafe_arena_release_begin() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.begin)
+  
+  ::NavMessage::Pose2d* temp = begin_;
+  begin_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::Pose2d* Action::_internal_mutable_begin() {
+  
+  if (begin_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::Pose2d>(GetArena());
+    begin_ = p;
+  }
+  return begin_;
+}
+inline ::NavMessage::Pose2d* Action::mutable_begin() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.begin)
+  return _internal_mutable_begin();
+}
+inline void Action::set_allocated_begin(::NavMessage::Pose2d* begin) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete begin_;
+  }
+  if (begin) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(begin);
+    if (message_arena != submessage_arena) {
+      begin = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, begin, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  begin_ = begin;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.begin)
+}
+
+// .NavMessage.Pose2d target = 3;
 inline bool Action::_internal_has_target() const {
   return this != internal_default_instance() && target_ != nullptr;
 }
@@ -1751,7 +2306,7 @@ inline void Action::set_allocated_target(::NavMessage::Pose2d* target) {
   // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.target)
 }
 
-// .NavMessage.Pose2d target_diff = 3;
+// .NavMessage.Pose2d target_diff = 4;
 inline bool Action::_internal_has_target_diff() const {
   return this != internal_default_instance() && target_diff_ != nullptr;
 }
@@ -1834,6 +2389,255 @@ inline void Action::set_allocated_target_diff(::NavMessage::Pose2d* target_diff)
   // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.target_diff)
 }
 
+// .NavMessage.SpeedLimit velocity_limit = 5;
+inline bool Action::_internal_has_velocity_limit() const {
+  return this != internal_default_instance() && velocity_limit_ != nullptr;
+}
+inline bool Action::has_velocity_limit() const {
+  return _internal_has_velocity_limit();
+}
+inline void Action::clear_velocity_limit() {
+  if (GetArena() == nullptr && velocity_limit_ != nullptr) {
+    delete velocity_limit_;
+  }
+  velocity_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_velocity_limit() const {
+  const ::NavMessage::SpeedLimit* p = velocity_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::velocity_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.velocity_limit)
+  return _internal_velocity_limit();
+}
+inline void Action::unsafe_arena_set_allocated_velocity_limit(
+    ::NavMessage::SpeedLimit* velocity_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(velocity_limit_);
+  }
+  velocity_limit_ = velocity_limit;
+  if (velocity_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.velocity_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_velocity_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = velocity_limit_;
+  velocity_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_velocity_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.velocity_limit)
+  
+  ::NavMessage::SpeedLimit* temp = velocity_limit_;
+  velocity_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_velocity_limit() {
+  
+  if (velocity_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    velocity_limit_ = p;
+  }
+  return velocity_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_velocity_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.velocity_limit)
+  return _internal_mutable_velocity_limit();
+}
+inline void Action::set_allocated_velocity_limit(::NavMessage::SpeedLimit* velocity_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete velocity_limit_;
+  }
+  if (velocity_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(velocity_limit);
+    if (message_arena != submessage_arena) {
+      velocity_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, velocity_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  velocity_limit_ = velocity_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.velocity_limit)
+}
+
+// .NavMessage.SpeedLimit angular_limit = 6;
+inline bool Action::_internal_has_angular_limit() const {
+  return this != internal_default_instance() && angular_limit_ != nullptr;
+}
+inline bool Action::has_angular_limit() const {
+  return _internal_has_angular_limit();
+}
+inline void Action::clear_angular_limit() {
+  if (GetArena() == nullptr && angular_limit_ != nullptr) {
+    delete angular_limit_;
+  }
+  angular_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_angular_limit() const {
+  const ::NavMessage::SpeedLimit* p = angular_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::angular_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.angular_limit)
+  return _internal_angular_limit();
+}
+inline void Action::unsafe_arena_set_allocated_angular_limit(
+    ::NavMessage::SpeedLimit* angular_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(angular_limit_);
+  }
+  angular_limit_ = angular_limit;
+  if (angular_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.angular_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_angular_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = angular_limit_;
+  angular_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_angular_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.angular_limit)
+  
+  ::NavMessage::SpeedLimit* temp = angular_limit_;
+  angular_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_angular_limit() {
+  
+  if (angular_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    angular_limit_ = p;
+  }
+  return angular_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_angular_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.angular_limit)
+  return _internal_mutable_angular_limit();
+}
+inline void Action::set_allocated_angular_limit(::NavMessage::SpeedLimit* angular_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete angular_limit_;
+  }
+  if (angular_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(angular_limit);
+    if (message_arena != submessage_arena) {
+      angular_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, angular_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  angular_limit_ = angular_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.angular_limit)
+}
+
+// .NavMessage.SpeedLimit horize_limit = 7;
+inline bool Action::_internal_has_horize_limit() const {
+  return this != internal_default_instance() && horize_limit_ != nullptr;
+}
+inline bool Action::has_horize_limit() const {
+  return _internal_has_horize_limit();
+}
+inline void Action::clear_horize_limit() {
+  if (GetArena() == nullptr && horize_limit_ != nullptr) {
+    delete horize_limit_;
+  }
+  horize_limit_ = nullptr;
+}
+inline const ::NavMessage::SpeedLimit& Action::_internal_horize_limit() const {
+  const ::NavMessage::SpeedLimit* p = horize_limit_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::NavMessage::SpeedLimit*>(
+      &::NavMessage::_SpeedLimit_default_instance_);
+}
+inline const ::NavMessage::SpeedLimit& Action::horize_limit() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Action.horize_limit)
+  return _internal_horize_limit();
+}
+inline void Action::unsafe_arena_set_allocated_horize_limit(
+    ::NavMessage::SpeedLimit* horize_limit) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(horize_limit_);
+  }
+  horize_limit_ = horize_limit;
+  if (horize_limit) {
+    
+  } else {
+    
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:NavMessage.Action.horize_limit)
+}
+inline ::NavMessage::SpeedLimit* Action::release_horize_limit() {
+  
+  ::NavMessage::SpeedLimit* temp = horize_limit_;
+  horize_limit_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::unsafe_arena_release_horize_limit() {
+  // @@protoc_insertion_point(field_release:NavMessage.Action.horize_limit)
+  
+  ::NavMessage::SpeedLimit* temp = horize_limit_;
+  horize_limit_ = nullptr;
+  return temp;
+}
+inline ::NavMessage::SpeedLimit* Action::_internal_mutable_horize_limit() {
+  
+  if (horize_limit_ == nullptr) {
+    auto* p = CreateMaybeMessage<::NavMessage::SpeedLimit>(GetArena());
+    horize_limit_ = p;
+  }
+  return horize_limit_;
+}
+inline ::NavMessage::SpeedLimit* Action::mutable_horize_limit() {
+  // @@protoc_insertion_point(field_mutable:NavMessage.Action.horize_limit)
+  return _internal_mutable_horize_limit();
+}
+inline void Action::set_allocated_horize_limit(::NavMessage::SpeedLimit* horize_limit) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete horize_limit_;
+  }
+  if (horize_limit) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(horize_limit);
+    if (message_arena != submessage_arena) {
+      horize_limit = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, horize_limit, submessage_arena);
+    }
+    
+  } else {
+    
+  }
+  horize_limit_ = horize_limit;
+  // @@protoc_insertion_point(field_set_allocated:NavMessage.Action.horize_limit)
+}
+
 // -------------------------------------------------------------------
 
 // NavCmd
@@ -2265,6 +3069,10 @@ inline void NavStatu::set_allocated_predict_traj(::NavMessage::Trajectory* predi
 
 // -------------------------------------------------------------------
 
+// -------------------------------------------------------------------
+
+// -------------------------------------------------------------------
+
 
 // @@protoc_insertion_point(namespace_scope)
 

+ 1 - 3
projects/controllers/agv_child/emqx-client/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);

+ 1 - 1
projects/controllers/agv_child/emqx-client/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);

二进制
合同/武汉智象机器人有限公司-镭神智能slam定位盒子采购合同.pdf


二进制
合同/雷神智能slam定位盒子采购合同_1_20230323 .docx


二进制
合同/雷神智能slam定位盒子采购合同_1_20230323 .pdf