1.添加预判延迟后下发速度与角速度。

2.入库前旋转时，开启另一车位姿超时判断

MPC/loaded_mpc.cpp

@@ -8,6 +8,7 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t N = 15;                  //优化考虑后面多少步
+size_t delay = 3;  // 发送到执行的延迟，即几个周期的时间
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -245,6 +246,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
      * 障碍物是否进入 碰撞检测范围内
      */
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
+    distance = 1000;
     bool find_obs = distance < 10;
     if (find_obs) printf(" find obs ..............................\n");
     size_t n_constraints = 5 * N + find_obs * N/2;
@@ -259,11 +261,21 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     for (int i = nv; i < nv + N; i++) {
         constraints_lowerbound[i] = -max_acc_line_velocity;
         constraints_upperbound[i] = max_acc_line_velocity;
+        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
+        if (i < nv+delay) {
+            constraints_lowerbound[i] = 0;
+            constraints_upperbound[i] = 0;
+        }
     }
     //// acc ndlt
     for (int i = ndlt; i < ndlt + N; i++) {
         constraints_lowerbound[i] = -max_acc_dlt;
         constraints_upperbound[i] = max_acc_dlt;
+        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
+        if (i < nv+delay){
+            constraints_lowerbound[i] = 0;
+            constraints_upperbound[i] = 0;
+        }
     }
 
     // 与障碍物保持距离的约束
@@ -341,8 +353,8 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     out.clear();
 
-    double solve_velocity = solution.x[nv];
-    double solve_angular = solution.x[ndlt];
+    double solve_velocity = solution.x[nv+delay];
+    double solve_angular = solution.x[ndlt+delay];
 
     //纠正角速度/线速度，使其满足最小转弯半径
     double correct_angular = solve_angular;

MPC/monitor/emqx/message.pb.cc

@@ -84,6 +84,10 @@ PROTOBUF_CONSTEXPR ToAgvCmd::ToAgvCmd(
 
   , /*decltype(_impl_.l_)*/ 0
 
+  , /*decltype(_impl_.p_)*/ 0
+
+  , /*decltype(_impl_.d_)*/ 0
+
   , /*decltype(_impl_.end_)*/ 0
 
   , /*decltype(_impl_._cached_size_)*/{}} {}
@@ -353,6 +357,8 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.v_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.w_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.l_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.p_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.d_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.end_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
@@ -491,15 +497,15 @@ static const ::_pbi::MigrationSchema
         { 0, -1, -1, sizeof(::NavMessage::LidarOdomStatu)},
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
         { 27, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
-        { 42, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 53, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 67, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 76, 91, -1, sizeof(::NavMessage::NewAction)},
-        { 98, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 109, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 119, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 130, 147, -1, sizeof(::NavMessage::NavStatu)},
-        { 156, 168, -1, sizeof(::NavMessage::RobotStatu)},
+        { 44, -1, -1, sizeof(::NavMessage::Pose2d)},
+        { 55, -1, -1, sizeof(::NavMessage::PathNode)},
+        { 69, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 78, 93, -1, sizeof(::NavMessage::NewAction)},
+        { 100, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 111, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 121, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 132, 149, -1, sizeof(::NavMessage::NavStatu)},
+        { 158, 170, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -522,44 +528,44 @@ const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE
     "(\002\022\t\n\001v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"j\n\010AgvStatu\022\t"
     "\n\001v\030\001 \001(\002\022\t\n\001w\030\002 \001(\002\022\r\n\005clamp\030\003 \001(\005\022\023\n\013c"
     "lamp_other\030\004 \001(\005\022\016\n\006lifter\030\005 \001(\005\022\024\n\014lift"
-    "er_other\030\006 \001(\005\"Y\n\010ToAgvCmd\022\t\n\001H\030\001 \001(\005\022\t\n"
+    "er_other\030\006 \001(\005\"o\n\010ToAgvCmd\022\t\n\001H\030\001 \001(\005\022\t\n"
     "\001M\030\002 \001(\005\022\t\n\001T\030\003 \001(\005\022\t\n\001V\030\004 \001(\002\022\t\n\001W\030\005 \001("
-    "\002\022\t\n\001L\030\006 \001(\002\022\013\n\003end\030\007 \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\"Q\n\010Path"
-    "Node\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n"
-    "\001w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nT"
-    "rajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Po"
-    "se2d\"\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspac"
-    "eNode\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010pas"
-    "sNode\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstr"
-    "eetNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tp"
-    "athNodes\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\t"
-    "wheelbase\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006"
-    "NavCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nn"
-    "ewActions\030\005 \003(\0132\025.NavMessage.NewAction\"("
-    "\n\013NavResponse\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t"
-    "\"B\n\tManualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_"
-    "type\030\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStat"
-    "u\022\r\n\005statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmo"
-    "ve_mode\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage"
-    ".LidarOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected"
-    "_traj\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014p"
-    "redict_traj\030\007 \001(\0132\026.NavMessage.Trajector"
-    "y\022\020\n\010in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n"
-    "\nRobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005the"
-    "ta\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage."
-    "AgvStatu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMe"
-    "ssage.NavCmd\032\027.NavMessage.NavResponse\"\000\022"
-    "7\n\006Cancel\022\022.NavMessage.NavCmd\032\027.NavMessa"
-    "ge.NavResponse\"\000\022C\n\017ManualOperation\022\025.Na"
-    "vMessage.ManualCmd\032\027.NavMessage.NavRespo"
-    "nse\"\000b\006proto3"
+    "\002\022\t\n\001L\030\006 \001(\002\022\t\n\001P\030\007 \001(\005\022\t\n\001D\030\010 \001(\002\022\013\n\003en"
+    "d\030\t \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\"Q\n\010PathNode\022\t\n\001x\030\001 \001(\002\022\t\n"
+    "\001y\030\002 \001(\002\022\t\n\001l\030\003 \001(\002\022\t\n\001w\030\004 \001(\002\022\r\n\005theta\030"
+    "\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nTrajectory\022!\n\005poses"
+    "\030\001 \003(\0132\022.NavMessage.Pose2d\"\345\001\n\tNewAction"
+    "\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001(\0132\024.NavM"
+    "essage.PathNode\022&\n\010passNode\030\003 \001(\0132\024.NavM"
+    "essage.PathNode\022(\n\nstreetNode\030\004 \001(\0132\024.Na"
+    "vMessage.PathNode\022\'\n\tpathNodes\030\005 \003(\0132\024.N"
+    "avMessage.PathNode\022\021\n\twheelbase\030\006 \001(\002\022\023\n"
+    "\013changedMode\030\007 \001(\005\"P\n\006NavCmd\022\016\n\006action\030\001"
+    " \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewActions\030\005 \003(\0132\025."
+    "NavMessage.NewAction\"(\n\013NavResponse\022\013\n\003r"
+    "et\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManualCmd\022\013\n\003k"
+    "ey\030\001 \001(\t\022\026\n\016operation_type\030\002 \001(\005\022\020\n\010velo"
+    "city\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005statu\030\001 \001(\005\022\020"
+    "\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mode\030\003 \001(\005\022(\n\004o"
+    "dom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013\n"
+    "\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.NavM"
+    "essage.Trajectory\022,\n\014predict_traj\030\007 \001(\0132"
+    "\026.NavMessage.Trajectory\022\020\n\010in_space\030\010 \001("
+    "\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobotStatu\022\t\n\001x\030\001"
+    " \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 \001(\002\022&\n\010agvSta"
+    "tu\030\004 \001(\0132\024.NavMessage.AgvStatu2\302\001\n\nNavEx"
+    "cutor\0226\n\005Start\022\022.NavMessage.NavCmd\032\027.Nav"
+    "Message.NavResponse\"\000\0227\n\006Cancel\022\022.NavMes"
+    "sage.NavCmd\032\027.NavMessage.NavResponse\"\000\022C"
+    "\n\017ManualOperation\022\025.NavMessage.ManualCmd"
+    "\032\027.NavMessage.NavResponse\"\000b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1453,
+    1475,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -1305,6 +1311,10 @@ inline void ToAgvCmd::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.l_) { 0 }
 
+    , decltype(_impl_.p_) { 0 }
+
+    , decltype(_impl_.d_) { 0 }
+
     , decltype(_impl_.end_) { 0 }
 
     , /*decltype(_impl_._cached_size_)*/{}
@@ -1400,9 +1410,27 @@ const char* ToAgvCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
-      // int32 end = 7;
+      // int32 P = 7;
       case 7:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 56)) {
+          _impl_.p_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float D = 8;
+      case 8:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 69)) {
+          _impl_.d_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 end = 9;
+      case 9:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 72)) {
           _impl_.end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
@@ -1492,11 +1520,29 @@ failure:
         6, this->_internal_l(), target);
   }
 
-  // int32 end = 7;
+  // int32 P = 7;
+  if (this->_internal_p() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        7, this->_internal_p(), target);
+  }
+
+  // float D = 8;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d = this->_internal_d();
+  ::uint32_t raw_d;
+  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
+  if (raw_d != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        8, this->_internal_d(), target);
+  }
+
+  // int32 end = 9;
   if (this->_internal_end() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        7, this->_internal_end(), target);
+        9, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1560,7 +1606,22 @@ failure:
     total_size += 5;
   }
 
-  // int32 end = 7;
+  // int32 P = 7;
+  if (this->_internal_p() != 0) {
+    total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
+        this->_internal_p());
+  }
+
+  // float D = 8;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d = this->_internal_d();
+  ::uint32_t raw_d;
+  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
+  if (raw_d != 0) {
+    total_size += 5;
+  }
+
+  // int32 end = 9;
   if (this->_internal_end() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
         this->_internal_end());
@@ -1614,6 +1675,16 @@ void ToAgvCmd::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   if (raw_l != 0) {
     _this->_internal_set_l(from._internal_l());
   }
+  if (from._internal_p() != 0) {
+    _this->_internal_set_p(from._internal_p());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_d = from._internal_d();
+  ::uint32_t raw_d;
+  memcpy(&raw_d, &tmp_d, sizeof(tmp_d));
+  if (raw_d != 0) {
+    _this->_internal_set_d(from._internal_d());
+  }
   if (from._internal_end() != 0) {
     _this->_internal_set_end(from._internal_end());
   }

MPC/monitor/emqx/message.pb.h

@@ -674,7 +674,9 @@ class ToAgvCmd final :
     kVFieldNumber = 4,
     kWFieldNumber = 5,
     kLFieldNumber = 6,
-    kEndFieldNumber = 7,
+    kPFieldNumber = 7,
+    kDFieldNumber = 8,
+    kEndFieldNumber = 9,
   };
   // int32 H = 1;
   void clear_h() ;
@@ -736,7 +738,27 @@ class ToAgvCmd final :
   void _internal_set_l(float value);
 
   public:
-  // int32 end = 7;
+  // int32 P = 7;
+  void clear_p() ;
+  ::int32_t p() const;
+  void set_p(::int32_t value);
+
+  private:
+  ::int32_t _internal_p() const;
+  void _internal_set_p(::int32_t value);
+
+  public:
+  // float D = 8;
+  void clear_d() ;
+  float d() const;
+  void set_d(float value);
+
+  private:
+  float _internal_d() const;
+  void _internal_set_d(float value);
+
+  public:
+  // int32 end = 9;
   void clear_end() ;
   ::int32_t end() const;
   void set_end(::int32_t value);
@@ -760,6 +782,8 @@ class ToAgvCmd final :
     float v_;
     float w_;
     float l_;
+    ::int32_t p_;
+    float d_;
     ::int32_t end_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
@@ -2995,7 +3019,47 @@ inline void ToAgvCmd::_internal_set_l(float value) {
   _impl_.l_ = value;
 }
 
-// int32 end = 7;
+// int32 P = 7;
+inline void ToAgvCmd::clear_p() {
+  _impl_.p_ = 0;
+}
+inline ::int32_t ToAgvCmd::p() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.P)
+  return _internal_p();
+}
+inline void ToAgvCmd::set_p(::int32_t value) {
+  _internal_set_p(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.P)
+}
+inline ::int32_t ToAgvCmd::_internal_p() const {
+  return _impl_.p_;
+}
+inline void ToAgvCmd::_internal_set_p(::int32_t value) {
+  ;
+  _impl_.p_ = value;
+}
+
+// float D = 8;
+inline void ToAgvCmd::clear_d() {
+  _impl_.d_ = 0;
+}
+inline float ToAgvCmd::d() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.D)
+  return _internal_d();
+}
+inline void ToAgvCmd::set_d(float value) {
+  _internal_set_d(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.D)
+}
+inline float ToAgvCmd::_internal_d() const {
+  return _impl_.d_;
+}
+inline void ToAgvCmd::_internal_set_d(float value) {
+  ;
+  _impl_.d_ = value;
+}
+
+// int32 end = 9;
 inline void ToAgvCmd::clear_end() {
   _impl_.end_ = 0;
 }

MPC/navigation.cpp

@@ -598,11 +598,11 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
 
 Navigation::MpcResult
 Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
-//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
 //    if (timedPose_.timeout()) {
-//        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
-//        return eMpcFailed;
-//    }
+        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
+        return eMpcFailed;
+    }
     NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();
 
     stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
@@ -703,10 +703,6 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
         }
-        //进库前提升机构上升
-        if (lifter_rise() == false){
-            printf(" Enter space | lifter rise failed, line:%d\n",__LINE__);
-        }
         //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
         printf("inout ----------------------------------------\n");
         //计算车位朝向
@@ -716,6 +712,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
             return false;
         }
+        //进库前提升机构上升
+        if (lifter_rise() == false){
+            printf(" Enter space | lifter rise failed, line:%d\n",__LINE__);
+        }
+
         if (MoveToTarget(new_target, limit_v, limit_w, true, false) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -1034,7 +1035,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     }
     printf(" exec along autoDirect:%d\n", autoDirect);
     while (cancel_ == false) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
         if (pause_ == true) {
             //发送暂停消息
             continue;
@@ -1078,6 +1079,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             if (out[0] < 0 && out[0] > -limit_v.min)
                 out[0] = -limit_v.min;
         }
+        //行进方向上最后一米不纠偏
+        if(fabs(target_in_agv.x()) < 1.4){
+            out[1] = 0.0;
+        }
         //下发速度
         //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         if (directY == false)

MPC/navigation_main.cpp

@@ -192,8 +192,8 @@ bool NavigationMain::lifter_down() {
 
 Navigation::MpcResult
 NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
-//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
-    if (timedPose_.timeout()) {
+    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
+//    if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
         return eMpcFailed;
     }

MPC/rotate_mpc.cpp

@@ -8,6 +8,7 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t mpc_rotate_steps = 20;                  //优化考虑后面多少步
+size_t delay = 3;  // 发送到执行的延迟，即几个周期的时间
 #define N mpc_rotate_steps
 
 class FG_eval_rotate {
@@ -159,7 +160,7 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     float distance = 1000;//屏蔽障碍物
     bool find_obs = distance < 5;
     if (find_obs) printf(" find obs ..............................\n");
-    size_t n_constraints = 2 * N + find_obs * N;// 0~N角度，N~2N角加速度, 2N~3N障碍物约束
+    size_t n_constraints = 2 * N + find_obs * N;// 0~N角度约束，N~2N角加速度约束, 2N~3N障碍物约束
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);
     for (auto i = 0; i < n_constraints; ++i) {