增加障碍物优化

MPC/loaded_mpc.cpp

@@ -14,6 +14,8 @@ size_t nth = ny + N;
 size_t nv = nth + N;
 size_t ndlt = nv + N;
 
+size_t nobs=ndlt+N;
+
 class FG_eval_half_agv {
  public:
     // Fitted polynomial coefficients
@@ -37,19 +39,23 @@ class FG_eval_half_agv {
       double ref_v=m_condition[1];
       double v=m_statu[0];
       double delta=m_statu[1];
-      //double wheelbase=m_statu[2];
+      double obs_x=m_statu[2];
+      double obs_y=m_statu[3];
+
+      double obs_distance=sqrt(obs_x*obs_x+obs_y*obs_y);
 
       // Reference State Cost
       // Below defines the cost related the reference state and
       // any anything you think may be beneficial.
 
       // Weights for how "important" each cost is - can be tuned
-      const int y_cost_weight = 5000;
-      const int th_cost_weight = 4000;
-      const int v_cost_weight = 20;
-      const int vth_cost_weight = 5000;
-      const int a_cost_weight = 1;
-      const int ath_cost_weight=1000;
+      const double y_cost_weight = 5000;
+      const double th_cost_weight = 4000;
+      const double v_cost_weight = 20;
+      const double vth_cost_weight = 5000;
+      const double a_cost_weight = 1;
+      const double ath_cost_weight=1000;
+      const double obs_distance_weight=1000.0;
 
       // Cost for CTE, psi error and velocity
       for (int t = 0; t < N; t++) {
@@ -64,8 +70,11 @@ class FG_eval_half_agv {
         //目标速度loss
         fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-ref_v,2);
 
-      }
+        //loss 加上车位姿与障碍物的距离
+        /*if(obs_distance<4.0)
+          fg[0]+=obs_distance_weight*(16.0-(CppAD::pow(vars[nx+t]-obs_x,2)));*/
 
+      }
       // Costs for steering (delta) and acceleration (a)
 
       for (int t = 0; t < N-1; t++) {
@@ -74,7 +83,6 @@ class FG_eval_half_agv {
         fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
         fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt+t+1]-vars[ndlt+t], 2);
       }
-      //fg[0]*=0.01;
 
       /////////////////////
       fg[1 + nx] = vars[nx]-vars[nv]*dt;
@@ -112,11 +120,16 @@ class FG_eval_half_agv {
         fg[1+ndlt+t]=(vars[ndlt+t]-vars[ndlt+t-1])/dt;
       }
 
+      for(int t=0;t<N;++t)
+      {
+        fg[1+nobs+t]=CppAD::pow(vars[nx+t]-obs_x,2)+CppAD::pow(vars[ny+t]-obs_y,2);
+      }
+
     }
 };
 
-LoadedMPC::LoadedMPC(double wheelbase){
-  wheelbase_=wheelbase;
+LoadedMPC::LoadedMPC(const Pose2d& obs){
+  obs_relative_pose_=obs;
 }
 LoadedMPC::~LoadedMPC(){}
 
@@ -157,20 +170,20 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   if(ref_velocity<0.05)
     ref_velocity=0.05;
   //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
-  Pose2d targetPoseInAGV=Pose2d::TargetByPose(target,pose_agv);
+  Pose2d targetPoseInAGV=Pose2d::relativePose(target,pose_agv);
 
   if(targetPoseInAGV.x()<0)
     ref_velocity = -ref_velocity;
 
   double dt = 0.1;
-  double min_velocity = -2.0;
-  double max_velocity = 2.0;
-  double max_dlt=10*M_PI/180.0;
-  double max_acc_line_velocity=max_velocity;
-  double max_acc_dlt=50.*M_PI/180.0;
+  double min_velocity = -1.0;
+  double max_velocity = 1.0;
+  double max_dlt=2*M_PI/180.0;
+  double max_acc_line_velocity=0.3;
+  double max_acc_dlt=5.*M_PI/180.0;
 
   size_t n_vars = N * 5;
-  size_t n_constraints = N * 5;
+
 
   Dvector vars(n_vars);
   for (int i = 0; i < n_vars; i++)
@@ -202,6 +215,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   }
 
   // Lower and upper limits for the constraints
+  size_t n_constraints = N * (5+1);
   Dvector constraints_lowerbound(n_constraints);
   Dvector constraints_upperbound(n_constraints);
   for (int i = 0; i < n_constraints; i++)
@@ -221,9 +235,16 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     constraints_lowerbound[i] = -max_acc_dlt;
     constraints_upperbound[i] = max_acc_dlt;
   }
+  // 与障碍物保持距离
+  double dobs=3.0;
+  for(int i=nobs;i<nobs+N;++i)
+  {
+    constraints_lowerbound[i] = pow(float(nobs+N-i)/float(N)*dobs,2);
+    constraints_upperbound[i] = 1e19;
+  }
 
 
-  Eigen::VectorXd statu_velocity(3);
+  Eigen::VectorXd statu_velocity(4);
   if(line_velocity>max_velocity)
   {
     line_velocity=max_velocity;
@@ -246,7 +267,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     printf(" input -dlt limited\n");
   }
 
-  statu_velocity << line_velocity, delta,wheelbase_;
+  statu_velocity << line_velocity, delta,obs_relative_pose_.x(),obs_relative_pose_.y();
 
   Eigen::VectorXd condition(2);
   condition << dt, ref_velocity;
@@ -274,8 +295,8 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
   if (ok == false)
   {
-    printf(" mpc failed statu : %d  input: %.4f  %.4f  \n",solution.status,line_velocity,
-           delta);
+    printf(" mpc failed statu : %d  input: %.4f  %.4f relative:(%f,%f) \n",solution.status,line_velocity,
+           delta,obs_relative_pose_.x(),obs_relative_pose_.y());
     return false;
   }
 
@@ -289,7 +310,8 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   out.push_back(solve_velocity);
   double front_delta=solution.x[ndlt];
   out.push_back(front_delta);
-  printf(" input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",line_velocity,
+  printf("input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",
+         line_velocity,
          delta,solve_velocity,front_delta,ref_velocity,time);
 
   optimize_trajectory.clear();

MPC/loaded_mpc.h

@@ -11,7 +11,7 @@
 class LoadedMPC
 {
  public:
-    LoadedMPC(double wheelbase);
+    LoadedMPC(const Pose2d& obs);
     ~LoadedMPC();
     virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
                        std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
@@ -27,7 +27,7 @@ class LoadedMPC
      * 根据路径点，拟合曲线
      */
     static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
-    double wheelbase_;
+    Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
 
 };
 

MPC/monitor/emqx/message.pb.cc

@@ -174,6 +174,7 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, end_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -218,11 +219,11 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::Speed)},
-  { 19, -1, sizeof(::NavMessage::Pose2d)},
-  { 27, -1, sizeof(::NavMessage::Trajectory)},
-  { 33, -1, sizeof(::NavMessage::Action)},
-  { 41, -1, sizeof(::NavMessage::NavCmd)},
-  { 49, -1, sizeof(::NavMessage::NavStatu)},
+  { 20, -1, sizeof(::NavMessage::Pose2d)},
+  { 28, -1, sizeof(::NavMessage::Trajectory)},
+  { 34, -1, sizeof(::NavMessage::Action)},
+  { 42, -1, sizeof(::NavMessage::NavCmd)},
+  { 50, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -238,20 +239,20 @@ static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] =
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\022"
   "\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022\t\n\001"
-  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"3\n\005Speed\022\t\n\001H\030\001 \001(\005"
-  "\022\t\n\001T\030\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\"-\n\006Pos"
-  "e2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002"
-  "\"/\n\nTrajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessa"
-  "ge.Pose2d\"c\n\006Action\022\014\n\004type\030\001 \001(\005\022\"\n\006tar"
-  "get\030\002 \001(\0132\022.NavMessage.Pose2d\022\'\n\013target_"
-  "diff\030\003 \001(\0132\022.NavMessage.Pose2d\"J\n\006NavCmd"
-  "\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022#\n\007actions"
-  "\030\003 \003(\0132\022.NavMessage.Action\"\263\001\n\010NavStatu\022"
-  "\r\n\005statu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinishe"
-  "d_actions\030\003 \003(\0132\022.NavMessage.Action\022-\n\rs"
-  "elected_traj\030\004 \001(\0132\026.NavMessage.Trajecto"
-  "ry\022,\n\014predict_traj\030\005 \001(\0132\026.NavMessage.Tr"
-  "ajectoryb\006proto3"
+  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005"
+  "\022\t\n\001T\030\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end"
+  "\030\005 \001(\005\"-\n\006Pose2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r"
+  "\n\005theta\030\003 \001(\002\"/\n\nTrajectory\022!\n\005poses\030\001 \003"
+  "(\0132\022.NavMessage.Pose2d\"c\n\006Action\022\014\n\004type"
+  "\030\001 \001(\005\022\"\n\006target\030\002 \001(\0132\022.NavMessage.Pose"
+  "2d\022\'\n\013target_diff\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001"
+  "(\t\022#\n\007actions\030\003 \003(\0132\022.NavMessage.Action\""
+  "\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t"
+  "\022.\n\022unfinished_actions\030\003 \003(\0132\022.NavMessag"
+  "e.Action\022-\n\rselected_traj\030\004 \001(\0132\026.NavMes"
+  "sage.Trajectory\022,\n\014predict_traj\030\005 \001(\0132\026."
+  "NavMessage.Trajectoryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -266,7 +267,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 616,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 629,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 7, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 7, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -584,15 +585,15 @@ Speed::Speed(const Speed& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
   ::memcpy(&h_, &from.h_,
-    static_cast<size_t>(reinterpret_cast<char*>(&w_) -
-    reinterpret_cast<char*>(&h_)) + sizeof(w_));
+    static_cast<size_t>(reinterpret_cast<char*>(&end_) -
+    reinterpret_cast<char*>(&h_)) + sizeof(end_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.Speed)
 }
 
 void Speed::SharedCtor() {
   ::memset(&h_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&w_) -
-      reinterpret_cast<char*>(&h_)) + sizeof(w_));
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
 }
 
 Speed::~Speed() {
@@ -627,8 +628,8 @@ void Speed::Clear() {
   (void) cached_has_bits;
 
   ::memset(&h_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&w_) -
-      reinterpret_cast<char*>(&h_)) + sizeof(w_));
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -668,6 +669,13 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // int32 end = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+          end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -720,6 +728,12 @@ failure:
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
   }
 
+  // int32 end = 5;
+  if (this->end() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -760,6 +774,13 @@ size_t Speed::ByteSizeLong() const {
     total_size += 1 + 4;
   }
 
+  // int32 end = 5;
+  if (this->end() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_end());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -803,6 +824,9 @@ void Speed::MergeFrom(const Speed& from) {
   if (!(from.w() <= 0 && from.w() >= 0)) {
     _internal_set_w(from._internal_w());
   }
+  if (from.end() != 0) {
+    _internal_set_end(from._internal_end());
+  }
 }
 
 void Speed::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -827,8 +851,8 @@ void Speed::InternalSwap(Speed* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(Speed, w_)
-      + sizeof(Speed::w_)
+      PROTOBUF_FIELD_OFFSET(Speed, end_)
+      + sizeof(Speed::end_)
       - PROTOBUF_FIELD_OFFSET(Speed, h_)>(
           reinterpret_cast<char*>(&h_),
           reinterpret_cast<char*>(&other->h_));

MPC/monitor/emqx/message.pb.h

@@ -388,6 +388,7 @@ class Speed PROTOBUF_FINAL :
     kTFieldNumber = 2,
     kVFieldNumber = 3,
     kWFieldNumber = 4,
+    kEndFieldNumber = 5,
   };
   // int32 H = 1;
   void clear_h();
@@ -425,6 +426,15 @@ class Speed PROTOBUF_FINAL :
   void _internal_set_w(float value);
   public:
 
+  // int32 end = 5;
+  void clear_end();
+  ::PROTOBUF_NAMESPACE_ID::int32 end() const;
+  void set_end(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_end() const;
+  void _internal_set_end(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
   // @@protoc_insertion_point(class_scope:NavMessage.Speed)
  private:
   class _Internal;
@@ -436,6 +446,7 @@ class Speed PROTOBUF_FINAL :
   ::PROTOBUF_NAMESPACE_ID::int32 t_;
   float v_;
   float w_;
+  ::PROTOBUF_NAMESPACE_ID::int32 end_;
   mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   friend struct ::TableStruct_message_2eproto;
 };
@@ -1506,6 +1517,26 @@ inline void Speed::set_w(float value) {
   // @@protoc_insertion_point(field_set:NavMessage.Speed.W)
 }
 
+// int32 end = 5;
+inline void Speed::clear_end() {
+  end_ = 0;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::_internal_end() const {
+  return end_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 Speed::end() const {
+  // @@protoc_insertion_point(field_get:NavMessage.Speed.end)
+  return _internal_end();
+}
+inline void Speed::_internal_set_end(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  
+  end_ = value;
+}
+inline void Speed::set_end(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_end(value);
+  // @@protoc_insertion_point(field_set:NavMessage.Speed.end)
+}
+
 // -------------------------------------------------------------------
 
 // Pose2d

MPC/monitor/monitor_emqx.cpp

@@ -42,11 +42,12 @@ void Monitor_emqx::set_speed(SpeedType type,double v,double a)
 {
   MqttMsg msg;
   NavMessage::Speed speed;
-  heat_=(heat_++)%255;
+  heat_=(heat_+1)%255;
   speed.set_h(heat_);
   speed.set_t(type);
   speed.set_v(v);
   speed.set_w(a);
+  speed.set_end(1);
   msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);
@@ -58,11 +59,12 @@ void Monitor_emqx::stop()
 {
   MqttMsg msg;
   NavMessage::Speed speed;
-  heat_=(heat_++)%255;
+  heat_=(heat_+1)%255;
   speed.set_h(heat_);
   speed.set_t(eStop);
   speed.set_v(0);
   speed.set_w(0);
+  speed.set_end(1);
   msg.fromProtoMessage(speed);
   if(client_)
     client_->publish(pubTopic_,1,msg);

MPC/navigation.cpp

@@ -289,7 +289,7 @@ bool Navigation::exec_adjust_action(const Pose2d& target,const Pose2d& target_di
     }
 
     //计算目标点在当前pose下的pose
-    Pose2d diff = Pose2d::TargetByPose(target, timedPose_.Get());
+    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
 
     //停止状态
     if (action == 0) {
@@ -354,7 +354,9 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
   double velocity=timedV_.Get();    //从plc获取状态
   double angular=timedA_.Get();
 
-  LoadedMPC MPC(2.7);
+  Pose2d brother=timedBrotherPose_.Get();
+  Pose2d relative=Pose2d::relativePose(brother,pose);//计算另一节在当前小车坐标系下的坐标
+  LoadedMPC MPC(relative);
   Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
 
   statu[0]=pose.x();
@@ -374,7 +376,7 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                            const Pose2d& target,const Pose2d& diff)
 {
-  Pose2d distance=Pose2d::TargetByPose(target,cur);
+  Pose2d distance=Pose2d::relativePose(target,cur);
     if(Pose2d::abs(distance)<diff)
     {
         if(fabs(velocity)<0.1 && fabs(angular)< 10*M_PI/180.0)
@@ -428,6 +430,7 @@ bool Navigation::execute_along_action(const Pose2d& target,const Pose2d& target_
         return false;
       }
       monitor_->set_speed(Monitor_emqx::eMPC,out[0],out[1]);
+      //std::this_thread::sleep_for(std::chrono::milliseconds (400));
     }
 
 

MPC/pose2d.cpp

@@ -32,7 +32,7 @@ const float Pose2d::gridient() const
   return gradient;
 }
 
-Pose2d Pose2d::TargetByPose(const Pose2d& target_pose,const Pose2d& axis_pose)
+Pose2d Pose2d::relativePose(const Pose2d& target_pose,const Pose2d& axis_pose)
 {
   Pose2d diff=target_pose-axis_pose;
   Pose2d nPose=diff.rotate(-axis_pose.theta());

MPC/pose2d.h

@@ -95,7 +95,7 @@ class Pose2d
     const float gridient()const;
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
-    static Pose2d TargetByPose(const Pose2d& world_pose,const Pose2d& axis_pose);
+    static Pose2d relativePose(const Pose2d& world_pose,const Pose2d& axis_pose);
 
  protected:
     std::atomic<float>                      m_x;

config/navigation.prototxt

@@ -1,7 +1,7 @@
 Agv_emqx
 {
 	NodeId:"agv1"
-	ip:"127.0.0.1"
+	ip:"192.168.0.70"
 	port:1883
 	pubTopic:"monitor/speedcmd"
 	subTopic:"monitor/statu"
@@ -10,7 +10,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv1-nav"
-	ip:"127.0.0.1"
+	ip:"192.168.0.70"
 	port:1883
 	pubStatuTopic:"agv1/agv_statu"
 	pubNavStatuTopic:"agv1/nav_statu"

config/navigation_child.prototxt

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

message.proto

@@ -13,6 +13,7 @@ message Speed {
   int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
   float V=3;  //角速度
   float W=4;  //线速度
+  int32 end=5;
 }
 
 

parameter.proto

@@ -0,0 +1,27 @@
+syntax = "proto3";
+message AgvEmqx_parameter
+{
+  string NodeId=1;
+  string ip=2;
+  int32 port=3;
+  string pubTopic=4;
+  string subTopic=5;
+}
+message Emqx_parameter
+{
+    string NodeId=1;
+    string ip=2;
+    int32 port=3;
+    string pubStatuTopic=4;
+    string pubNavStatuTopic=5;
+    string subNavCmdTopic=6;
+  string subBrotherStatuTopic=7;
+
+}
+
+message Navigation_parameter
+{
+  AgvEmqx_parameter Agv_emqx=1;
+  Emqx_parameter Terminal_emqx=2;
+}
+

projects/controllers/AGV_controller/AGV_controller.cpp

@@ -216,7 +216,7 @@ int main(int argc, char **argv) {
 
     //变速----------------------------------
     double cmd_v=g_speed.Get().v();
-    double cmd_vth=g_speed.Get().vth();
+    double cmd_vth=g_speed.Get().w();
 
     //初始值
     double R1=0,R2=0,R3=0,R4=0;
@@ -225,16 +225,16 @@ int main(int argc, char **argv) {
     double L3=cmd_v;
     double L4=cmd_v;
 
-    if(g_speed.Get().type()==1) //原地旋转
+    if(g_speed.Get().t()==1) //原地旋转
     {
       if (fabs(cmd_vth) > 0.0001)
         Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else if(g_speed.Get().type()==2)    //横移
+    else if(g_speed.Get().t()==2)    //横移
     {
       Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else if(g_speed.Get().type()==3) {    //巡线/前进
+    else if(g_speed.Get().t()==3) {    //巡线/前进
 
       if (fabs(cmd_vth) > 0.4) {
         cmd_vth = cmd_vth > 0 ? 0.4 : -0.4;

projects/controllers/AGV_controller/emqx-client/message.pb.cc

@@ -14,7 +14,6 @@
 #include <google/protobuf/wire_format.h>
 // @@protoc_insertion_point(includes)
 #include <google/protobuf/port_def.inc>
-extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_AGVStatu_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Action_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Pose2d_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Trajectory_message_2eproto;
@@ -103,10 +102,9 @@ static void InitDefaultsscc_info_NavStatu_message_2eproto() {
   ::NavMessage::NavStatu::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<3> scc_info_NavStatu_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 3, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavStatu_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
       &scc_info_Action_message_2eproto.base,
-      &scc_info_AGVStatu_message_2eproto.base,
       &scc_info_Trajectory_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_Pose2d_message_2eproto() {
@@ -172,9 +170,11 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, type_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, vth_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, end_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -213,20 +213,17 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, statu_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, unfinished_actions_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, current_pose_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, pose_timeout_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, twist_timeout_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, predict_traj_),
 };
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::Speed)},
-  { 18, -1, sizeof(::NavMessage::Pose2d)},
-  { 26, -1, sizeof(::NavMessage::Trajectory)},
-  { 32, -1, sizeof(::NavMessage::Action)},
-  { 40, -1, sizeof(::NavMessage::NavCmd)},
-  { 48, -1, sizeof(::NavMessage::NavStatu)},
+  { 20, -1, sizeof(::NavMessage::Pose2d)},
+  { 28, -1, sizeof(::NavMessage::Trajectory)},
+  { 34, -1, sizeof(::NavMessage::Action)},
+  { 42, -1, sizeof(::NavMessage::NavCmd)},
+  { 50, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -242,22 +239,20 @@ static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] =
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\022"
   "\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022\t\n\001"
-  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"-\n\005Speed\022\014\n\004type\030\001 "
-  "\001(\005\022\t\n\001v\030\002 \001(\002\022\013\n\003vth\030\003 \001(\002\"-\n\006Pose2d\022\t\n"
-  "\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"/\n\nTr"
-  "ajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pos"
-  "e2d\"c\n\006Action\022\014\n\004type\030\001 \001(\005\022\"\n\006target\030\002 "
-  "\001(\0132\022.NavMessage.Pose2d\022\'\n\013target_diff\030\003"
-  " \001(\0132\022.NavMessage.Pose2d\"J\n\006NavCmd\022\016\n\006ac"
-  "tion\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\013"
-  "2\022.NavMessage.Action\"\214\002\n\010NavStatu\022\r\n\005sta"
-  "tu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinished_acti"
-  "ons\030\003 \003(\0132\022.NavMessage.Action\022*\n\014current"
-  "_pose\030\004 \001(\0132\024.NavMessage.AGVStatu\022\024\n\014pos"
-  "e_timeout\030\005 \001(\010\022\025\n\rtwist_timeout\030\006 \001(\010\022-"
-  "\n\rselected_traj\030\007 \001(\0132\026.NavMessage.Traje"
-  "ctory\022,\n\014predict_traj\030\010 \001(\0132\026.NavMessage"
-  ".Trajectoryb\006proto3"
+  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005"
+  "\022\t\n\001T\030\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end"
+  "\030\005 \001(\005\"-\n\006Pose2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r"
+  "\n\005theta\030\003 \001(\002\"/\n\nTrajectory\022!\n\005poses\030\001 \003"
+  "(\0132\022.NavMessage.Pose2d\"c\n\006Action\022\014\n\004type"
+  "\030\001 \001(\005\022\"\n\006target\030\002 \001(\0132\022.NavMessage.Pose"
+  "2d\022\'\n\013target_diff\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001"
+  "(\t\022#\n\007actions\030\003 \003(\0132\022.NavMessage.Action\""
+  "\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t"
+  "\022.\n\022unfinished_actions\030\003 \003(\0132\022.NavMessag"
+  "e.Action\022-\n\rselected_traj\030\004 \001(\0132\026.NavMes"
+  "sage.Trajectory\022,\n\014predict_traj\030\005 \001(\0132\026."
+  "NavMessage.Trajectoryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -272,7 +267,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 699,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 629,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 7, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 7, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -589,16 +584,16 @@ Speed::Speed(::PROTOBUF_NAMESPACE_ID::Arena* arena)
 Speed::Speed(const Speed& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
-  ::memcpy(&type_, &from.type_,
-    static_cast<size_t>(reinterpret_cast<char*>(&vth_) -
-    reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memcpy(&h_, &from.h_,
+    static_cast<size_t>(reinterpret_cast<char*>(&end_) -
+    reinterpret_cast<char*>(&h_)) + sizeof(end_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.Speed)
 }
 
 void Speed::SharedCtor() {
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
 }
 
 Speed::~Speed() {
@@ -632,9 +627,9 @@ void Speed::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -646,27 +641,41 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // int32 type = 1;
+      // int32 H = 1;
       case 1:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
-          type_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          h_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float v = 2;
+      // int32 T = 2;
       case 2:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 21)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float vth = 3;
+      // float V = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          vth_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // float W = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // int32 end = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+          end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -695,22 +704,34 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_type(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
+  }
+
+  // int32 T = 2;
+  if (this->t() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
   }
 
-  // float v = 2;
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(2, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_vth(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
+  }
+
+  // int32 end = 5;
+  if (this->end() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -729,23 +750,37 @@ size_t Speed::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-        this->_internal_type());
+        this->_internal_h());
   }
 
-  // float v = 2;
+  // int32 T = 2;
+  if (this->t() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_t());
+  }
+
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
+  // int32 end = 5;
+  if (this->end() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_end());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -777,14 +812,20 @@ void Speed::MergeFrom(const Speed& from) {
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (from.type() != 0) {
-    _internal_set_type(from._internal_type());
+  if (from.h() != 0) {
+    _internal_set_h(from._internal_h());
+  }
+  if (from.t() != 0) {
+    _internal_set_t(from._internal_t());
   }
   if (!(from.v() <= 0 && from.v() >= 0)) {
     _internal_set_v(from._internal_v());
   }
-  if (!(from.vth() <= 0 && from.vth() >= 0)) {
-    _internal_set_vth(from._internal_vth());
+  if (!(from.w() <= 0 && from.w() >= 0)) {
+    _internal_set_w(from._internal_w());
+  }
+  if (from.end() != 0) {
+    _internal_set_end(from._internal_end());
   }
 }
 
@@ -810,11 +851,11 @@ void Speed::InternalSwap(Speed* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(Speed, vth_)
-      + sizeof(Speed::vth_)
-      - PROTOBUF_FIELD_OFFSET(Speed, type_)>(
-          reinterpret_cast<char*>(&type_),
-          reinterpret_cast<char*>(&other->type_));
+      PROTOBUF_FIELD_OFFSET(Speed, end_)
+      + sizeof(Speed::end_)
+      - PROTOBUF_FIELD_OFFSET(Speed, h_)>(
+          reinterpret_cast<char*>(&h_),
+          reinterpret_cast<char*>(&other->h_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata Speed::GetMetadata() const {
@@ -1833,8 +1874,6 @@ void NavCmd::InternalSwap(NavCmd* other) {
 // ===================================================================
 
 void NavStatu::InitAsDefaultInstance() {
-  ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->current_pose_ = const_cast< ::NavMessage::AGVStatu*>(
-      ::NavMessage::AGVStatu::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->selected_traj_ = const_cast< ::NavMessage::Trajectory*>(
       ::NavMessage::Trajectory::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->predict_traj_ = const_cast< ::NavMessage::Trajectory*>(
@@ -1842,15 +1881,10 @@ void NavStatu::InitAsDefaultInstance() {
 }
 class NavStatu::_Internal {
  public:
-  static const ::NavMessage::AGVStatu& current_pose(const NavStatu* msg);
   static const ::NavMessage::Trajectory& selected_traj(const NavStatu* msg);
   static const ::NavMessage::Trajectory& predict_traj(const NavStatu* msg);
 };
 
-const ::NavMessage::AGVStatu&
-NavStatu::_Internal::current_pose(const NavStatu* msg) {
-  return *msg->current_pose_;
-}
 const ::NavMessage::Trajectory&
 NavStatu::_Internal::selected_traj(const NavStatu* msg) {
   return *msg->selected_traj_;
@@ -1875,11 +1909,6 @@ NavStatu::NavStatu(const NavStatu& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
-  if (from._internal_has_current_pose()) {
-    current_pose_ = new ::NavMessage::AGVStatu(*from.current_pose_);
-  } else {
-    current_pose_ = nullptr;
-  }
   if (from._internal_has_selected_traj()) {
     selected_traj_ = new ::NavMessage::Trajectory(*from.selected_traj_);
   } else {
@@ -1890,18 +1919,16 @@ NavStatu::NavStatu(const NavStatu& from)
   } else {
     predict_traj_ = nullptr;
   }
-  ::memcpy(&statu_, &from.statu_,
-    static_cast<size_t>(reinterpret_cast<char*>(&twist_timeout_) -
-    reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = from.statu_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavStatu)
 }
 
 void NavStatu::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavStatu_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  ::memset(&current_pose_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&current_pose_)) + sizeof(twist_timeout_));
+  ::memset(&selected_traj_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&statu_) -
+      reinterpret_cast<char*>(&selected_traj_)) + sizeof(statu_));
 }
 
 NavStatu::~NavStatu() {
@@ -1913,7 +1940,6 @@ NavStatu::~NavStatu() {
 void NavStatu::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  if (this != internal_default_instance()) delete current_pose_;
   if (this != internal_default_instance()) delete selected_traj_;
   if (this != internal_default_instance()) delete predict_traj_;
 }
@@ -1941,10 +1967,6 @@ void NavStatu::Clear() {
 
   unfinished_actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  if (GetArena() == nullptr && current_pose_ != nullptr) {
-    delete current_pose_;
-  }
-  current_pose_ = nullptr;
   if (GetArena() == nullptr && selected_traj_ != nullptr) {
     delete selected_traj_;
   }
@@ -1953,9 +1975,7 @@ void NavStatu::Clear() {
     delete predict_traj_;
   }
   predict_traj_ = nullptr;
-  ::memset(&statu_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = false;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1995,37 +2015,16 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
         } else goto handle_unusual;
         continue;
-      // .NavMessage.AGVStatu current_pose = 4;
+      // .NavMessage.Trajectory selected_traj = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
-          ptr = ctx->ParseMessage(_internal_mutable_current_pose(), ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool pose_timeout = 5;
-      case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
-          pose_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool twist_timeout = 6;
-      case 6:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
-          twist_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // .NavMessage.Trajectory selected_traj = 7;
-      case 7:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 58)) {
           ptr = ctx->ParseMessage(_internal_mutable_selected_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // .NavMessage.Trajectory predict_traj = 8;
-      case 8:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 66)) {
+      // .NavMessage.Trajectory predict_traj = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
           ptr = ctx->ParseMessage(_internal_mutable_predict_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -2082,40 +2081,20 @@ failure:
       InternalWriteMessage(3, this->_internal_unfinished_actions(i), target, stream);
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(
-        4, _Internal::current_pose(this), target, stream);
-  }
-
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(5, this->_internal_pose_timeout(), target);
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(6, this->_internal_twist_timeout(), target);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        7, _Internal::selected_traj(this), target, stream);
+        4, _Internal::selected_traj(this), target, stream);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        8, _Internal::predict_traj(this), target, stream);
+        5, _Internal::predict_traj(this), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2148,21 +2127,14 @@ size_t NavStatu::ByteSizeLong() const {
         this->_internal_key());
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    total_size += 1 +
-      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
-        *current_pose_);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *selected_traj_);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
@@ -2174,16 +2146,6 @@ size_t NavStatu::ByteSizeLong() const {
     total_size += 1 + 1;
   }
 
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    total_size += 1 + 1;
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 0) {
-    total_size += 1 + 1;
-  }
-
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -2219,9 +2181,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
-  if (from.has_current_pose()) {
-    _internal_mutable_current_pose()->::NavMessage::AGVStatu::MergeFrom(from._internal_current_pose());
-  }
   if (from.has_selected_traj()) {
     _internal_mutable_selected_traj()->::NavMessage::Trajectory::MergeFrom(from._internal_selected_traj());
   }
@@ -2231,12 +2190,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.statu() != 0) {
     _internal_set_statu(from._internal_statu());
   }
-  if (from.pose_timeout() != 0) {
-    _internal_set_pose_timeout(from._internal_pose_timeout());
-  }
-  if (from.twist_timeout() != 0) {
-    _internal_set_twist_timeout(from._internal_twist_timeout());
-  }
 }
 
 void NavStatu::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -2263,11 +2216,11 @@ void NavStatu::InternalSwap(NavStatu* other) {
   unfinished_actions_.InternalSwap(&other->unfinished_actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(NavStatu, twist_timeout_)
-      + sizeof(NavStatu::twist_timeout_)
-      - PROTOBUF_FIELD_OFFSET(NavStatu, current_pose_)>(
-          reinterpret_cast<char*>(&current_pose_),
-          reinterpret_cast<char*>(&other->current_pose_));
+      PROTOBUF_FIELD_OFFSET(NavStatu, statu_)
+      + sizeof(NavStatu::statu_)
+      - PROTOBUF_FIELD_OFFSET(NavStatu, selected_traj_)>(
+          reinterpret_cast<char*>(&selected_traj_),
+          reinterpret_cast<char*>(&other->selected_traj_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavStatu::GetMetadata() const {

projects/controllers/AGV_controller/emqx-client/message.pb.h

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

projects/controllers/agv_child/AGV_controller.cpp

@@ -216,7 +216,7 @@ int main(int argc, char **argv) {
 
     //变速----------------------------------
     double cmd_v=g_speed.Get().v();
-    double cmd_vth=g_speed.Get().vth();
+    double cmd_vth=g_speed.Get().w();
 
     //初始值
     double R1=0,R2=0,R3=0,R4=0;
@@ -225,12 +225,12 @@ int main(int argc, char **argv) {
     double L3=cmd_v;
     double L4=cmd_v;
 
-    if(g_speed.Get().type()==1) //原地旋转
+    if(g_speed.Get().t()==1) //原地旋转
     {
       if (fabs(cmd_vth) > 0.0001)
         Rotating(cmd_vth, w / 2.0, l / 2.0, R1, R2, R3, R4, L1, L2, L3, L4);
     }
-    else if(g_speed.Get().type()==2)    //横移
+    else if(g_speed.Get().t()==2)    //横移
     {
       Horizontal(g_speed.Get().v(),R1, R2, R3, R4, L1, L2, L3, L4);
     }

projects/controllers/agv_child/emqx-client/message.pb.cc

@@ -14,7 +14,6 @@
 #include <google/protobuf/wire_format.h>
 // @@protoc_insertion_point(includes)
 #include <google/protobuf/port_def.inc>
-extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_AGVStatu_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Action_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Pose2d_message_2eproto;
 extern PROTOBUF_INTERNAL_EXPORT_message_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Trajectory_message_2eproto;
@@ -103,10 +102,9 @@ static void InitDefaultsscc_info_NavStatu_message_2eproto() {
   ::NavMessage::NavStatu::InitAsDefaultInstance();
 }
 
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<3> scc_info_NavStatu_message_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 3, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_NavStatu_message_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_NavStatu_message_2eproto}, {
       &scc_info_Action_message_2eproto.base,
-      &scc_info_AGVStatu_message_2eproto.base,
       &scc_info_Trajectory_message_2eproto.base,}};
 
 static void InitDefaultsscc_info_Pose2d_message_2eproto() {
@@ -172,9 +170,11 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, type_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, h_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, t_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, v_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, vth_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, w_),
+  PROTOBUF_FIELD_OFFSET(::NavMessage::Speed, end_),
   ~0u,  // no _has_bits_
   PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -213,20 +213,17 @@ const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_message_2eproto::offsets[] PRO
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, statu_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, key_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, unfinished_actions_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, current_pose_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, pose_timeout_),
-  PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, twist_timeout_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, selected_traj_),
   PROTOBUF_FIELD_OFFSET(::NavMessage::NavStatu, predict_traj_),
 };
 static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
   { 0, -1, sizeof(::NavMessage::AGVStatu)},
   { 10, -1, sizeof(::NavMessage::Speed)},
-  { 18, -1, sizeof(::NavMessage::Pose2d)},
-  { 26, -1, sizeof(::NavMessage::Trajectory)},
-  { 32, -1, sizeof(::NavMessage::Action)},
-  { 40, -1, sizeof(::NavMessage::NavCmd)},
-  { 48, -1, sizeof(::NavMessage::NavStatu)},
+  { 20, -1, sizeof(::NavMessage::Pose2d)},
+  { 28, -1, sizeof(::NavMessage::Trajectory)},
+  { 34, -1, sizeof(::NavMessage::Action)},
+  { 42, -1, sizeof(::NavMessage::NavCmd)},
+  { 50, -1, sizeof(::NavMessage::NavStatu)},
 };
 
 static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
@@ -242,22 +239,20 @@ static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] =
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
   "\n\rmessage.proto\022\nNavMessage\"G\n\010AGVStatu\022"
   "\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022\t\n\001"
-  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"-\n\005Speed\022\014\n\004type\030\001 "
-  "\001(\005\022\t\n\001v\030\002 \001(\002\022\013\n\003vth\030\003 \001(\002\"-\n\006Pose2d\022\t\n"
-  "\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\"/\n\nTr"
-  "ajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pos"
-  "e2d\"c\n\006Action\022\014\n\004type\030\001 \001(\005\022\"\n\006target\030\002 "
-  "\001(\0132\022.NavMessage.Pose2d\022\'\n\013target_diff\030\003"
-  " \001(\0132\022.NavMessage.Pose2d\"J\n\006NavCmd\022\016\n\006ac"
-  "tion\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022#\n\007actions\030\003 \003(\013"
-  "2\022.NavMessage.Action\"\214\002\n\010NavStatu\022\r\n\005sta"
-  "tu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t\022.\n\022unfinished_acti"
-  "ons\030\003 \003(\0132\022.NavMessage.Action\022*\n\014current"
-  "_pose\030\004 \001(\0132\024.NavMessage.AGVStatu\022\024\n\014pos"
-  "e_timeout\030\005 \001(\010\022\025\n\rtwist_timeout\030\006 \001(\010\022-"
-  "\n\rselected_traj\030\007 \001(\0132\026.NavMessage.Traje"
-  "ctory\022,\n\014predict_traj\030\010 \001(\0132\026.NavMessage"
-  ".Trajectoryb\006proto3"
+  "v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"@\n\005Speed\022\t\n\001H\030\001 \001(\005"
+  "\022\t\n\001T\030\002 \001(\005\022\t\n\001V\030\003 \001(\002\022\t\n\001W\030\004 \001(\002\022\013\n\003end"
+  "\030\005 \001(\005\"-\n\006Pose2d\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r"
+  "\n\005theta\030\003 \001(\002\"/\n\nTrajectory\022!\n\005poses\030\001 \003"
+  "(\0132\022.NavMessage.Pose2d\"c\n\006Action\022\014\n\004type"
+  "\030\001 \001(\005\022\"\n\006target\030\002 \001(\0132\022.NavMessage.Pose"
+  "2d\022\'\n\013target_diff\030\003 \001(\0132\022.NavMessage.Pos"
+  "e2d\"J\n\006NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001"
+  "(\t\022#\n\007actions\030\003 \003(\0132\022.NavMessage.Action\""
+  "\263\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\010\022\013\n\003key\030\002 \001(\t"
+  "\022.\n\022unfinished_actions\030\003 \003(\0132\022.NavMessag"
+  "e.Action\022-\n\rselected_traj\030\004 \001(\0132\026.NavMes"
+  "sage.Trajectory\022,\n\014predict_traj\030\005 \001(\0132\026."
+  "NavMessage.Trajectoryb\006proto3"
   ;
 static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_message_2eproto_deps[1] = {
 };
@@ -272,7 +267,7 @@ static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_mes
 };
 static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_message_2eproto_once;
 const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_message_2eproto = {
-  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 699,
+  false, false, descriptor_table_protodef_message_2eproto, "message.proto", 629,
   &descriptor_table_message_2eproto_once, descriptor_table_message_2eproto_sccs, descriptor_table_message_2eproto_deps, 7, 0,
   schemas, file_default_instances, TableStruct_message_2eproto::offsets,
   file_level_metadata_message_2eproto, 7, file_level_enum_descriptors_message_2eproto, file_level_service_descriptors_message_2eproto,
@@ -589,16 +584,16 @@ Speed::Speed(::PROTOBUF_NAMESPACE_ID::Arena* arena)
 Speed::Speed(const Speed& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
-  ::memcpy(&type_, &from.type_,
-    static_cast<size_t>(reinterpret_cast<char*>(&vth_) -
-    reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memcpy(&h_, &from.h_,
+    static_cast<size_t>(reinterpret_cast<char*>(&end_) -
+    reinterpret_cast<char*>(&h_)) + sizeof(end_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.Speed)
 }
 
 void Speed::SharedCtor() {
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
 }
 
 Speed::~Speed() {
@@ -632,9 +627,9 @@ void Speed::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  ::memset(&type_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&vth_) -
-      reinterpret_cast<char*>(&type_)) + sizeof(vth_));
+  ::memset(&h_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&end_) -
+      reinterpret_cast<char*>(&h_)) + sizeof(end_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -646,27 +641,41 @@ const char* Speed::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::inte
     ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
     CHK_(ptr);
     switch (tag >> 3) {
-      // int32 type = 1;
+      // int32 H = 1;
       case 1:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
-          type_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          h_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float v = 2;
+      // int32 T = 2;
       case 2:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 21)) {
-          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
-          ptr += sizeof(float);
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
+          t_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // float vth = 3;
+      // float V = 3;
       case 3:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 29)) {
-          vth_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          v_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
           ptr += sizeof(float);
         } else goto handle_unusual;
         continue;
+      // float W = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 37)) {
+          w_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else goto handle_unusual;
+        continue;
+      // int32 end = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
+          end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
       default: {
       handle_unusual:
         if ((tag & 7) == 4 || tag == 0) {
@@ -695,22 +704,34 @@ failure:
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_type(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(1, this->_internal_h(), target);
+  }
+
+  // int32 T = 2;
+  if (this->t() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_t(), target);
   }
 
-  // float v = 2;
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(2, this->_internal_v(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_v(), target);
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(3, this->_internal_vth(), target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteFloatToArray(4, this->_internal_w(), target);
+  }
+
+  // int32 end = 5;
+  if (this->end() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(5, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -729,23 +750,37 @@ size_t Speed::ByteSizeLong() const {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // int32 type = 1;
-  if (this->type() != 0) {
+  // int32 H = 1;
+  if (this->h() != 0) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-        this->_internal_type());
+        this->_internal_h());
   }
 
-  // float v = 2;
+  // int32 T = 2;
+  if (this->t() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_t());
+  }
+
+  // float V = 3;
   if (!(this->v() <= 0 && this->v() >= 0)) {
     total_size += 1 + 4;
   }
 
-  // float vth = 3;
-  if (!(this->vth() <= 0 && this->vth() >= 0)) {
+  // float W = 4;
+  if (!(this->w() <= 0 && this->w() >= 0)) {
     total_size += 1 + 4;
   }
 
+  // int32 end = 5;
+  if (this->end() != 0) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_end());
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -777,14 +812,20 @@ void Speed::MergeFrom(const Speed& from) {
   ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (from.type() != 0) {
-    _internal_set_type(from._internal_type());
+  if (from.h() != 0) {
+    _internal_set_h(from._internal_h());
+  }
+  if (from.t() != 0) {
+    _internal_set_t(from._internal_t());
   }
   if (!(from.v() <= 0 && from.v() >= 0)) {
     _internal_set_v(from._internal_v());
   }
-  if (!(from.vth() <= 0 && from.vth() >= 0)) {
-    _internal_set_vth(from._internal_vth());
+  if (!(from.w() <= 0 && from.w() >= 0)) {
+    _internal_set_w(from._internal_w());
+  }
+  if (from.end() != 0) {
+    _internal_set_end(from._internal_end());
   }
 }
 
@@ -810,11 +851,11 @@ void Speed::InternalSwap(Speed* other) {
   using std::swap;
   _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(Speed, vth_)
-      + sizeof(Speed::vth_)
-      - PROTOBUF_FIELD_OFFSET(Speed, type_)>(
-          reinterpret_cast<char*>(&type_),
-          reinterpret_cast<char*>(&other->type_));
+      PROTOBUF_FIELD_OFFSET(Speed, end_)
+      + sizeof(Speed::end_)
+      - PROTOBUF_FIELD_OFFSET(Speed, h_)>(
+          reinterpret_cast<char*>(&h_),
+          reinterpret_cast<char*>(&other->h_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata Speed::GetMetadata() const {
@@ -1833,8 +1874,6 @@ void NavCmd::InternalSwap(NavCmd* other) {
 // ===================================================================
 
 void NavStatu::InitAsDefaultInstance() {
-  ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->current_pose_ = const_cast< ::NavMessage::AGVStatu*>(
-      ::NavMessage::AGVStatu::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->selected_traj_ = const_cast< ::NavMessage::Trajectory*>(
       ::NavMessage::Trajectory::internal_default_instance());
   ::NavMessage::_NavStatu_default_instance_._instance.get_mutable()->predict_traj_ = const_cast< ::NavMessage::Trajectory*>(
@@ -1842,15 +1881,10 @@ void NavStatu::InitAsDefaultInstance() {
 }
 class NavStatu::_Internal {
  public:
-  static const ::NavMessage::AGVStatu& current_pose(const NavStatu* msg);
   static const ::NavMessage::Trajectory& selected_traj(const NavStatu* msg);
   static const ::NavMessage::Trajectory& predict_traj(const NavStatu* msg);
 };
 
-const ::NavMessage::AGVStatu&
-NavStatu::_Internal::current_pose(const NavStatu* msg) {
-  return *msg->current_pose_;
-}
 const ::NavMessage::Trajectory&
 NavStatu::_Internal::selected_traj(const NavStatu* msg) {
   return *msg->selected_traj_;
@@ -1875,11 +1909,6 @@ NavStatu::NavStatu(const NavStatu& from)
     key_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_key(),
       GetArena());
   }
-  if (from._internal_has_current_pose()) {
-    current_pose_ = new ::NavMessage::AGVStatu(*from.current_pose_);
-  } else {
-    current_pose_ = nullptr;
-  }
   if (from._internal_has_selected_traj()) {
     selected_traj_ = new ::NavMessage::Trajectory(*from.selected_traj_);
   } else {
@@ -1890,18 +1919,16 @@ NavStatu::NavStatu(const NavStatu& from)
   } else {
     predict_traj_ = nullptr;
   }
-  ::memcpy(&statu_, &from.statu_,
-    static_cast<size_t>(reinterpret_cast<char*>(&twist_timeout_) -
-    reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = from.statu_;
   // @@protoc_insertion_point(copy_constructor:NavMessage.NavStatu)
 }
 
 void NavStatu::SharedCtor() {
   ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_NavStatu_message_2eproto.base);
   key_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  ::memset(&current_pose_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&current_pose_)) + sizeof(twist_timeout_));
+  ::memset(&selected_traj_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&statu_) -
+      reinterpret_cast<char*>(&selected_traj_)) + sizeof(statu_));
 }
 
 NavStatu::~NavStatu() {
@@ -1913,7 +1940,6 @@ NavStatu::~NavStatu() {
 void NavStatu::SharedDtor() {
   GOOGLE_DCHECK(GetArena() == nullptr);
   key_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
-  if (this != internal_default_instance()) delete current_pose_;
   if (this != internal_default_instance()) delete selected_traj_;
   if (this != internal_default_instance()) delete predict_traj_;
 }
@@ -1941,10 +1967,6 @@ void NavStatu::Clear() {
 
   unfinished_actions_.Clear();
   key_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
-  if (GetArena() == nullptr && current_pose_ != nullptr) {
-    delete current_pose_;
-  }
-  current_pose_ = nullptr;
   if (GetArena() == nullptr && selected_traj_ != nullptr) {
     delete selected_traj_;
   }
@@ -1953,9 +1975,7 @@ void NavStatu::Clear() {
     delete predict_traj_;
   }
   predict_traj_ = nullptr;
-  ::memset(&statu_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&twist_timeout_) -
-      reinterpret_cast<char*>(&statu_)) + sizeof(twist_timeout_));
+  statu_ = false;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1995,37 +2015,16 @@ const char* NavStatu::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::i
           } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<26>(ptr));
         } else goto handle_unusual;
         continue;
-      // .NavMessage.AGVStatu current_pose = 4;
+      // .NavMessage.Trajectory selected_traj = 4;
       case 4:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
-          ptr = ctx->ParseMessage(_internal_mutable_current_pose(), ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool pose_timeout = 5;
-      case 5:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 40)) {
-          pose_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // bool twist_timeout = 6;
-      case 6:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 48)) {
-          twist_timeout_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // .NavMessage.Trajectory selected_traj = 7;
-      case 7:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 58)) {
           ptr = ctx->ParseMessage(_internal_mutable_selected_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
         continue;
-      // .NavMessage.Trajectory predict_traj = 8;
-      case 8:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 66)) {
+      // .NavMessage.Trajectory predict_traj = 5;
+      case 5:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 42)) {
           ptr = ctx->ParseMessage(_internal_mutable_predict_traj(), ptr);
           CHK_(ptr);
         } else goto handle_unusual;
@@ -2082,40 +2081,20 @@ failure:
       InternalWriteMessage(3, this->_internal_unfinished_actions(i), target, stream);
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
-      InternalWriteMessage(
-        4, _Internal::current_pose(this), target, stream);
-  }
-
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(5, this->_internal_pose_timeout(), target);
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 0) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteBoolToArray(6, this->_internal_twist_timeout(), target);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        7, _Internal::selected_traj(this), target, stream);
+        4, _Internal::selected_traj(this), target, stream);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     target = stream->EnsureSpace(target);
     target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
       InternalWriteMessage(
-        8, _Internal::predict_traj(this), target, stream);
+        5, _Internal::predict_traj(this), target, stream);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -2148,21 +2127,14 @@ size_t NavStatu::ByteSizeLong() const {
         this->_internal_key());
   }
 
-  // .NavMessage.AGVStatu current_pose = 4;
-  if (this->has_current_pose()) {
-    total_size += 1 +
-      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
-        *current_pose_);
-  }
-
-  // .NavMessage.Trajectory selected_traj = 7;
+  // .NavMessage.Trajectory selected_traj = 4;
   if (this->has_selected_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
         *selected_traj_);
   }
 
-  // .NavMessage.Trajectory predict_traj = 8;
+  // .NavMessage.Trajectory predict_traj = 5;
   if (this->has_predict_traj()) {
     total_size += 1 +
       ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
@@ -2174,16 +2146,6 @@ size_t NavStatu::ByteSizeLong() const {
     total_size += 1 + 1;
   }
 
-  // bool pose_timeout = 5;
-  if (this->pose_timeout() != 0) {
-    total_size += 1 + 1;
-  }
-
-  // bool twist_timeout = 6;
-  if (this->twist_timeout() != 0) {
-    total_size += 1 + 1;
-  }
-
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
         _internal_metadata_, total_size, &_cached_size_);
@@ -2219,9 +2181,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.key().size() > 0) {
     _internal_set_key(from._internal_key());
   }
-  if (from.has_current_pose()) {
-    _internal_mutable_current_pose()->::NavMessage::AGVStatu::MergeFrom(from._internal_current_pose());
-  }
   if (from.has_selected_traj()) {
     _internal_mutable_selected_traj()->::NavMessage::Trajectory::MergeFrom(from._internal_selected_traj());
   }
@@ -2231,12 +2190,6 @@ void NavStatu::MergeFrom(const NavStatu& from) {
   if (from.statu() != 0) {
     _internal_set_statu(from._internal_statu());
   }
-  if (from.pose_timeout() != 0) {
-    _internal_set_pose_timeout(from._internal_pose_timeout());
-  }
-  if (from.twist_timeout() != 0) {
-    _internal_set_twist_timeout(from._internal_twist_timeout());
-  }
 }
 
 void NavStatu::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
@@ -2263,11 +2216,11 @@ void NavStatu::InternalSwap(NavStatu* other) {
   unfinished_actions_.InternalSwap(&other->unfinished_actions_);
   key_.Swap(&other->key_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(NavStatu, twist_timeout_)
-      + sizeof(NavStatu::twist_timeout_)
-      - PROTOBUF_FIELD_OFFSET(NavStatu, current_pose_)>(
-          reinterpret_cast<char*>(&current_pose_),
-          reinterpret_cast<char*>(&other->current_pose_));
+      PROTOBUF_FIELD_OFFSET(NavStatu, statu_)
+      + sizeof(NavStatu::statu_)
+      - PROTOBUF_FIELD_OFFSET(NavStatu, selected_traj_)>(
+          reinterpret_cast<char*>(&selected_traj_),
+          reinterpret_cast<char*>(&other->selected_traj_));
 }
 
 ::PROTOBUF_NAMESPACE_ID::Metadata NavStatu::GetMetadata() const {

projects/controllers/agv_child/emqx-client/message.pb.h

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

projects/webots_project/AGV.wbt

@@ -12,12 +12,12 @@ WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.5760441325065558 0.5751112137241222 0.5808788593618609 4.182587952469744
-  position -3.538070276713455 126.88364004394931 51.801022262364185
+  orientation 0.5818033438377976 0.5693298869845917 0.5808341836404513 4.18252091947868
+  position 2.8316854176465314 124.06549024084453 47.35952361688281
 }
 DEF BASE Solid {
-  translation 19.999990830377016 0.7911399999816232 0.00016165862726908996
-  rotation -2.0130560254593927e-15 1 7.363074259590324e-12 -0.0015310670374691837
+  translation 19.999990830376994 0.7911399999999998 0.00016165862726906342
+  rotation 3.813686307174172e-15 1 -7.392696162231656e-14 -0.0015310670374691813
   children [
     DEF BASE Shape {
       appearance Appearance {
@@ -35,12 +35,12 @@ DEF BASE Solid {
   physics Physics {
     density 50
   }
-  linearVelocity -1.4743974668906547e-13 4.03055963476773e-11 -1.7297234650653956e-16
-  angularVelocity -1.5853179696867315e-16 -3.3832790224174415e-18 1.474749008551123e-13
+  linearVelocity 6.859325028145343e-17 4.291439081864306e-16 2.487177595931127e-17
+  angularVelocity 3.641100384136907e-17 4.184616225450991e-18 -9.31478056624474e-17
 }
 DEF Foot Solid {
-  translation 9.999393453313743 0.007628624545976362 -0.08621733915398472
-  rotation -0.5617423039575042 0.5636575435204496 -0.6055871180738632 2.0620628463374904
+  translation 9.999393453313743 0.007628624545976334 -0.08621733915398466
+  rotation -0.5617423039575042 0.5636575435204495 -0.6055871180738632 2.062062846337491
   children [
     DEF FootGroup Group {
       children [
@@ -305,8 +305,8 @@ DEF Foot Solid {
   physics Physics {
     density 9800
   }
-  linearVelocity -3.6974789832420584e-16 -2.650470768245878e-16 1.7522337911886197e-16
-  angularVelocity 2.6013308475630827e-15 2.3889030657730265e-17 5.552351041726368e-15
+  linearVelocity -2.6835060801495474e-16 -1.6305332766423367e-16 2.379287783062859e-16
+  angularVelocity 3.71681486153735e-15 2.1903057875089765e-17 4.014447047551636e-15
 }
 DEF BODY Solid {
   translation 10 0.1 -1.80464e-16
@@ -547,8 +547,8 @@ DEF BODY Solid {
   boundingObject USE BODY_G
 }
 DEF AGV Robot {
-  translation -15.946900645151336 0.5039311169807567 79.9995547312277
-  rotation 0.00026382617979766964 -0.9999999620814533 7.894833200944853e-05 1.5687613041805686
+  translation 1.20997 0.5081 30
+  rotation 0.00028790749930202177 -0.9999997701294638 -0.0006138813332151157 1.5688474942134538
   children [
     Gyro {
       translation 0 0.4 0
@@ -599,7 +599,7 @@ DEF AGV Robot {
     }
     DEF LFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -1.6015124459349117e-06
+        position -1.79684045043229e-06
         axis 0 1 0
         anchor 1.25 -0.1 0.65
       }
@@ -611,12 +611,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2498259680852588 -0.3173116083844618 0.6500300313820206
-        rotation -0.3450053983768298 0.08857329698477825 -0.9344121393432833 0.0001773757543540236
+        translation 1.2498259680793753 -0.31731160838446126 0.6500300313479261
+        rotation -0.3450388528276186 0.0874773040788691 -0.9345030290536752 0.00017735837807788534
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12714.789584591206
+              position 12714.792964099648
             }
             device [
               RotationalMotor {
@@ -626,8 +626,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.370593425440064e-05 0.00023896694069727465 -0.0011520793543657014
-              rotation -0.9999999986109445 -9.334247343122127e-07 -5.2699524128607924e-05 2.377561881515879
+              translation 4.3705934254399906e-05 0.0002428590305611499 -0.0011512651861221195
+              rotation -0.999999998609062 -8.449461872153176e-07 -5.273672628566991e-05 2.3741823727955986
               children [
                 DEF WHEEL Shape {
                   appearance Appearance {
@@ -664,8 +664,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -1.727708208910232e-06 -3.0353411996579134e-07 5.383560198155585e-07
-              angularVelocity -3.915476205628914e-07 1.6731454221005538e-05 3.469523783188443e-06
             }
           }
           DEF FootGroup Group {
@@ -931,13 +929,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.2330555464911483e-07 -3.0326963883448886e-07 3.3567187463498345e-07
-        angularVelocity -3.528672825937743e-07 1.6843569043173982e-05 4.769569829882877e-07
       }
     }
     DEF LBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position 1.2907280425126706e-06
+        position 1.909799640451601e-07
         axis 0 1 0
         anchor 1.25 -0.1 -0.65
       }
@@ -948,8 +944,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation 1.250214294502471 -0.3171563536107086 -0.6499857456769544
-        rotation -0.44785497304850674 0.5238565819886335 0.7245689785126791 0.0001523164772456462
+        translation 1.2502142944867947 -0.3171563536107094 -0.6499857454412845
+        rotation -0.44954716989579785 0.518588054893846 0.7273058306930422 0.00015174308343144375
         children [
           DEF RightFootGroup Group {
             children [
@@ -1211,7 +1207,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12089.811925323958
+              position 12089.81243920925
             }
             device [
               RotationalMotor {
@@ -1221,8 +1217,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation -0.0006519639232456149 0.0009472563220323584 0.0011432277458326742
-              rotation -0.9999999911249553 7.64477561834716e-05 -0.00010911383865990992 1.0007802720565455
+              translation -0.0006519639232456022 0.000946668709059292 0.001143714375952278
+              rotation -0.9999999911166082 7.651174463449885e-05 -0.00010914548283850542 1.0002663867689645
               children [
                 USE WHEEL
               ]
@@ -1231,8 +1227,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -1.4005023627713521e-06 2.2229711255853747e-07 -6.010689074877271e-07
-              angularVelocity -2.780881107085909e-07 1.291395600471874e-05 4.433979964711076e-06
             }
           }
         ]
@@ -1241,13 +1235,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -3.172669598689421e-07 2.806869145038782e-07 -7.612760617516632e-07
-        angularVelocity -2.7670030220336e-07 1.3105466716609632e-05 5.382069649055033e-07
       }
     }
     DEF RFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -2.4976438233409734e-06
+        position -8.891405361579447e-07
         axis 0 1 0
         anchor -1.25 -0.1 0.65
       }
@@ -1258,8 +1250,8 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2499103940164846 -0.31904283227123426 0.6499825579176793
-        rotation -5.223018610432791e-05 0.9999999976721874 -4.3904819439063606e-05 3.1416192075632288
+        translation -1.2499103940445404 -0.31904283227123564 0.6499825577735479
+        rotation -5.22302214154086e-05 0.9999999976721874 -4.3904777433319685e-05 3.1416208160665122
         children [
           DEF RightFootGroup Group {
             children [
@@ -1521,7 +1513,7 @@ DEF AGV Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9401.40779109314
+              position -9401.409793096984
             }
             device [
               RotationalMotor {
@@ -1531,8 +1523,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 1.597498009431313e-07 -3.934082001063387e-05 -1.2408942170803014e-05
-              rotation 0.9999999999913047 1.05247409519458e-06 4.035205703343735e-06 1.0199533831481333
+              translation 1.5974980094312817e-07 -3.936558390461395e-05 -1.2330156882871794e-05
+              rotation 0.9999999999912735 1.058407525554635e-06 4.041384843813483e-06 1.017951379303512
               children [
                 USE WHEEL
               ]
@@ -1541,8 +1533,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -3.5807286019047e-06 -1.0498590057030663e-06 -2.5158759757209607e-07
-              angularVelocity -6.433116903245698e-08 -2.5368328015348077e-05 4.192053624517757e-05
             }
           }
         ]
@@ -1551,13 +1541,11 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -9.486386621055678e-07 -1.0404307648840248e-06 -2.13946869689786e-07
-        angularVelocity -1.7910548854494414e-07 -2.5860798433741353e-05 4.099395880747905e-07
       }
     }
     DEF RBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position 2.5921654917808282e-06
+        position -2.0564580800644458e-08
         axis 0 1 0
         anchor -1.25 -0.1 -0.65
       }
@@ -1568,12 +1556,12 @@ DEF AGV Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2500226470364457 -0.31905992517371873 -0.6499245257802676
-        rotation -0.00021957094320641967 -0.9999993153212807 0.0011494109669191663 3.1415684380391338
+        translation -1.2500226472337264 -0.3190599251737202 -0.6499245258394647
+        rotation -0.00021957244541735292 -0.9999993153212807 0.0011494106799343137 3.1415710519229796
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -9666.452738828799
+              position -9666.451849226816
             }
             device [
               RotationalMotor {
@@ -1583,8 +1571,8 @@ DEF AGV Robot {
               }
             ]
             endPoint Solid {
-              translation 4.1127246297456296e-07 -1.4630078512474653e-05 -0.00028848440065958443
-              rotation -0.9999999999999168 9.063976262897537e-08 3.9719911493225113e-07 2.913767801326471
+              translation 4.1127246297455867e-07 -1.4373436462477738e-05 -0.0002884973014526482
+              rotation -0.999999999999917 9.046138582306803e-08 3.972319567795937e-07 2.9128781971540816
               children [
                 USE WHEEL
               ]
@@ -1593,8 +1581,6 @@ DEF AGV Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -2.7050782017270717e-06 -2.989862229640371e-07 -1.0216865106350463e-06
-              angularVelocity -1.8550437718207525e-07 2.5509255991253717e-05 -2.4088252588814306e-06
             }
           }
           DEF FootGroup Group {
@@ -1861,8 +1847,6 @@ DEF AGV Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -5.469240021789478e-06 -3.429797884439069e-07 -1.0514047337308335e-06
-        angularVelocity -2.034954716648684e-07 2.597696439158162e-05 5.505752150300793e-07
       }
     }
     USE BODY_G
@@ -1872,12 +1856,10 @@ DEF AGV Robot {
     density 300
   }
   controller "AGV_controller"
-  linearVelocity -2.796202352887268e-06 -3.576053846651072e-07 -5.731427562516054e-07
-  angularVelocity -2.512255417707071e-07 6.088615247717306e-07 4.926256022861004e-07
 }
 DEF AGV_Child Robot {
-  translation 0.005468414267849941 0.5088081562898559 0.07836090364311495
-  rotation 0.06335501549251402 -0.9954850215202791 0.07068036460512098 -0.004137019583816275
+  translation 0.012501861789795291 0.5077791024641277 2.6383865754479874e-08
+  rotation 0.027669147222891582 -0.9615319771236447 -0.2732959481233722 0.0038261300703193997
   children [
     Gyro {
       translation 0 0.4 0
@@ -1928,7 +1910,7 @@ DEF AGV_Child Robot {
     }
     DEF LFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -4.451406787659149e-10
+        position -2.4802110916695665e-06
         axis 0 1 0
         anchor 1.25 -0.1 0.65
       }
@@ -1940,12 +1922,12 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2498259681333412 -0.31731160838446193 0.6500300316606574
-        rotation -0.3447166241741208 0.09751780846079272 -0.9336277234802971 0.00017752472237173315
+        translation 1.2498259680588435 -0.31731160838446193 0.6500300312289441
+        rotation -0.34515237513599833 0.08365011509426826 -0.9348114762788778 0.00017730000736543874
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 12008.533465858332
+              position 12307.700467152443
             }
             device [
               RotationalMotor {
@@ -1955,8 +1937,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 4.370593425440064e-05 -0.0008495568944899791 0.0008140301717543697
-              rotation -0.9999999969982798 -7.439105861020821e-05 -2.166589094163871e-05 4.916926206393822
+              translation 4.370593425440063e-05 0.0011752270732034837 -5.686267590853934e-05
+              rotation 0.9999999953171763 -5.7891599477254037e-05 7.75513385988885e-05 5.223550950436673
               children [
                 USE WHEEL
               ]
@@ -1965,8 +1947,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -7.590141950721092e-09 -3.701237361046493e-09 5.436610667276621e-08
-              angularVelocity -1.5649601169014019e-06 -3.207858963083751e-08 8.727580857985011e-09
+              linearVelocity -1.2463705531961533e-07 2.702934390871517e-09 1.2584170670532758e-08
+              angularVelocity 4.864752173035862e-07 -2.3303108111081817e-05 -6.949369883311067e-08
             }
           }
           DEF FootGroup Group {
@@ -2232,13 +2214,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.4705956834470104e-08 -3.7008021795088694e-09 7.117046715297314e-08
-        angularVelocity 5.684033606453133e-09 -3.301502751849706e-08 9.140621231128934e-10
+        linearVelocity -1.274852691462624e-07 -1.3150612376118644e-09 2.1328814995306245e-06
+        angularVelocity -1.0591834477549399e-07 -2.3659658762424915e-05 -5.0408640937264485e-08
       }
     }
     DEF LBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position 2.1104925802873383e-09
+        position -1.878497285546203e-06
         axis 0 1 0
         anchor 1.25 -0.1 -0.65
       }
@@ -2249,8 +2231,8 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation 1.2502142944841024 -0.31715635361070915 -0.6499857454008109
-        rotation -0.4498373380083578 0.5176772454445777 0.7277751293376745 0.00015164540058941302
+        translation 1.2502142944572947 -0.3171563536107094 -0.649985744997807
+        rotation -0.4527187831722299 0.5085116796910004 0.7324353725625852 0.00015068044182219758
         children [
           DEF RightFootGroup Group {
             children [
@@ -2512,7 +2494,7 @@ DEF AGV_Child Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position 11382.274234647171
+              position 11681.691492248954
             }
             device [
               RotationalMotor {
@@ -2522,8 +2504,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation -0.0006519639232456151 -0.0014552214655460254 -0.00029426978214897764
-              rotation -0.9999999954139505 -9.226365511990913e-05 -2.5681062504683e-05 4.821716532699311
+              translation -0.0006519639232456147 0.0005854200882226155 0.001364385406979036
+              rotation 0.9999999832802581 -0.00012524187577806436 0.00013324397157103812 5.5690169233124855
               children [
                 USE WHEEL
               ]
@@ -2532,8 +2514,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 8.58567779744139e-09 -1.0650549693630678e-09 5.001518135976187e-08
-              angularVelocity 2.1340102835272386e-06 -3.318348065840496e-08 -1.4358406221785835e-08
+              linearVelocity 2.9403652998287883e-08 -7.67486106471827e-09 -4.0037267568602437e-08
+              angularVelocity 3.772312796100735e-07 -1.7661033105430275e-05 5.2192781031318894e-08
             }
           }
         ]
@@ -2542,13 +2524,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity 1.544816420750473e-08 -3.970866165404896e-09 7.092843986268032e-08
-        angularVelocity 5.61647755831843e-09 -3.2995950631413276e-08 -4.298844913270526e-09
+        linearVelocity 1.5362640445851933e-09 -4.706795349483696e-09 1.5689997941535845e-06
+        angularVelocity -8.549362698362294e-08 -1.7916506073771674e-05 3.334017753199761e-08
       }
     }
     DEF RFJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -4.1415169771650835e-10
+        position -2.470709389425638e-06
         axis 0 1 0
         anchor -1.25 -0.1 0.65
       }
@@ -2559,8 +2541,8 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2499103940600418 -0.3190428322712342 0.6499825576939127
-        rotation -5.223024092540276e-05 0.9999999976721873 -4.390475422440621e-05 3.1416217047928945
+        translation -1.2499103940169398 -0.3190428322712342 0.64998255791534
+        rotation -5.223018667742951e-05 0.9999999976721873 -4.39048187573072e-05 3.141619233669359
         children [
           DEF RightFootGroup Group {
             children [
@@ -2822,7 +2804,7 @@ DEF AGV_Child Robot {
           }
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -8714.882772071418
+              position -9013.053850905386
             }
             device [
               RotationalMotor {
@@ -2832,8 +2814,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 1.5974980094313124e-07 1.5781268263109487e-05 -3.811342987603186e-05
-              rotation -0.9999999999978124 1.1353871994264502e-06 -1.756742580399652e-06 3.6054113848916365
+              translation 1.5974980094313124e-07 -2.5706738710861403e-05 3.226213802212933e-05
+              rotation -0.999999999753419 1.6784791061780748e-05 1.4540720922414024e-05 0.1835954742853224
               children [
                 USE WHEEL
               ]
@@ -2842,8 +2824,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity -7.84973082292511e-09 1.2050622038630861e-09 2.126243545329618e-08
-              angularVelocity -4.302750942731766e-07 -3.2062431374565554e-08 3.995725466255144e-09
+              linearVelocity -9.479216871629677e-09 6.193487328952021e-09 -1.9738256271295776e-08
+              angularVelocity -4.186455017932854e-07 -2.3212574741223283e-05 -4.046487527771698e-08
             }
           }
         ]
@@ -2852,13 +2834,13 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.4961256092007293e-08 1.0932666758160638e-09 7.355687156660066e-09
-        angularVelocity -4.274974439913585e-09 -3.230778099368917e-08 9.325216870257438e-10
+        linearVelocity -7.848574641127519e-09 9.208588502326832e-09 -2.14115896295869e-06
+        angularVelocity 3.7247946052213685e-08 -2.3599254956502662e-05 -2.7725253756786877e-08
       }
     }
     DEF RBJoint HingeJoint {
       jointParameters HingeJointParameters {
-        position -6.847871876459744e-07
+        position -2.0782248245816432e-08
         axis 0 1 0
         anchor -1.25 -0.1 -0.65
       }
@@ -2869,12 +2851,12 @@ DEF AGV_Child Robot {
         }
       ]
       endPoint Solid {
-        translation -1.2500226472837588 -0.3190599251737186 -0.6499245258544777
-        rotation -0.00021957282639143864 -0.9999993153212807 0.001149410607152393 3.141571714827227
+        translation -1.2500226472336593 -0.3190599251737188 -0.6499245258394445
+        rotation -0.00021957244490572204 -0.9999993153212808 0.0011494106800320546 3.141571051032729
         children [
           HingeJoint {
             jointParameters HingeJointParameters {
-              position -8983.136951173221
+              position -9281.255272816281
             }
             device [
               RotationalMotor {
@@ -2884,8 +2866,8 @@ DEF AGV_Child Robot {
               }
             ]
             endPoint Solid {
-              translation 4.112724629745623e-07 -0.0002887137901050157 9.035265596411059e-06
-              rotation 0.9999999999998684 -4.317639549020174e-07 -2.7719525673321773e-07 1.8180066788562141
+              translation 4.112724629745624e-07 0.0002758086978941395 8.5830360520856e-05
+              rotation 0.9999999999996374 5.724838541247699e-07 -6.304785558795505e-07 5.292579780444217
               children [
                 USE WHEEL
               ]
@@ -2894,8 +2876,8 @@ DEF AGV_Child Robot {
               physics Physics {
                 density 100
               }
-              linearVelocity 3.37044996116767e-09 9.017022664158151e-10 1.0896609711750992e-08
-              angularVelocity 6.247173628872931e-07 6.694857021364571e-06 -1.0858316090156255e-08
+              linearVelocity 2.9065478756883955e-08 -4.037445362981478e-09 -5.588226143914096e-08
+              angularVelocity -4.7563380471949156e-07 3.9043303971723615e-07 4.468780733155552e-08
             }
           }
           DEF FootGroup Group {
@@ -3162,8 +3144,8 @@ DEF AGV_Child Robot {
         physics Physics {
           density 500
         }
-        linearVelocity -1.98542677026058e-08 1.4282384946111838e-09 6.536758323543926e-07
-        angularVelocity -6.008929777255136e-09 6.786382769550092e-06 -6.799210617229629e-09
+        linearVelocity -5.942246879835356e-08 -7.039117750791243e-09 1.2946692850208225e-07
+        angularVelocity 4.771892723355824e-08 3.935196247470993e-07 3.139072412216777e-08
       }
     }
     USE BODY_G
@@ -3174,8 +3156,8 @@ DEF AGV_Child Robot {
     density 300
   }
   controller "agv_child"
-  linearVelocity 1.2627097765082106e-09 -1.3402399794735552e-09 4.122021380587888e-08
-  angularVelocity -2.1880775389469515e-10 -3.289505212922818e-08 -1.866424737029179e-09
+  linearVelocity -1.18411132873794e-08 -6.094527601784615e-10 -1.4096933303990628e-08
+  angularVelocity -8.956879261476707e-09 1.9204272072681243e-07 -1.533348904728203e-09
 }
 TexturedBackground {
 }