1.巡线旋转，并行指令，并修改通信协议

2.调整巡线，旋转速度加速度曲线。
3.载车板通信（未测试）

MPC/custom_type.h

@@ -103,9 +103,9 @@ enum ActionType {
     eLifterDown =8,
     eClampFullyOpen = 9
 };
-///////////////////////////////////////////////
+//////////////////////////////////////////////////
 
-///////////////////////////////////////////////
+//////////////////////////////////////////////////
 /// 提升机构运动状态
 enum LifterStatus {
     eInvalid = 0,
@@ -114,4 +114,26 @@ enum LifterStatus {
 };
 //////////////////////////////////////////////////
 
+//////////////////////////////////////////////////
+/// 并行夹持和提升机构运作
+enum eClamLiftActionType{
+    eByteCLampClose = 1,
+    eByteClampHalfOpen=2,
+    eByteClampFullyOpen=4,
+    eByteLifterDown=8,
+    eByteLifterUp=16
+};
+//////////////////////////////////////////////////
+
+//////////////////////////////////////////////////
+/// plc区域+编号载车板 对应 车位表
+/*enum eCarryNo2SpaceNo{
+    eByteCLampClose = 1,
+    eByteClampHalfOpen=2,
+    eByteClampFullyOpen=4,
+    eByteLifterDown=8,
+    eByteLifterUp=16
+};*/
+//////////////////////////////////////////////////
+
 #endif //NAVIGATION_CUSTOM_TYPE_H

MPC/loaded_mpc.cpp

@@ -8,7 +8,7 @@
 #include <cppad/ipopt/solve.hpp>
 
 size_t N = 15;                  //优化考虑后面多少步
-size_t delay = 2;  // 预判发送到执行的延迟，即几个周期的时间
+size_t delay = 1;  // 预判发送到执行的延迟，即几个周期的时间
 size_t down_count = 3;//下发前多少步
 
 size_t nx = 0;
@@ -191,21 +191,23 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     //根据当前点和目标点距离,计算目标速度
     double dis=Pose2d::distance(pose_agv, target);
-    double ref_velocity = 1.0/(1+exp(-4.*(dis-1)));
 
-    //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
 
-    Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
-    //std::cout<<"target:"<<target<<", agv:"<<pose_agv<<", relative:"<<targetPoseInAGV<<std::endl;
-    if (targetPoseInAGV.x() < 0)
-        ref_velocity = -ref_velocity;
+    //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
 
     double dt = mpc_param.dt;
 
     //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
     double max_dlt = max_wmg;//5*M_PI/180.0;
-    double max_acc_line_velocity = mpc_param.acc_velocity;
-    double max_acc_dlt = mpc_param.acc_angular * M_PI / 180.0;
+    double acc_sigmoid= 1.3/(1+exp(-9.*(fabs(line_velocity)-0.5)))+0.3;
+    double max_acc_line_velocity = mpc_param.acc_velocity*acc_sigmoid;
+    double max_acc_dlt = mpc_param.acc_angular;
+
+    double ref_velocity = max_velocity_/(1+exp(-4.*(dis-1)));
+    Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
+    //std::cout<<"target:"<<target<<", agv:"<<pose_agv<<", relative:"<<targetPoseInAGV<<std::endl;
+    if (targetPoseInAGV.x() < 0)
+        ref_velocity = -ref_velocity;
 
     size_t n_vars = N * 5;
 
@@ -368,14 +370,10 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
             if (solve_angular[i] < 0) correct_angular = -correct_angular;
         }
         ///-----
+        /*
         printf(" P[%d],input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
                i, line_velocity, wmg, angular, solve_velocity[i], solve_angular[i], correct_angular,
-               ref_velocity, max_velocity_, time);
-
-        /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
-          solve_velocity=min_velocity_;
-        if(solve_velocity<0 && solve_velocity>-min_velocity_)
-          solve_velocity=-min_velocity_;*/
+               ref_velocity, max_velocity_, time);*/
 
         out.push_back(solve_velocity[i]);
         out.push_back(correct_angular);

MPC/monitor/emqx/message.pb.cc

@@ -46,7 +46,10 @@ PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
 template <typename>
 PROTOBUF_CONSTEXPR AgvStatu::AgvStatu(
     ::_pbi::ConstantInitialized): _impl_{
-    /*decltype(_impl_.v_)*/ 0
+    /*decltype(_impl_.zcb_)*/ {}
+  ,/* _impl_._zcb_cached_byte_size_ = */ { 0 }
+
+  , /*decltype(_impl_.v_)*/ 0
 
   , /*decltype(_impl_.w_)*/ 0
 
@@ -58,6 +61,8 @@ PROTOBUF_CONSTEXPR AgvStatu::AgvStatu(
 
   , /*decltype(_impl_.lifter_other_)*/ 0
 
+  , /*decltype(_impl_.door_)*/ 0
+
   , /*decltype(_impl_._cached_size_)*/{}} {}
 struct AgvStatuDefaultTypeInternal {
   PROTOBUF_CONSTEXPR AgvStatuDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
@@ -96,6 +101,12 @@ PROTOBUF_CONSTEXPR ToAgvCmd::ToAgvCmd(
 
   , /*decltype(_impl_.d1_)*/ 0
 
+  , /*decltype(_impl_.y1_)*/ 0
+
+  , /*decltype(_impl_.y2_)*/ 0
+
+  , /*decltype(_impl_.cl_)*/ 0
+
   , /*decltype(_impl_.end_)*/ 0
 
   , /*decltype(_impl_._cached_size_)*/{}} {}
@@ -351,6 +362,8 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.clamp_other_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.lifter_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.lifter_other_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.zcb_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::AgvStatu, _impl_.door_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -371,6 +384,9 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.l1_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.p1_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.d1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.y1_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.y2_),
+    PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.cl_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::ToAgvCmd, _impl_.end_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _internal_metadata_),
@@ -508,16 +524,16 @@ static const ::_pbi::MigrationSchema
     schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
         { 0, -1, -1, sizeof(::NavMessage::LidarOdomStatu)},
         { 13, -1, -1, sizeof(::NavMessage::AgvStatu)},
-        { 27, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
-        { 48, -1, -1, sizeof(::NavMessage::Pose2d)},
-        { 59, -1, -1, sizeof(::NavMessage::PathNode)},
-        { 73, -1, -1, sizeof(::NavMessage::Trajectory)},
-        { 82, 97, -1, sizeof(::NavMessage::NewAction)},
-        { 104, -1, -1, sizeof(::NavMessage::NavCmd)},
-        { 115, -1, -1, sizeof(::NavMessage::NavResponse)},
-        { 125, -1, -1, sizeof(::NavMessage::ManualCmd)},
-        { 136, 153, -1, sizeof(::NavMessage::NavStatu)},
-        { 162, 174, -1, sizeof(::NavMessage::RobotStatu)},
+        { 29, -1, -1, sizeof(::NavMessage::ToAgvCmd)},
+        { 53, -1, -1, sizeof(::NavMessage::Pose2d)},
+        { 64, -1, -1, sizeof(::NavMessage::PathNode)},
+        { 78, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 87, 102, -1, sizeof(::NavMessage::NewAction)},
+        { 109, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 120, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 130, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 141, 158, -1, sizeof(::NavMessage::NavStatu)},
+        { 167, 179, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -537,49 +553,50 @@ static const ::_pb::Message* const file_default_instances[] = {
 const char descriptor_table_protodef_message_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
     "\n\rmessage.proto\022\nNavMessage\"M\n\016LidarOdom"
     "Statu\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\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\"\247\001\n\010ToAgvCmd\022\n\n\002H1\030\001 \001(\005\022"
-    "\n\n\002M1\030\002 \001(\005\022\n\n\002T1\030\003 \001(\005\022\n\n\002V1\030\004 \001(\002\022\n\n\002W"
-    "1\030\005 \001(\002\022\n\n\002V2\030\006 \001(\002\022\n\n\002W2\030\007 \001(\002\022\n\n\002V3\030\010 "
-    "\001(\002\022\n\n\002W3\030\t \001(\002\022\n\n\002L1\030\n \001(\002\022\n\n\002P1\030\013 \001(\005\022"
-    "\n\n\002D1\030\014 \001(\002\022\013\n\003end\030\r \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\010PathN"
-    "ode\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\001"
-    "w\030\004 \001(\002\022\r\n\005theta\030\005 \001(\002\022\n\n\002id\030\006 \001(\t\"/\n\nTr"
-    "ajectory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pos"
-    "e2d\"\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspace"
-    "Node\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010pass"
-    "Node\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstre"
-    "etNode\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpa"
-    "thNodes\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\tw"
-    "heelbase\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006N"
-    "avCmd\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nne"
-    "wActions\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_t"
-    "ype\030\002 \001(\005\022\020\n\010velocity\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\tmov"
-    "e_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\014pr"
-    "edict_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\n"
-    "RobotStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005thet"
-    "a\030\003 \001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage.A"
-    "gvStatu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMes"
-    "sage.NavCmd\032\027.NavMessage.NavResponse\"\000\0227"
-    "\n\006Cancel\022\022.NavMessage.NavCmd\032\027.NavMessag"
-    "e.NavResponse\"\000\022C\n\017ManualOperation\022\025.Nav"
-    "Message.ManualCmd\032\027.NavMessage.NavRespon"
-    "se\"\000b\006proto3"
+    "(\002\022\t\n\001v\030\004 \001(\002\022\013\n\003vth\030\005 \001(\002\"\205\001\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\013"
+    "clamp_other\030\004 \001(\005\022\016\n\006lifter\030\005 \001(\005\022\024\n\014lif"
+    "ter_other\030\006 \001(\005\022\013\n\003zcb\030\007 \003(\005\022\014\n\004door\030\010 \001"
+    "(\005\"\313\001\n\010ToAgvCmd\022\n\n\002H1\030\001 \001(\005\022\n\n\002M1\030\002 \001(\005\022"
+    "\n\n\002T1\030\003 \001(\005\022\n\n\002V1\030\004 \001(\002\022\n\n\002W1\030\005 \001(\002\022\n\n\002V"
+    "2\030\006 \001(\002\022\n\n\002W2\030\007 \001(\002\022\n\n\002V3\030\010 \001(\002\022\n\n\002W3\030\t "
+    "\001(\002\022\n\n\002L1\030\n \001(\002\022\n\n\002P1\030\013 \001(\005\022\n\n\002D1\030\014 \001(\002\022"
+    "\n\n\002Y1\030\r \001(\002\022\n\n\002Y2\030\016 \001(\002\022\n\n\002CL\030\017 \001(\005\022\013\n\003e"
+    "nd\030\020 \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\005pose"
+    "s\030\001 \003(\0132\022.NavMessage.Pose2d\"\345\001\n\tNewActio"
+    "n\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode\030\002 \001(\0132\024.Nav"
+    "Message.PathNode\022&\n\010passNode\030\003 \001(\0132\024.Nav"
+    "Message.PathNode\022(\n\nstreetNode\030\004 \001(\0132\024.N"
+    "avMessage.PathNode\022\'\n\tpathNodes\030\005 \003(\0132\024."
+    "NavMessage.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\003"
+    "ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tManualCmd\022\013\n\003"
+    "key\030\001 \001(\t\022\026\n\016operation_type\030\002 \001(\005\022\020\n\010vel"
+    "ocity\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\004"
+    "odom\030\004 \001(\0132\032.NavMessage.LidarOdomStatu\022\013"
+    "\n\003key\030\005 \001(\t\022-\n\rselected_traj\030\006 \001(\0132\026.Nav"
+    "Message.Trajectory\022,\n\014predict_traj\030\007 \001(\013"
+    "2\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\010agvSt"
+    "atu\030\004 \001(\0132\024.NavMessage.AgvStatu2\302\001\n\nNavE"
+    "xcutor\0226\n\005Start\022\022.NavMessage.NavCmd\032\027.Na"
+    "vMessage.NavResponse\"\000\0227\n\006Cancel\022\022.NavMe"
+    "ssage.NavCmd\032\027.NavMessage.NavResponse\"\000\022"
+    "C\n\017ManualOperation\022\025.NavMessage.ManualCm"
+    "d\032\027.NavMessage.NavResponse\"\000b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1532,
+    1596,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -967,16 +984,42 @@ AgvStatu::AgvStatu(::PROTOBUF_NAMESPACE_ID::Arena* arena)
   // @@protoc_insertion_point(arena_constructor:NavMessage.AgvStatu)
 }
 AgvStatu::AgvStatu(const AgvStatu& from)
-  : ::PROTOBUF_NAMESPACE_ID::Message(), _impl_(from._impl_) {
-  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(
-      from._internal_metadata_);
+  : ::PROTOBUF_NAMESPACE_ID::Message() {
+  AgvStatu* const _this = this; (void)_this;
+  new (&_impl_) Impl_{
+      decltype(_impl_.zcb_) { from._impl_.zcb_ }
+    ,/* _impl_._zcb_cached_byte_size_ = */ { 0 }
+
+    , decltype(_impl_.v_) {}
+
+    , decltype(_impl_.w_) {}
+
+    , decltype(_impl_.clamp_) {}
+
+    , decltype(_impl_.clamp_other_) {}
+
+    , decltype(_impl_.lifter_) {}
+
+    , decltype(_impl_.lifter_other_) {}
+
+    , decltype(_impl_.door_) {}
+
+    , /*decltype(_impl_._cached_size_)*/{}};
+
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  ::memcpy(&_impl_.v_, &from._impl_.v_,
+    static_cast<::size_t>(reinterpret_cast<char*>(&_impl_.door_) -
+    reinterpret_cast<char*>(&_impl_.v_)) + sizeof(_impl_.door_));
   // @@protoc_insertion_point(copy_constructor:NavMessage.AgvStatu)
 }
 
 inline void AgvStatu::SharedCtor(::_pb::Arena* arena) {
   (void)arena;
   new (&_impl_) Impl_{
-      decltype(_impl_.v_) { 0 }
+      decltype(_impl_.zcb_) { arena }
+    ,/* _impl_._zcb_cached_byte_size_ = */ { 0 }
+
+    , decltype(_impl_.v_) { 0 }
 
     , decltype(_impl_.w_) { 0 }
 
@@ -988,6 +1031,8 @@ inline void AgvStatu::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.lifter_other_) { 0 }
 
+    , decltype(_impl_.door_) { 0 }
+
     , /*decltype(_impl_._cached_size_)*/{}
   };
 }
@@ -1003,6 +1048,7 @@ AgvStatu::~AgvStatu() {
 
 inline void AgvStatu::SharedDtor() {
   ABSL_DCHECK(GetArenaForAllocation() == nullptr);
+  _impl_.zcb_.~RepeatedField();
 }
 
 void AgvStatu::SetCachedSize(int size) const {
@@ -1015,9 +1061,10 @@ void AgvStatu::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
+  _internal_mutable_zcb()->Clear();
   ::memset(&_impl_.v_, 0, static_cast<::size_t>(
-      reinterpret_cast<char*>(&_impl_.lifter_other_) -
-      reinterpret_cast<char*>(&_impl_.v_)) + sizeof(_impl_.lifter_other_));
+      reinterpret_cast<char*>(&_impl_.door_) -
+      reinterpret_cast<char*>(&_impl_.v_)) + sizeof(_impl_.door_));
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
@@ -1081,6 +1128,27 @@ const char* AgvStatu::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
+      // repeated int32 zcb = 7;
+      case 7:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 58)) {
+          ptr = ::PROTOBUF_NAMESPACE_ID::internal::PackedInt32Parser(_internal_mutable_zcb(), ptr, ctx);
+          CHK_(ptr);
+        } else if (static_cast<::uint8_t>(tag) == 56) {
+          _internal_add_zcb(::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr));
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 door = 8;
+      case 8:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 64)) {
+          _impl_.door_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
       default:
         goto handle_unusual;
     }  // switch
@@ -1160,6 +1228,22 @@ failure:
         6, this->_internal_lifter_other(), target);
   }
 
+  // repeated int32 zcb = 7;
+  {
+    int byte_size = _impl_._zcb_cached_byte_size_.Get();
+    if (byte_size > 0) {
+      target = stream->WriteInt32Packed(7, _internal_zcb(),
+                                                 byte_size, target);
+    }
+  }
+
+  // int32 door = 8;
+  if (this->_internal_door() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        8, this->_internal_door(), target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -1176,6 +1260,20 @@ failure:
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
+  // repeated int32 zcb = 7;
+  {
+    std::size_t data_size = ::_pbi::WireFormatLite::Int32Size(
+        this->_internal_zcb())
+    ;
+    _impl_._zcb_cached_byte_size_.Set(::_pbi::ToCachedSize(data_size));
+    std::size_t tag_size = data_size == 0
+        ? 0
+        : 1 + ::_pbi::WireFormatLite::Int32Size(
+                            static_cast<int32_t>(data_size))
+    ;
+    total_size += tag_size + data_size;
+  }
+
   // float v = 1;
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
   float tmp_v = this->_internal_v();
@@ -1218,6 +1316,12 @@ failure:
         this->_internal_lifter_other());
   }
 
+  // int32 door = 8;
+  if (this->_internal_door() != 0) {
+    total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
+        this->_internal_door());
+  }
+
   return MaybeComputeUnknownFieldsSize(total_size, &_impl_._cached_size_);
 }
 
@@ -1236,6 +1340,7 @@ void AgvStatu::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   ::uint32_t cached_has_bits = 0;
   (void) cached_has_bits;
 
+  _this->_impl_.zcb_.MergeFrom(from._impl_.zcb_);
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
   float tmp_v = from._internal_v();
   ::uint32_t raw_v;
@@ -1262,6 +1367,9 @@ void AgvStatu::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   if (from._internal_lifter_other() != 0) {
     _this->_internal_set_lifter_other(from._internal_lifter_other());
   }
+  if (from._internal_door() != 0) {
+    _this->_internal_set_door(from._internal_door());
+  }
   _this->_internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
 }
 
@@ -1279,9 +1387,10 @@ bool AgvStatu::IsInitialized() const {
 void AgvStatu::InternalSwap(AgvStatu* other) {
   using std::swap;
   _internal_metadata_.InternalSwap(&other->_internal_metadata_);
+  _impl_.zcb_.InternalSwap(&other->_impl_.zcb_);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.lifter_other_)
-      + sizeof(AgvStatu::_impl_.lifter_other_)
+      PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.door_)
+      + sizeof(AgvStatu::_impl_.door_)
       - PROTOBUF_FIELD_OFFSET(AgvStatu, _impl_.v_)>(
           reinterpret_cast<char*>(&_impl_.v_),
           reinterpret_cast<char*>(&other->_impl_.v_));
@@ -1337,6 +1446,12 @@ inline void ToAgvCmd::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.d1_) { 0 }
 
+    , decltype(_impl_.y1_) { 0 }
+
+    , decltype(_impl_.y2_) { 0 }
+
+    , decltype(_impl_.cl_) { 0 }
+
     , decltype(_impl_.end_) { 0 }
 
     , /*decltype(_impl_._cached_size_)*/{}
@@ -1486,9 +1601,36 @@ const char* ToAgvCmd::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
-      // int32 end = 13;
+      // float Y1 = 13;
       case 13:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 104)) {
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 109)) {
+          _impl_.y1_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // float Y2 = 14;
+      case 14:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 117)) {
+          _impl_.y2_ = ::PROTOBUF_NAMESPACE_ID::internal::UnalignedLoad<float>(ptr);
+          ptr += sizeof(float);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 CL = 15;
+      case 15:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 120)) {
+          _impl_.cl_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
+          CHK_(ptr);
+        } else {
+          goto handle_unusual;
+        }
+        continue;
+      // int32 end = 16;
+      case 16:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 128)) {
           _impl_.end_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint32(&ptr);
           CHK_(ptr);
         } else {
@@ -1640,11 +1782,40 @@ failure:
         12, this->_internal_d1(), target);
   }
 
-  // int32 end = 13;
+  // float Y1 = 13;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y1 = this->_internal_y1();
+  ::uint32_t raw_y1;
+  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
+  if (raw_y1 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        13, this->_internal_y1(), target);
+  }
+
+  // float Y2 = 14;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y2 = this->_internal_y2();
+  ::uint32_t raw_y2;
+  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
+  if (raw_y2 != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteFloatToArray(
+        14, this->_internal_y2(), target);
+  }
+
+  // int32 CL = 15;
+  if (this->_internal_cl() != 0) {
+    target = stream->EnsureSpace(target);
+    target = ::_pbi::WireFormatLite::WriteInt32ToArray(
+        15, this->_internal_cl(), target);
+  }
+
+  // int32 end = 16;
   if (this->_internal_end() != 0) {
     target = stream->EnsureSpace(target);
     target = ::_pbi::WireFormatLite::WriteInt32ToArray(
-        13, this->_internal_end(), target);
+        16, this->_internal_end(), target);
   }
 
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
@@ -1759,10 +1930,34 @@ failure:
     total_size += 5;
   }
 
-  // int32 end = 13;
-  if (this->_internal_end() != 0) {
+  // float Y1 = 13;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y1 = this->_internal_y1();
+  ::uint32_t raw_y1;
+  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
+  if (raw_y1 != 0) {
+    total_size += 5;
+  }
+
+  // float Y2 = 14;
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y2 = this->_internal_y2();
+  ::uint32_t raw_y2;
+  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
+  if (raw_y2 != 0) {
+    total_size += 5;
+  }
+
+  // int32 CL = 15;
+  if (this->_internal_cl() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
-        this->_internal_end());
+        this->_internal_cl());
+  }
+
+  // int32 end = 16;
+  if (this->_internal_end() != 0) {
+    total_size += 2 + ::_pbi::WireFormatLite::Int32Size(
+                                    this->_internal_end());
   }
 
   return MaybeComputeUnknownFieldsSize(total_size, &_impl_._cached_size_);
@@ -1851,6 +2046,23 @@ void ToAgvCmd::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   if (raw_d1 != 0) {
     _this->_internal_set_d1(from._internal_d1());
   }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y1 = from._internal_y1();
+  ::uint32_t raw_y1;
+  memcpy(&raw_y1, &tmp_y1, sizeof(tmp_y1));
+  if (raw_y1 != 0) {
+    _this->_internal_set_y1(from._internal_y1());
+  }
+  static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
+  float tmp_y2 = from._internal_y2();
+  ::uint32_t raw_y2;
+  memcpy(&raw_y2, &tmp_y2, sizeof(tmp_y2));
+  if (raw_y2 != 0) {
+    _this->_internal_set_y2(from._internal_y2());
+  }
+  if (from._internal_cl() != 0) {
+    _this->_internal_set_cl(from._internal_cl());
+  }
   if (from._internal_end() != 0) {
     _this->_internal_set_end(from._internal_end());
   }

MPC/monitor/emqx/message.pb.h

@@ -453,13 +453,35 @@ class AgvStatu final :
   // accessors -------------------------------------------------------
 
   enum : int {
+    kZcbFieldNumber = 7,
     kVFieldNumber = 1,
     kWFieldNumber = 2,
     kClampFieldNumber = 3,
     kClampOtherFieldNumber = 4,
     kLifterFieldNumber = 5,
     kLifterOtherFieldNumber = 6,
+    kDoorFieldNumber = 8,
   };
+  // repeated int32 zcb = 7;
+  int zcb_size() const;
+  private:
+  int _internal_zcb_size() const;
+
+  public:
+  void clear_zcb() ;
+  ::int32_t zcb(int index) const;
+  void set_zcb(int index, ::int32_t value);
+  void add_zcb(::int32_t value);
+  const ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>& zcb() const;
+  ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>* mutable_zcb();
+
+  private:
+  ::int32_t _internal_zcb(int index) const;
+  void _internal_add_zcb(::int32_t value);
+  const ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>& _internal_zcb() const;
+  ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>* _internal_mutable_zcb();
+
+  public:
   // float v = 1;
   void clear_v() ;
   float v() const;
@@ -519,6 +541,16 @@ class AgvStatu final :
   ::int32_t _internal_lifter_other() const;
   void _internal_set_lifter_other(::int32_t value);
 
+  public:
+  // int32 door = 8;
+  void clear_door() ;
+  ::int32_t door() const;
+  void set_door(::int32_t value);
+
+  private:
+  ::int32_t _internal_door() const;
+  void _internal_set_door(::int32_t value);
+
   public:
   // @@protoc_insertion_point(class_scope:NavMessage.AgvStatu)
  private:
@@ -528,12 +560,15 @@ class AgvStatu final :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   struct Impl_ {
+    ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t> zcb_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _zcb_cached_byte_size_;
     float v_;
     float w_;
     ::int32_t clamp_;
     ::int32_t clamp_other_;
     ::int32_t lifter_;
     ::int32_t lifter_other_;
+    ::int32_t door_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
   union { Impl_ _impl_; };
@@ -680,7 +715,10 @@ class ToAgvCmd final :
     kL1FieldNumber = 10,
     kP1FieldNumber = 11,
     kD1FieldNumber = 12,
-    kEndFieldNumber = 13,
+    kY1FieldNumber = 13,
+    kY2FieldNumber = 14,
+    kCLFieldNumber = 15,
+    kEndFieldNumber = 16,
   };
   // int32 H1 = 1;
   void clear_h1() ;
@@ -802,7 +840,37 @@ class ToAgvCmd final :
   void _internal_set_d1(float value);
 
   public:
-  // int32 end = 13;
+  // float Y1 = 13;
+  void clear_y1() ;
+  float y1() const;
+  void set_y1(float value);
+
+  private:
+  float _internal_y1() const;
+  void _internal_set_y1(float value);
+
+  public:
+  // float Y2 = 14;
+  void clear_y2() ;
+  float y2() const;
+  void set_y2(float value);
+
+  private:
+  float _internal_y2() const;
+  void _internal_set_y2(float value);
+
+  public:
+  // int32 CL = 15;
+  void clear_cl() ;
+  ::int32_t cl() const;
+  void set_cl(::int32_t value);
+
+  private:
+  ::int32_t _internal_cl() const;
+  void _internal_set_cl(::int32_t value);
+
+  public:
+  // int32 end = 16;
   void clear_end() ;
   ::int32_t end() const;
   void set_end(::int32_t value);
@@ -832,6 +900,9 @@ class ToAgvCmd final :
     float l1_;
     ::int32_t p1_;
     float d1_;
+    float y1_;
+    float y2_;
+    ::int32_t cl_;
     ::int32_t end_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
@@ -2943,6 +3014,70 @@ inline void AgvStatu::_internal_set_lifter_other(::int32_t value) {
   _impl_.lifter_other_ = value;
 }
 
+// repeated int32 zcb = 7;
+inline int AgvStatu::_internal_zcb_size() const {
+  return _impl_.zcb_.size();
+}
+inline int AgvStatu::zcb_size() const {
+  return _internal_zcb_size();
+}
+inline void AgvStatu::clear_zcb() {
+  _internal_mutable_zcb()->Clear();
+}
+inline ::int32_t AgvStatu::zcb(int index) const {
+  // @@protoc_insertion_point(field_get:NavMessage.AgvStatu.zcb)
+  return _internal_zcb(index);
+}
+inline void AgvStatu::set_zcb(int index, ::int32_t value) {
+  _internal_mutable_zcb()->Set(index, value);
+  // @@protoc_insertion_point(field_set:NavMessage.AgvStatu.zcb)
+}
+inline void AgvStatu::add_zcb(::int32_t value) {
+  _internal_add_zcb(value);
+  // @@protoc_insertion_point(field_add:NavMessage.AgvStatu.zcb)
+}
+inline const ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>& AgvStatu::zcb() const {
+  // @@protoc_insertion_point(field_list:NavMessage.AgvStatu.zcb)
+  return _internal_zcb();
+}
+inline ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>* AgvStatu::mutable_zcb() {
+  // @@protoc_insertion_point(field_mutable_list:NavMessage.AgvStatu.zcb)
+  return _internal_mutable_zcb();
+}
+
+inline ::int32_t AgvStatu::_internal_zcb(int index) const {
+  return _internal_zcb().Get(index);
+}
+inline void AgvStatu::_internal_add_zcb(::int32_t value) {
+  _internal_mutable_zcb()->Add(value);
+}
+inline const ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>& AgvStatu::_internal_zcb() const {
+  return _impl_.zcb_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::RepeatedField<::int32_t>* AgvStatu::_internal_mutable_zcb() {
+  return &_impl_.zcb_;
+}
+
+// int32 door = 8;
+inline void AgvStatu::clear_door() {
+  _impl_.door_ = 0;
+}
+inline ::int32_t AgvStatu::door() const {
+  // @@protoc_insertion_point(field_get:NavMessage.AgvStatu.door)
+  return _internal_door();
+}
+inline void AgvStatu::set_door(::int32_t value) {
+  _internal_set_door(value);
+  // @@protoc_insertion_point(field_set:NavMessage.AgvStatu.door)
+}
+inline ::int32_t AgvStatu::_internal_door() const {
+  return _impl_.door_;
+}
+inline void AgvStatu::_internal_set_door(::int32_t value) {
+  ;
+  _impl_.door_ = value;
+}
+
 // -------------------------------------------------------------------
 
 // ToAgvCmd
@@ -3187,7 +3322,67 @@ inline void ToAgvCmd::_internal_set_d1(float value) {
   _impl_.d1_ = value;
 }
 
-// int32 end = 13;
+// float Y1 = 13;
+inline void ToAgvCmd::clear_y1() {
+  _impl_.y1_ = 0;
+}
+inline float ToAgvCmd::y1() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y1)
+  return _internal_y1();
+}
+inline void ToAgvCmd::set_y1(float value) {
+  _internal_set_y1(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y1)
+}
+inline float ToAgvCmd::_internal_y1() const {
+  return _impl_.y1_;
+}
+inline void ToAgvCmd::_internal_set_y1(float value) {
+  ;
+  _impl_.y1_ = value;
+}
+
+// float Y2 = 14;
+inline void ToAgvCmd::clear_y2() {
+  _impl_.y2_ = 0;
+}
+inline float ToAgvCmd::y2() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.Y2)
+  return _internal_y2();
+}
+inline void ToAgvCmd::set_y2(float value) {
+  _internal_set_y2(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.Y2)
+}
+inline float ToAgvCmd::_internal_y2() const {
+  return _impl_.y2_;
+}
+inline void ToAgvCmd::_internal_set_y2(float value) {
+  ;
+  _impl_.y2_ = value;
+}
+
+// int32 CL = 15;
+inline void ToAgvCmd::clear_cl() {
+  _impl_.cl_ = 0;
+}
+inline ::int32_t ToAgvCmd::cl() const {
+  // @@protoc_insertion_point(field_get:NavMessage.ToAgvCmd.CL)
+  return _internal_cl();
+}
+inline void ToAgvCmd::set_cl(::int32_t value) {
+  _internal_set_cl(value);
+  // @@protoc_insertion_point(field_set:NavMessage.ToAgvCmd.CL)
+}
+inline ::int32_t ToAgvCmd::_internal_cl() const {
+  return _impl_.cl_;
+}
+inline void ToAgvCmd::_internal_set_cl(::int32_t value) {
+  ;
+  _impl_.cl_ = value;
+}
+
+// int32 end = 16;
 inline void ToAgvCmd::clear_end() {
   _impl_.end_ = 0;
 }

MPC/monitor/monitor_emqx.cpp

@@ -99,6 +99,7 @@ void Monitor_emqx::lifter_down(int mode)
     Publish(clampLifterCmd_topic_,msg);
 }
 
+/*
 void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L)
 {
   if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
@@ -124,6 +125,7 @@ void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L
   Publish(speedcmd_topic_,msg);
 
 }
+ */
 void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a)
 {
   double w=fabs(a)>0.0001?a:0.0;
@@ -164,13 +166,50 @@ void Monitor_emqx::stop()
   Publish(speedcmd_topic_,msg);
 }
 
+void Monitor_emqx::set_ToAgvCmd(int clampLifterAction,int mode,ActionType type,double v[],double w[],
+                  double L,int P,double D,double Y1,double Y2){
+    printf(" Cmd :  clampLifter %d,mode:%d  type:%d  v:%f  w:%f  wheelBase:%f  spaceid:%d  distance:%f  Y1:%f  Y2:%f\n",
+           clampLifterAction,mode,type,v[0],w[0],L,P,D,Y1,Y2);
+    int count = 3;
+    double w_correct[count];
+    double velocity[count];
+    for (int i = 0; i < count; ++i) {
+        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
+        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
+    }
+
+    double yaw1 =fabs(Y1)>0.0001?Y1:0.0;
+    double yaw2 =fabs(Y2)>0.0001?Y2:0.0;
+
+    MqttMsg msg;
+    NavMessage::ToAgvCmd agvCmd;
+    heat_=(heat_+1)%255;
+    agvCmd.set_h1(heat_);
+    agvCmd.set_m1(mode);
+    agvCmd.set_t1(type);
+    agvCmd.set_v1(velocity[0]);
+    agvCmd.set_w1(w_correct[0]);
+    agvCmd.set_v2(velocity[1]);
+    agvCmd.set_w2(w_correct[1]);
+    agvCmd.set_v3(velocity[2]);
+    agvCmd.set_w3(w_correct[2]);
+    agvCmd.set_y1(yaw1);
+    agvCmd.set_y2(yaw2);
+    agvCmd.set_l1(L);
+    agvCmd.set_p1(P);
+    agvCmd.set_d1(D);
+    agvCmd.set_cl(clampLifterAction);
+
+    agvCmd.set_end(1);
+    msg.fromProtoMessage(agvCmd);
+    Publish(speedcmd_topic_,msg);
+
+}
+
+/*
 void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[],
                                 double L, int P, double D,double Y1,double Y2) {
-//    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
-//    {
-//        printf(" Main agv mpc must set wheelBase\n ");
-//        return;
-//    }
+
     int count = 3;
     double w_correct[count];
     double velocity[count];
@@ -204,6 +243,7 @@ void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[
     msg.fromProtoMessage(agvCmd);
     Publish(speedcmd_topic_,msg);
 }
+ */
 //
 //void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double diff_yaw[], double L, int P, double D) {
 ////    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)

MPC/monitor/monitor_emqx.h

@@ -17,9 +17,14 @@ class Monitor_emqx : public Terminator_emqx
     void set_speedcmd_topic(std::string speedcmd);
     void set_clampLifterCmd_topic(std::string topic);
     void set_speed(int mode,ActionType type,double v,double a);
+
+    /*
     void set_speed(int mode,ActionType type,double v,double a,double L);
     void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],
                       double L=0,int P=0,double D=0,double Y1=0.,double Y2=0.);
+    */
+    void set_ToAgvCmd(int clampLifterAction,int mode,ActionType type,double v[],double w[],
+                      double L=0,int P=0,double D=0,double Y1=0.,double Y2=0.);
 //    void set_ToAgvCmd(int mode,ActionType type,double v[],double w[],double yaw_diff[], double L=0,int P=0,double D=0);
     void clamp_close(int mode);
     void clamp_half_open(int mode);

MPC/navigation.cpp

@@ -136,13 +136,32 @@ void Navigation::RobotPoseCallback(const MqttMsg &msg, void *context) {
 
 void Navigation::HandleAgvStatu(const MqttMsg &msg) {
     NavMessage::AgvStatu speed;
-    if (msg.toProtoMessage(speed)) {
-        ResetStatu(speed.v(), speed.w());
-        ResetClamp((ClampStatu) speed.clamp());
-        ResetLifter((LifterStatus) speed.lifter());
+    if (IsMasterAGV()) {
+        if (msg.toProtoMessage(speed)) {
+            ResetStatu(speed.v(), speed.w());
+            ResetClamp((ClampStatu) speed.clamp());
+//            ResetOtherClamp((ClampStatu) speed.clamp_other());
+            ResetLifter((LifterStatus) speed.lifter());
+//            ResetOtherLifter((LifterStatus) speed.lifter_other());
+            //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
+        }
+    } else {
+        if (msg.toProtoMessage(speed)) {
+            ResetStatu(speed.v(), speed.w());
+            ResetClamp((ClampStatu) speed.clamp());
+            ResetLifter((LifterStatus) speed.lifter());
+
+        }
     }
+    ResetInsideDoor(speed.door());
+    std::vector<int32_t> vecStatus;
+    for(int i=0;i<speed.zcb_size();++i){
+        vecStatus.push_back(speed.zcb(i));
+    }
+    ResetCarrier(vecStatus);
 }
 
+
 void Navigation::RobotSpeedCallback(const MqttMsg &msg, void *context) {
     Navigation *navigator = (Navigation *) context;
     navigator->HandleAgvStatu(msg);
@@ -255,6 +274,7 @@ void Navigation::ResetLifter(LifterStatus status) {
     timed_lifter_.reset(status, 1);
 }
 
+
 void Navigation::ResetPose(const Pose2d &pose) {
     timedPose_.reset(pose, 1.0);
     if (pose.theta() > 0) {
@@ -331,6 +351,7 @@ bool Navigation::SlowlyStop() {
 
 
 Navigation::Navigation() {
+    is_master_AGV=false;
     wheelBase_ = 0;
     isInSpace_ = false; //是否在车位或者正在进入车位
     RWheel_position_ = eUnknow;
@@ -362,6 +383,7 @@ void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) {
 }
 
 //未使用
+/*
 bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect) {
 
     while (cancel_ == false) {
@@ -425,10 +447,10 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
     }
     return false;
 }
+*/
 
-
-bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate) {
-    std::cout<<"Adjust to : "<<target<<std::endl;
+bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate,int directions) {
+    std::cout << "Adjust to : " << target << std::endl;
     if (inited_ == false) {
         printf(" MPC_rotate has not inited\n");
         return false;
@@ -442,19 +464,19 @@ bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate)
     NavMessage::PathNode yawTarget;
     yawTarget.set_x(current.x() + 100.);
     yawTarget.set_y(current.y());
-    int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
-    std::cout<<"Adjust RotateRefer : "<<Pose2d(yawTarget.x(),yawTarget.y(),0)<<std::endl;
+
+    std::cout << "Adjust RotateRefer : " << Pose2d(yawTarget.x(), yawTarget.y(), 0) << std::endl;
     RotateReferToTarget(yawTarget, limit_rotate, directions);
 
     current = timedPose_.Get();
-    Pose2d diff=target-current;
+    Pose2d diff = target - current;
     Pose2d newTarget;
     NavMessage::PathNode node;
-    if(fabs(diff.x())>fabs(diff.y())){
-        newTarget=Pose2d(current.x(),target.y(),current.theta());
-        node.set_theta(M_PI/2.0);
-    }else{
-        newTarget=Pose2d(target.x(),current.y(),current.theta());
+    if (fabs(diff.x()) > fabs(diff.y())) {
+        newTarget = Pose2d(current.x(), target.y(), current.theta());
+        node.set_theta(M_PI / 2.0);
+    } else {
+        newTarget = Pose2d(target.x(), current.y(), current.theta());
         node.set_theta(0);
     }
     node.set_x(newTarget.x());
@@ -462,11 +484,10 @@ bool Navigation::AdjustReferToTarget(const Pose2d& target, stLimit limit_rotate)
     node.set_l(0.03);
     node.set_w(0.2);
 
-    stLimit limitV={0.03,0.5};
-    stLimit limitW={0.03,0.1};
-    std::cout<<" Adjust MoveTarget:"<<Pose2d(node.x(),node.y(),node.theta())<<std::endl;
-    return MoveToTarget(node,limitV,limitW,true,true);
-
+    stLimit limitV = {0.03, 0.5};
+    stLimit limitW = {0.03, 0.1};
+    std::cout << " Adjust MoveTarget:" << Pose2d(node.x(), node.y(), node.theta()) << std::endl;
+    return MoveToTarget(node, limitV, limitW, true, true);
 }
 
 Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node, stLimit limit_rotate, int directions) {
@@ -549,22 +570,15 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
             const int down_count = 3;
             double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
-//            if(out[0]>=0){
-//                w[0]=1.*M_PI/180.0;
-//                w[1]=1.*M_PI/180.0;
-//                w[2]=1.*M_PI/180.0;
-//            }else{
-//                w[0]=-1.*M_PI/180.0;
-//                w[1]=-1.*M_PI/180.0;
-//                w[2]=-1.*M_PI/180.0;
-//            }
-            SendMoveCmd(move_mode_, eRotation, v, w);
+
+            SendMoveCmd(0, move_mode_, eRotation, v, w);
             actionType_ = eRotation;
             double input_w = timedA_.Get();
+            /*
             for (int i = 0; i < down_count; ++i) {
                 printf("  MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
                        i, input_w, w[i], w[i] / M_PI * 180, target_yaw_diff, directions);
-            }
+            }*/
         }
         continue;
     }
@@ -572,8 +586,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
 }
 
 bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
-                              bool anyDirect, bool enable_rotate) {
-//    LogWriter("MoveToTarget beg");
+                              bool anyDirect, bool enable_rotate, int clampLifterAction) {
     if (IsArrived(node)) {
         return true;
     }
@@ -600,9 +613,12 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         Pose2d target(node.x(), node.y(), 0);
         Pose2d targetInCurrent = Pose2d::relativePose(target, current);
 
-        if (enableTotarget == false ) {
-            bool adjustRet=AdjustReferToTarget(target,limit_w);
-            if(adjustRet==false)
+        if (enableTotarget == false) {
+            int directions = Direction::eForward;
+            if(anyDirect)
+                 directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+            bool adjustRet = AdjustReferToTarget(target, limit_w,directions);
+            if (adjustRet == false)
                 return false;
             /*if (already_in_mpc && enable_rotate == false) {
                 printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
@@ -623,7 +639,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         }
 
         already_in_mpc = true;
-        MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
+        MpcResult ret = MpcToTarget(node, limit_v, anyDirect, clampLifterAction);
         if (ret == eMpcSuccess) {
             printf(" MPC to target:%f %f l:%f,w:%f theta:%f  completed\n",
                    node.x(), node.y(), node.l(), node.w(), node.theta());
@@ -655,9 +671,11 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
         stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
 
         /////////////////////////////////////////
-
-        if(move_mode_==eSingle) {
-            clamp_half_open();
+        int clampLifterAction = 0;
+        if (move_mode_ == eSingle) {
+            clampLifterAction |= eByteClampHalfOpen;
+        }else{
+            clampLifterAction |= eByteLifterDown;
         }
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
@@ -685,7 +703,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
                     return false;
                 }
             }*/
-            if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true) == false)
+            if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false)
                 return false;
             else
                 isInSpace_ = false;
@@ -701,7 +719,51 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
     return false;
 }
 
-Navigation::MpcResult Navigation::RotateAfterOutSpace() {
+Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
+    if (move_mode_ != eDouble)
+        return eMpcSuccess;
+    Pose2d init = timedPose_.Get();
+    double diff1 = fabs(M_PI / 2.0 - init.theta());
+    double diff2 = fabs(-M_PI / 2.0 - init.theta());
+    double target = diff1 < diff2 ? M_PI / 2.0 : -M_PI / 2.0;
+    stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
+    while (cancel_ == false) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        Pose2d current = timedPose_.Get();
+        double yawDiff = (target - current.theta());
+
+        //一次变速
+        std::vector<double> out;
+        bool ret;
+        TimerRecord::Execute([&, this]() {
+            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
+        }, "Rotation_mpc_once");
+        if (ret == false) {
+            Stop();
+            return eMpcFailed;
+        }
+
+        //下发速度
+        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+            printf(" DoubleOutSpaceCorrectTheta refer target completed\\n,cuv:%f\n", yawDiff);
+            Stop();
+            return eMpcSuccess;
+        } else {
+            const int down_count = 3;
+            double v[down_count] = {0, 0, 0};
+            double w[down_count] = {out[0], out[1], out[2]};
+            SendMoveCmd(0, move_mode_, eRotation, v, w);
+            actionType_ = eRotation;
+            printf(" DoubleOutSpaceCorrectTheta | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
+        }
+        continue;
+    }
+    return eMpcFailed;
+}
+
+
+Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
     if (move_mode_ != eDouble)
         return eMpcSuccess;
     Pose2d init = timedPose_.Get();
@@ -726,7 +788,7 @@ Navigation::MpcResult Navigation::RotateAfterOutSpace() {
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
@@ -734,10 +796,10 @@ Navigation::MpcResult Navigation::RotateAfterOutSpace() {
             const int down_count = 3;
             double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
-            SendMoveCmd(move_mode_, eRotation, v, w);
+            SendMoveCmd(0, move_mode_, eRotation, v, w);
             actionType_ = eRotation;
-            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
+//            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+//                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
         }
         continue;
     }
@@ -745,8 +807,8 @@ Navigation::MpcResult Navigation::RotateAfterOutSpace() {
 }
 
 Navigation::MpcResult
-Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
-                                   double wheelbase, NavMessage::PathNode &target) {
+Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
+                                   NavMessage::PathNode &target, int clampLifterAction) {
 //    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
     if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
@@ -828,10 +890,10 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
             const int down_count = 3;
             double v[down_count] = {0, 0, 0};
             double w[down_count] = {out[0], out[1], out[2]};
-            SendMoveCmd(move_mode_, eRotation, v, w);
+            SendMoveCmd(clampLifterAction, move_mode_, eRotation, v, w);
             actionType_ = eRotation;
-            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
+//            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+//                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
         }
         continue;
 
@@ -840,9 +902,36 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
 
 }
 
-bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
-//    LogWriter("execute_InOut_space beg");
+bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
+    if (spaceNode.id().find("S") < 0)
+        return false;
+    std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1);
+    int space_id = stoi(id);
+    if (space_id > 99 && space_id < 1000) {
+        return true;
+    }
+    return false;
+}
 
+int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
+    if (spaceNode.id().find("S") >= 0) {
+        std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1);
+        int space_id = stoi(id);
+        return space_id;
+    } else {
+        return -1;
+    }
+}
+
+bool Navigation::TargetIsEntrance(NavMessage::PathNode node) {
+    return node.id().find("S1101") != -1;
+}
+
+bool Navigation::TargetIsExport(NavMessage::PathNode node) {
+    return node.id().find("S1101") != -1;
+}
+
+bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     if (action.type() != 1 && action.type() != 2) {
         printf(" Inout_space failed : msg action type must ==1\n ");
         return false;
@@ -861,38 +950,79 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
     //入库,起点为streetNode
     if (action.type() == 1) {
-//        LogWriter("In space beg-");
-
         Pose2d vec(action.spacenode().x() - action.streetnode().x(),
                    action.spacenode().y() - action.streetnode().y(),
                    0);
         action.mutable_streetnode()->set_l(0.02);
         action.mutable_streetnode()->set_w(0.2);
-        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y())+M_PI/2.0);
+        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
         /*
          * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
-
-        if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) {
+        int clampLifterAction = 0;
+        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
+            clampLifterAction |= eByteLifterUp;
+        } else {    //否则下降
+            clampLifterAction |= eByteLifterDown;
+        }
+        if (move_mode_ == eSingle)
+            clampLifterAction |= eByteClampHalfOpen;
+        if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
         }
         //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
         printf("inout ----------------------------------------\n");
+
+        //取车流程，送车到出口前旋转180
+        if (move_mode_ == eDouble && TargetIsExport(action.spacenode())) {
+            Pose2d current_pose = timedPose_.Get();
+            double current_theta = current_pose.theta();
+            if (RotateBeforeIntoExport() != eMpcSuccess) {
+                printf("Rotate before in entrance failed\n");
+                return false;
+            }
+        }
+
         //计算车位朝向
         action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
-        //计算当前车是进入车位1点还是2点,    提升下降动作与旋转调整并行
-        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
-            lifter_rise();
-        } else {    //否则下降
-            lifter_down();
-        }
+        //计算当前车是进入车位1点还是2点,
         NavMessage::PathNode new_target;
-        bool retRotate=RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcSuccess;
+        bool retRotate = RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(),
+                                                new_target, clampLifterAction) == eMpcSuccess;
 
-        if(retRotate==false){
+        if (retRotate == false) {
             return false;
         }
+        //去出入口，等内门打开
+        if (TargetIsExport(action.spacenode()) || TargetIsEntrance(action.spacenode())) {
+            printf(" Wait Inside door opened.......\n");
+            if (WaitInsideDoorOpen(action.spacenode().id()) == false) {
+                return false;
+            }
+            printf(" Wait Inside door opened completed!!!\n");
+        } else {
+            //去车位点
+            if (IsUperSpace(action.spacenode())) {
+                printf(" Wait Carrier down.......\n");
+                if (WaitCarrierCompleted(0/*待修改*/, eCarrierDown) == false) {
+                    return false;
+                }
+                printf(" Wait Carrier down completed!!!\n");
+            } else {
+                printf(" Wait Carrier up.......\n");
+                if (WaitCarrierCompleted(0/*待修改*/, eCarrierUp) == false) {
+                    return false;
+                }
+                printf(" Wait Carrier up completed!!!\n");
+            }
+        }
+
+        if (IsUperSpace(action.spacenode())) {
+            if(lifter_rise()== false){
+                return false;
+            }
+        }
 
         //入库
         Pose2d rotated = timedPose_.Get();
@@ -904,20 +1034,20 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (rotated.theta() < 0.)
             isFront = !isFront;
         // 后进车，且单车，且先进车已过后进车的目标点
-        Pose2d target(new_target.x(),new_target.y(),0);
+        Pose2d target(new_target.x(), new_target.y(), 0);
         if (isFront == false && move_mode_ == eSingle) {
-            while(true) {
+            while (true) {
                 if (cancel_) {
                     return false;
                 }
-                Pose2d current=timedPose_.Get();
+                Pose2d current = timedPose_.Get();
                 Pose2d brother_pose = timedBrotherPose_.Get();
-                Pose2d vecCurent2T=current-target;
-                Pose2d vecBrother2T=brother_pose-target;
-                double dot=vecCurent2T.x()*vecBrother2T.x()+vecCurent2T.y()*vecBrother2T.y();
-                if (timedBrotherNavStatu_.Get().in_space() == true && dot<0.) {
-                        printf("先进车已经过后进车目标点，后进车停止等待 dot=%f\n",dot);
-                        break;
+                Pose2d vecCurent2T = current - target;
+                Pose2d vecBrother2T = brother_pose - target;
+                double dot = vecCurent2T.x() * vecBrother2T.x() + vecCurent2T.y() * vecBrother2T.y();
+                if (timedBrotherNavStatu_.Get().in_space() == true && dot < 0.) {
+                    printf("先进车已经过后进车目标点，后进车停止等待 dot=%f\n", dot);
+                    break;
                 }
                 printf(" 后车 等待 先车先入库。。。\n");
                 usleep(1000 * 500);
@@ -926,14 +1056,26 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
         isInSpace_ = true;
         printf("Enter space beg...\n");
-        if(move_mode_==eSingle)
-            clamp_half_open();  //////// 后续再改为全打开位
-        bool retMove=MoveToTarget(new_target, limit_v, limit_w, true, false);
+        clampLifterAction = 0;
+        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
+            clampLifterAction |= eByteLifterUp;
+        } else {    //否则下降
+            clampLifterAction |= eByteLifterDown;
+        }
+        printf(" clampLifter_statu1 :%d\n", clampLifterAction);
+        if (move_mode_ == eSingle)
+            clampLifterAction |= eByteClampFullyOpen;  //////// 后续再改为全打开位
+        else
+            clampLifterAction |= eByteCLampClose;
+
+        printf(" clampLifter_statu2 :%d\n", clampLifterAction);
+        bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction);
 
-        if(retMove==false){
+        if (retMove == false) {
             printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type());
             return false;
         }
+
     } else if (action.type() == 2) {
 //        LogWriter("Out space beg-");
         // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
@@ -959,16 +1101,12 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
         isInSpace_ = false;
-        lifter_down();
-        //从入口出库旋转180
-        if (move_mode_ == eDouble && action.spacenode().id().find("S1101") != -1) {
-            Pose2d current_pose = timedPose_.Get();
-            double current_theta = current_pose.theta();
-            if (RotateAfterOutSpace() != eMpcSuccess) {
-                printf("Rotate after out entrance failed\n");
-                return false;
-            }
-        }
+        if (!lifter_down())
+            return false;
+        //摆正
+        if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
+            return false;
+
     }
     return true;
 }
@@ -1166,10 +1304,10 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type,
+void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type,
                              double v[], double angular[]) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular);
+        monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular);
         if (type == eRotation)
             RWheel_position_ = eR;
         if (type == eVertical)
@@ -1179,7 +1317,7 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
     }
 }
 
-void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+/*void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
                              int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
         monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance,Y1,Y2);
@@ -1190,6 +1328,19 @@ void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angul
         if (type == eHorizontal)
             RWheel_position_ = eY;
     }
+}*/
+
+void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance, double Y1, double Y2) {
+    if (monitor_) {
+        monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular, 0, space_id, distance, Y1, Y2);
+        if (type == eRotation)
+            RWheel_position_ = eR;
+        if (type == eVertical)
+            RWheel_position_ = eX;
+        if (type == eHorizontal)
+            RWheel_position_ = eY;
+    }
 }
 
 //void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) {
@@ -1246,28 +1397,17 @@ bool Navigation::clamp_half_open() {
     return false;
 }
 
-bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
-    if(spaceNode.id().find("S")<0)
-        return false;
-    std::string id = spaceNode.id().substr(1, spaceNode.id().length() - 1);
-    int space_id = stoi(id);
-    if (space_id > 99 && space_id < 1000) {
-        return true;
-    }
-    return false;
-}
-
-bool Navigation::singleClamp_fullyOpen_until_inspaceBegin(){
-    if(move_mode_==eDouble){
+bool Navigation::singleClamp_fullyOpen_until_inspaceBegin() {
+    if (move_mode_ == eDouble) {
         return true;
     }
-    while(cancel_==false){
-        if(timedV_.timeout()){
+    while (cancel_ == false) {
+        if (timedV_.timeout()) {
             return false;
         }
-        if(fabs(timedV_.Get())<0.02){
+        if (fabs(timedV_.Get()) < 0.02) {
 
-            usleep(1000*300);
+            usleep(1000 * 300);
             continue;
         }
         break;
@@ -1369,9 +1509,80 @@ bool Navigation::lifter_down() {
     return false;
 }
 
-Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
-//    LogWriter("MpcToTarget beg");
+bool Navigation::WaitInsideDoorOpen(std::string doorID) {
+    return true;
+    while (cancel_ == false) {
+        if (timed_insideDoor_.timeout()) {
+            return false;
+        }
 
+        InsideDoorStatu current = timed_insideDoor_.Get();
+        /*
+         *
+         */
+
+        usleep(1000 * 100);
+    }
+    if (cancel_ == true) {
+        return false;
+    }
+}
+
+bool Navigation::WaitCarrierCompleted(int carrierID, CarrierStatu statu) {
+    return true;
+    while (cancel_ == false) {
+        if (timed_Carrier_.timeout()) {
+            return false;
+        }
+//        CarrierStatu current = timed_Carrier_.Get();
+        /*   */
+
+        usleep(1000 * 100);
+    }
+    if (cancel_ == true) {
+        return false;
+    }
+}
+
+bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
+    int currentStatu = 0;
+    while (cancel_ == false) {
+        if (timed_clamp_.timeout() || timed_lifter_.timeout()) {
+            return false;
+        }
+        if (timed_clamp_.Get() == eClosed) {
+            currentStatu |= eByteCLampClose;
+        }
+        if (timed_clamp_.Get() == eHalfOpened) {
+            currentStatu |= eByteClampHalfOpen;
+        }
+        if (timed_clamp_.Get() == eFullyOpened) {
+            currentStatu |= eByteClampFullyOpen;
+        }
+        if (timed_lifter_.Get() == eRose) {
+            currentStatu |= eByteLifterUp;
+        }
+        if (timed_lifter_.Get() == eDowned) {
+            currentStatu |= eByteLifterDown;
+        }
+        printf(" current statu:%d   action status:%d  last:%d\n",
+               currentStatu,clampLifterAction,currentStatu&clampLifterAction);
+        if((currentStatu&clampLifterAction) == clampLifterAction)
+            return true;
+
+        double down_v[3] = {0, 0, 0};//下发线速度0
+        double down_w[3] = {0, 0, 0};//下发角速度0
+        SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
+        usleep(1000 * 100);
+    }
+    if (cancel_ == true) {
+        return false;
+    }
+    return false;
+}
+
+Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v,
+                                              bool autoDirect, int clampLifterAction) {
     if (IsArrived(node)) {
         printf(" current already in target completed !!!\n");
         return eMpcSuccess;
@@ -1400,8 +1611,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         } else {
             std::cout << " MPC X axis  target_relative:" << target_relative << std::endl;
         }
-        if(!PossibleToTarget(node, directY)){
-            directY=!directY;
+        if (!PossibleToTarget(node, directY)) {
+            directY = !directY;
         }
     }
 
@@ -1438,7 +1649,9 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             if (Stop()) {
                 printf(" exec along completed !!!\n");
                 ofs.close();//关闭文件
-                return eMpcSuccess;
+                bool waited = WaitClampLifterStatu(clampLifterAction);
+                if (waited) return eMpcSuccess;
+                else return eMpcFailed;
             }
         }
 
@@ -1450,7 +1663,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             ret = mpc_once(traj, limit_v, out, directY);
         }, "mpc_once");
         if (ret == false) {
-            Stop();
+            SlowlyStop();
             ofs.close();//关闭文件
             return eMpcFailed;
         }
@@ -1507,16 +1720,19 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             std::string id = node.id().substr(1, node.id().length() - 1);
             space_id = stoi(id);
         }
-        double Y1=0.0,Y2=0.0;
-        if(move_mode_==eDouble){
-            double dy=-timeYawDiff1_.Get();
-            Y1=0.2*(1.0/(1+exp(-16.*dy))-0.5)*1.0;
-            Y2=timeYawDiff2_.Get();
+        double Y1 = 0.0, Y2 = 0.0;
+        if (move_mode_ == eDouble) {
+            double dy = -timeYawDiff1_.Get();
+            Y1 = 0.2 * (1.0 / (1 + exp(-16. * dy)) - 0.5) * 1.0;
+            Y2 = timeYawDiff2_.Get();
+            if (directY == false) {
+                Y1 = 0;
+            }
         }
         if (directY == false)
-            SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance,Y1,Y2);
+            SendMoveCmd(clampLifterAction, move_mode_, eVertical, down_v, down_w, space_id, distance, Y1, Y2);
         else
-            SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance,Y1,Y2);
+            SendMoveCmd(clampLifterAction, move_mode_, eHorizontal, down_v, down_w, space_id, distance, Y1, Y2);
         actionType_ = directY ? eHorizontal : eVertical;
 
         //日志打印
@@ -1815,3 +2031,61 @@ void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::N
 //    running_ = false;
 }
 
+bool Navigation::IsMasterAGV() {
+    return is_master_AGV;
+}
+
+void Navigation::ResetInsideDoor(int32_t status) {
+    if ((status & eInsideDoorOpened) == eInsideDoorOpened){
+        timed_insideDoor_.reset(eInsideDoorOpened);
+    } else if ((status & eInsideDoorClosed) == eInsideDoorClosed){
+        timed_insideDoor_.reset(eInsideDoorClosed);
+    } else{
+        timed_insideDoor_.reset(eInsideDoorInvalid);
+    }
+}
+
+void Navigation::anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes){
+    int bytesize=sizeof(int32_t)*8;
+    regionID = (bytes>>((bytesize-1)*8));
+    for (int i = 0; i < (bytesize-8)/2; ++i) {
+        int statu =(bytes>>(i*2) & 0x03);
+
+        if ((statu & eCarrierUp) == eCarrierUp) {
+            status[i]=eCarrierUp;
+        }
+        else if ((statu & eCarrierDown) == eCarrierDown){
+            status[i]=eCarrierDown;
+        }
+        else {
+            status[i]=eCarrierInvalid;
+        }
+    }
+}
+
+void Navigation::ResetCarrier(std::vector<int32_t> status) {
+    std::map<int,CarrierStatuRegionMap> regionStatus;
+    for(int32_t statu : status){
+        int regionID;
+        CarrierStatuRegionMap region;
+        anasisCarierStatu(regionID,region,statu);
+        regionStatus[regionID]=region;
+    }
+    timed_Carrier_.reset(regionStatus);
+
+    printf("---------------------------------------\n");
+    for(std::map<int,CarrierStatuRegionMap>::iterator it=regionStatus.begin();
+        it!=regionStatus.end();it++){
+        int regionID=it->first;
+        printf(">>>>>>> Region ID :%d\n",regionID);
+        for(std::map<int,CarrierStatu>::iterator it1=it->second.begin();
+            it1!=it->second.end();++it1){
+            int id=it1->first;
+            CarrierStatu sta=it1->second;
+            printf("\t \tcarrier id :%d  statu:%d\n",id,sta);
+        }
+    }
+
+}
+
+

MPC/navigation.h

@@ -37,6 +37,20 @@ public:
         eFullyOpened = 3 //全开位
     };
 
+    enum CarrierStatu{
+        eCarrierInvalid=0,
+        eCarrierUp=1,
+        eCarrierDown=2
+    };
+
+    typedef std::map<int,CarrierStatu> CarrierStatuRegionMap;
+
+    enum InsideDoorStatu{
+        eInsideDoorInvalid=0,
+        eInsideDoorOpened=1,
+        eInsideDoorClosed=2
+    };
+
     enum MpcResult {
         eMpcSuccess = 0,
         eMpcFailed,
@@ -52,6 +66,8 @@ public:
 
     bool Init(const NavParameter::Navigation_parameter &parameter);
 
+    virtual bool IsMasterAGV();
+
     virtual void ResetPose(const Pose2d &pose);
 
     void ResetStatu(double v, double a);
@@ -60,6 +76,10 @@ public:
 
     virtual void ResetLifter(LifterStatus status);
 
+    virtual void ResetInsideDoor(int32_t status);
+
+    virtual void ResetCarrier(std::vector<int32_t> status);
+
     TimedLockData<Pose2d> &RealTimePose() {
         return timedPose_;
     }
@@ -86,34 +106,26 @@ public:
     virtual void SwitchMode(int mode, float wheelBase);
 
     bool PoseTimeout();
+    bool WaitClampLifterStatu(int clampLifterAction);
+    bool WaitInsideDoorOpen(std::string doorID);
+    bool WaitCarrierCompleted(int carrierID,CarrierStatu statu);
+
+    bool TargetIsEntrance(NavMessage::PathNode node);
+    bool TargetIsExport(NavMessage::PathNode node);
 
-    // 测试日志打印
-    bool LogWriter(std::string lines){
-        {
-            std::ofstream ofs;  //创建流对象
-            std::string log_file_dir = "../data/12_12/";
-            std::string log_file_name = "12_12.txt";
-            ofs.open(log_file_dir+log_file_name,std::ios::app);  //打开的地址和方式
-
-            time_t t = time(0);
-            char tmp[32]={NULL};
-            strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S",localtime(&t));
-            std::string time = tmp;
-            std::string log_inf_line = lines + ". " + time;
-            ofs << log_inf_line << std::endl;  //一行日志的内容
-            ofs.close();
-        }
-        return true;
-    }
 
 protected:
+    static void anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes);
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
     virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
+    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[]);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+    /*virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.0,double Y2=0.0);*/
+
+    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
                              int space_id, double distance,double Y1=0.0,double Y2=0.0);
 //    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
 
@@ -131,22 +143,23 @@ protected:
     /*
      * 移动到目标点，不能到达则旋转
      */
-    bool MoveToTarget(NavMessage::PathNode node, stLimit limit_v, stLimit limit_w, bool autoDirect, bool enable_rotate);
+    bool MoveToTarget(NavMessage::PathNode node, stLimit limit_v, stLimit limit_w,
+                      bool autoDirect, bool enable_rotate,int clampLifterAction=0);
 
     /*
      * autoDirect:自动选择 纵向还是横向巡线
      */
-    MpcResult MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect);
+    MpcResult MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect,int clampLifterAction=0);
 
     /*
      * 旋转摆正，朝向目标点,
      * anyDirect: 任意一个朝向对准目标点
      */
-    virtual bool RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect);
+    //virtual bool RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect);
 
     virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);
 
-    virtual bool AdjustReferToTarget(const Pose2d& target, stLimit limit_v);
+    virtual bool AdjustReferToTarget(const Pose2d& target, stLimit limit_v,int directions);
 
     /*
      * 按照路径点导航
@@ -165,10 +178,12 @@ protected:
      * target:输出目标点
      */
     virtual Navigation::MpcResult
-    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
+    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
+                           NavMessage::PathNode &target,int clampLifterAction=0);
 
-    virtual Navigation::MpcResult
-    RotateAfterOutSpace();
+    virtual Navigation::MpcResult RotateBeforeIntoExport();
+
+    virtual Navigation::MpcResult DoubleOutSpaceCorrectTheta();
 
     virtual bool clamp_close();
 
@@ -184,6 +199,8 @@ protected:
 
     static bool IsUperSpace(NavMessage::PathNode spaceNode);
 
+    virtual int GetSpaceId(NavMessage::PathNode spaceNode);
+
     bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
 
     /* 原地旋转mpc
@@ -221,8 +238,13 @@ protected:
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
-    TimedLockData<ClampStatu> timed_clamp_;     //加持杆
+
+    TimedLockData<InsideDoorStatu> timed_insideDoor_;     //内门
+    TimedLockData<std::map<int,CarrierStatuRegionMap>> timed_Carrier_;     //载车板
+    TimedLockData<ClampStatu> timed_clamp_;     //夹持杆
     TimedLockData<LifterStatus> timed_lifter_;    //提升机构
+    TimedLockData<ClampStatu> timed_other_clamp_;
+    TimedLockData<LifterStatus> timed_other_lifter_;
 
     TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
     TimedLockData<double> timedBrotherV_;              //底盘数据
@@ -237,6 +259,7 @@ protected:
     bool cancel_ = false;
 
     int move_mode_ = eSingle;
+    bool is_master_AGV = false; // 是否是主车
     float wheelBase_;   //两节agv的位姿与单节的位姿变换
 
     bool isInSpace_; //是否在车位或者正在进入车位
@@ -251,6 +274,7 @@ protected:
     double obs_h_;
     TimedLockData<bool> isFront_;
 
+
 };
 
 double limit_gause(double x, double min, double max);

MPC/navigation_main.cpp

@@ -7,6 +7,7 @@
 
 
 NavigationMain::NavigationMain() {
+    is_master_AGV = true;
     move_mode_ = eSingle;
     wheelBase_ = 0;
 }
@@ -68,17 +69,17 @@ void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavRespons
 
 }
 
-void NavigationMain::SendMoveCmd(int mode, ActionType type,
+void NavigationMain::SendMoveCmd(int clampLifterAction,int mode, ActionType type,
                                  double v[], double angular[]) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_);
+        monitor_->set_ToAgvCmd(clampLifterAction,mode, type, v, angular, wheelBase_);
     }
 }
 
-void NavigationMain::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
+void NavigationMain::SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
                                  int space_id, double distance,double Y1,double Y2) {
     if (monitor_) {
-        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_,
+        monitor_->set_ToAgvCmd(clampLifterAction,mode, type, v, angular, wheelBase_,
                                space_id, distance,Y1,Y2);
         if (type == eRotation)
             RWheel_position_ = eR;
@@ -109,14 +110,32 @@ void NavigationMain::ResetOtherLifter(LifterStatus status) {
 
 void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
     NavMessage::AgvStatu speed;
-    if (msg.toProtoMessage(speed)) {
-        ResetStatu(speed.v(), speed.w());
-        ResetClamp((ClampStatu) speed.clamp());
-        ResetOtherClamp((ClampStatu) speed.clamp_other());
-        ResetLifter((LifterStatus) speed.lifter());
-        ResetOtherLifter((LifterStatus) speed.lifter_other());
-        //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
+    if (IsMasterAGV()) {
+        if (msg.toProtoMessage(speed)) {
+            ResetStatu(speed.v(), speed.w());
+            ResetClamp((ClampStatu) speed.clamp());
+            ResetOtherClamp((ClampStatu) speed.clamp_other());
+            ResetLifter((LifterStatus) speed.lifter());
+            ResetOtherLifter((LifterStatus) speed.lifter_other());
+
+            //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
+        }
+    } else {
+        if (msg.toProtoMessage(speed)) {
+            ResetStatu(speed.v(), speed.w());
+            ResetClamp((ClampStatu) speed.clamp());
+//            ResetOtherClamp((ClampStatu) speed.clamp_other());
+            ResetLifter((LifterStatus) speed.lifter());
+//            ResetOtherLifter((LifterStatus) speed.lifter_other());
+            //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
+        }
+    }
+    ResetInsideDoor(speed.door());
+    std::vector<int32_t> vecStatus;
+    for(int i=0;i<speed.zcb_size();++i){
+        vecStatus.push_back(speed.zcb(i));
     }
+    ResetCarrier(vecStatus);
 }
 
 bool NavigationMain::clamp_close() {
@@ -173,7 +192,7 @@ bool NavigationMain::lifter_rise() {
     if (monitor_) {
         printf("双车提升机构提升\n");
         actionType_ = eLifterRise;
-        if (timed_lifter_.Get() == eRose)
+        if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get()==eRose)
             return true;
         else {
             if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
@@ -209,7 +228,7 @@ bool NavigationMain::lifter_down() {
     if (monitor_) {
         printf("双车提升机构下降\n");
         actionType_ = eLifterDown;
-        if (timed_lifter_.Get() == eDowned)
+        if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned)
             return true;
         else {
             if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
@@ -239,7 +258,8 @@ bool NavigationMain::lifter_down() {
 }
 
 Navigation::MpcResult
-NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
+NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
+                                       NavMessage::PathNode &target,int clampLifterAction) {
     if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
 //    if (timedPose_.timeout()) {
         printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
@@ -261,7 +281,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     if(vecTheta<0.)
         isFront=!isFront;
 
-    //前车先到,后车进入2点,保持与前车一致的朝向
+    //前车先到,后车进入2点,保持与车位一致的朝向
     if (isFront==false ) {
         printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
@@ -275,16 +295,17 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
 
     }
 
-
     if (move_mode_ == eDouble){
-        x -= wheelbase/2 * cos(vecTheta);
-        y -= wheelbase/2 * sin(vecTheta);
+        if((!isFront_.Get() && vecTheta>0) || (isFront_.Get()&&vecTheta<=0)){
+            x -= wheelbase * cos(vecTheta);
+            y -= wheelbase * sin(vecTheta);
+        }
+
         printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
     }
 
     Pose2d spaceInCurrent=Pose2d::relativePose(Pose2d(space.x(),space.y(),0),current);
 
-
     target.set_x(x);
     target.set_y(y);
     target.set_theta(current.theta());
@@ -293,7 +314,21 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     target.set_id(space.id());
     printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
 
-
+    if (move_mode_ == eDouble && space.id().find("S1101") != -1){
+        Pose2d nt(x,y,0);
+        Pose2d ntInCurrent=Pose2d::relativePose(nt,current);
+        nt=current*Pose2d(ntInCurrent.x(),0,0);
+        nt.set_theta(current.theta());
+
+        target.set_x(nt.x());
+        target.set_y(nt.y());
+        target.set_theta(nt.theta());
+        target.set_l(0.02);
+        target.set_w(0.5);
+        target.set_id(space.id());
+        std::cout<< "整车进出口：" << nt <<std::endl;
+        return eMpcSuccess;
+    }
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -323,7 +358,8 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 3 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
-
+            if(WaitClampLifterStatu(clampLifterAction)==false)
+                return eMpcFailed;
             usleep(1000*1000);
             Pose2d current_2 = timedPose_.Get();
             if(spaceInCurrent.x()<0.)
@@ -344,10 +380,12 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
             const int down_count = 3;
             double v[down_count] = {0,0,0};
             double w[down_count] = {out[0], out[1], out[2]};
-            SendMoveCmd(move_mode_, eRotation, v, w);
+            SendMoveCmd(clampLifterAction,move_mode_, eRotation, v, w);
             actionType_ = eRotation;
+            /*
             printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
                    timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
+                   */
         }
         continue;
 

MPC/navigation_main.h

@@ -68,11 +68,12 @@ public:
 
 protected:
     virtual Navigation::MpcResult
-    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
+    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
+                           NavMessage::PathNode &target,int clampLifterAction=0);
 
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[]);
-    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
-                             int space_id, double distance,double Y1=0.,double Y2=0.);
+    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[]);
+    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
+                             int space_id, double distance,double Y1=0.0,double Y2=0.0);
 
     virtual bool clamp_close();
 
@@ -86,8 +87,6 @@ protected:
 
 protected:
 
-    TimedLockData<int> timed_other_clamp_;
-    TimedLockData<int> timed_other_lifter_;
 };
 
 #endif //NAVIGATION_NAVIGATION_MAIN_H

MPC/parameter.pb.cc

@@ -42,6 +42,10 @@ PROTOBUF_CONSTEXPR AgvEmqx_parameter::AgvEmqx_parameter(
     &::_pbi::fixed_address_empty_string, ::_pbi::ConstantInitialized {}
   }
 
+  , /*decltype(_impl_.pubclampliftertopic_)*/ {
+    &::_pbi::fixed_address_empty_string, ::_pbi::ConstantInitialized {}
+  }
+
   , /*decltype(_impl_.port_)*/ 0
 
   , /*decltype(_impl_._cached_size_)*/{}} {}
@@ -224,6 +228,7 @@ const ::uint32_t TableStruct_parameter_2eproto::offsets[] PROTOBUF_SECTION_VARIA
     PROTOBUF_FIELD_OFFSET(::NavParameter::AgvEmqx_parameter, _impl_.pubspeedtopic_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::AgvEmqx_parameter, _impl_.subposetopic_),
     PROTOBUF_FIELD_OFFSET(::NavParameter::AgvEmqx_parameter, _impl_.subspeedtopic_),
+    PROTOBUF_FIELD_OFFSET(::NavParameter::AgvEmqx_parameter, _impl_.pubclampliftertopic_),
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavParameter::Emqx_parameter, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -314,12 +319,12 @@ const ::uint32_t TableStruct_parameter_2eproto::offsets[] PROTOBUF_SECTION_VARIA
 static const ::_pbi::MigrationSchema
     schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
         { 0, -1, -1, sizeof(::NavParameter::AgvEmqx_parameter)},
-        { 14, -1, -1, sizeof(::NavParameter::Emqx_parameter)},
-        { 27, -1, -1, sizeof(::NavParameter::BrotherEmqx)},
-        { 39, -1, -1, sizeof(::NavParameter::MPC_parameter)},
-        { 51, -1, -1, sizeof(::NavParameter::SpeedLimit)},
-        { 61, -1, -1, sizeof(::NavParameter::Accuracy)},
-        { 71, 89, -1, sizeof(::NavParameter::Navigation_parameter)},
+        { 15, -1, -1, sizeof(::NavParameter::Emqx_parameter)},
+        { 28, -1, -1, sizeof(::NavParameter::BrotherEmqx)},
+        { 40, -1, -1, sizeof(::NavParameter::MPC_parameter)},
+        { 52, -1, -1, sizeof(::NavParameter::SpeedLimit)},
+        { 62, -1, -1, sizeof(::NavParameter::Accuracy)},
+        { 72, 90, -1, sizeof(::NavParameter::Navigation_parameter)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -332,38 +337,39 @@ static const ::_pb::Message* const file_default_instances[] = {
     &::NavParameter::_Navigation_parameter_default_instance_._instance,
 };
 const char descriptor_table_protodef_parameter_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
-    "\n\017parameter.proto\022\014NavParameter\"\201\001\n\021AgvE"
+    "\n\017parameter.proto\022\014NavParameter\"\236\001\n\021AgvE"
     "mqx_parameter\022\016\n\006NodeId\030\001 \001(\t\022\n\n\002ip\030\002 \001("
     "\t\022\014\n\004port\030\003 \001(\005\022\025\n\rpubSpeedTopic\030\004 \001(\t\022\024"
     "\n\014subPoseTopic\030\005 \001(\t\022\025\n\rsubSpeedTopic\030\006 "
-    "\001(\t\"k\n\016Emqx_parameter\022\016\n\006NodeId\030\001 \001(\t\022\n\n"
-    "\002ip\030\002 \001(\t\022\014\n\004port\030\003 \001(\005\022\025\n\rpubStatuTopic"
-    "\030\004 \001(\t\022\030\n\020pubNavStatuTopic\030\005 \001(\t\"U\n\013Brot"
-    "herEmqx\022\016\n\006NodeId\030\001 \001(\t\022\n\n\002ip\030\002 \001(\t\022\014\n\004p"
-    "ort\030\003 \001(\005\022\034\n\024subBrotherStatuTopic\030\004 \001(\t\""
-    "_\n\rMPC_parameter\022\027\n\017shortest_radius\030\001 \001("
-    "\002\022\n\n\002dt\030\002 \001(\002\022\024\n\014acc_velocity\030\003 \001(\002\022\023\n\013a"
-    "cc_angular\030\004 \001(\002\"&\n\nSpeedLimit\022\013\n\003min\030\001 "
-    "\001(\002\022\013\n\003max\030\002 \001(\002\" \n\010Accuracy\022\t\n\001l\030\001 \001(\002\022"
-    "\t\n\001w\030\002 \001(\002\"\331\003\n\024Navigation_parameter\022\020\n\010m"
-    "ain_agv\030\001 \001(\010\0221\n\010Agv_emqx\030\002 \001(\0132\037.NavPar"
-    "ameter.AgvEmqx_parameter\0223\n\rTerminal_emq"
-    "x\030\003 \001(\0132\034.NavParameter.Emqx_parameter\022/\n"
-    "\014brother_emqx\030\004 \001(\0132\031.NavParameter.Broth"
-    "erEmqx\0224\n\017x_mpc_parameter\030\005 \001(\0132\033.NavPar"
-    "ameter.MPC_parameter\0224\n\017y_mpc_parameter\030"
-    "\006 \001(\0132\033.NavParameter.MPC_parameter\022-\n\013In"
-    "OutVLimit\030\007 \001(\0132\030.NavParameter.SpeedLimi"
-    "t\0223\n\021NodeVelocityLimit\030\010 \001(\0132\030.NavParame"
-    "ter.SpeedLimit\0222\n\020NodeAngularLimit\030\t \001(\013"
-    "2\030.NavParameter.SpeedLimit\022\022\n\nrpc_ipport"
-    "\030\n \001(\tb\006proto3"
+    "\001(\t\022\033\n\023pubClampLifterTopic\030\007 \001(\t\"k\n\016Emqx"
+    "_parameter\022\016\n\006NodeId\030\001 \001(\t\022\n\n\002ip\030\002 \001(\t\022\014"
+    "\n\004port\030\003 \001(\005\022\025\n\rpubStatuTopic\030\004 \001(\t\022\030\n\020p"
+    "ubNavStatuTopic\030\005 \001(\t\"U\n\013BrotherEmqx\022\016\n\006"
+    "NodeId\030\001 \001(\t\022\n\n\002ip\030\002 \001(\t\022\014\n\004port\030\003 \001(\005\022\034"
+    "\n\024subBrotherStatuTopic\030\004 \001(\t\"_\n\rMPC_para"
+    "meter\022\027\n\017shortest_radius\030\001 \001(\002\022\n\n\002dt\030\002 \001"
+    "(\002\022\024\n\014acc_velocity\030\003 \001(\002\022\023\n\013acc_angular\030"
+    "\004 \001(\002\"&\n\nSpeedLimit\022\013\n\003min\030\001 \001(\002\022\013\n\003max\030"
+    "\002 \001(\002\" \n\010Accuracy\022\t\n\001l\030\001 \001(\002\022\t\n\001w\030\002 \001(\002\""
+    "\331\003\n\024Navigation_parameter\022\020\n\010main_agv\030\001 \001"
+    "(\010\0221\n\010Agv_emqx\030\002 \001(\0132\037.NavParameter.AgvE"
+    "mqx_parameter\0223\n\rTerminal_emqx\030\003 \001(\0132\034.N"
+    "avParameter.Emqx_parameter\022/\n\014brother_em"
+    "qx\030\004 \001(\0132\031.NavParameter.BrotherEmqx\0224\n\017x"
+    "_mpc_parameter\030\005 \001(\0132\033.NavParameter.MPC_"
+    "parameter\0224\n\017y_mpc_parameter\030\006 \001(\0132\033.Nav"
+    "Parameter.MPC_parameter\022-\n\013InOutVLimit\030\007"
+    " \001(\0132\030.NavParameter.SpeedLimit\0223\n\021NodeVe"
+    "locityLimit\030\010 \001(\0132\030.NavParameter.SpeedLi"
+    "mit\0222\n\020NodeAngularLimit\030\t \001(\0132\030.NavParam"
+    "eter.SpeedLimit\022\022\n\nrpc_ipport\030\n \001(\tb\006pro"
+    "to3"
 };
 static ::absl::once_flag descriptor_table_parameter_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_parameter_2eproto = {
     false,
     false,
-    1014,
+    1043,
     descriptor_table_protodef_parameter_2eproto,
     "parameter.proto",
     &descriptor_table_parameter_2eproto_once,
@@ -421,6 +427,8 @@ AgvEmqx_parameter::AgvEmqx_parameter(const AgvEmqx_parameter& from)
 
     , decltype(_impl_.subspeedtopic_) {}
 
+    , decltype(_impl_.pubclampliftertopic_) {}
+
     , decltype(_impl_.port_) {}
 
     , /*decltype(_impl_._cached_size_)*/{}};
@@ -461,6 +469,13 @@ AgvEmqx_parameter::AgvEmqx_parameter(const AgvEmqx_parameter& from)
   if (!from._internal_subspeedtopic().empty()) {
     _this->_impl_.subspeedtopic_.Set(from._internal_subspeedtopic(), _this->GetArenaForAllocation());
   }
+  _impl_.pubclampliftertopic_.InitDefault();
+  #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+        _impl_.pubclampliftertopic_.Set("", GetArenaForAllocation());
+  #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  if (!from._internal_pubclampliftertopic().empty()) {
+    _this->_impl_.pubclampliftertopic_.Set(from._internal_pubclampliftertopic(), _this->GetArenaForAllocation());
+  }
   _this->_impl_.port_ = from._impl_.port_;
   // @@protoc_insertion_point(copy_constructor:NavParameter.AgvEmqx_parameter)
 }
@@ -478,6 +493,8 @@ inline void AgvEmqx_parameter::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.subspeedtopic_) {}
 
+    , decltype(_impl_.pubclampliftertopic_) {}
+
     , decltype(_impl_.port_) { 0 }
 
     , /*decltype(_impl_._cached_size_)*/{}
@@ -502,6 +519,10 @@ inline void AgvEmqx_parameter::SharedCtor(::_pb::Arena* arena) {
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
         _impl_.subspeedtopic_.Set("", GetArenaForAllocation());
   #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  _impl_.pubclampliftertopic_.InitDefault();
+  #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+        _impl_.pubclampliftertopic_.Set("", GetArenaForAllocation());
+  #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
 }
 
 AgvEmqx_parameter::~AgvEmqx_parameter() {
@@ -520,6 +541,7 @@ inline void AgvEmqx_parameter::SharedDtor() {
   _impl_.pubspeedtopic_.Destroy();
   _impl_.subposetopic_.Destroy();
   _impl_.subspeedtopic_.Destroy();
+  _impl_.pubclampliftertopic_.Destroy();
 }
 
 void AgvEmqx_parameter::SetCachedSize(int size) const {
@@ -537,6 +559,7 @@ void AgvEmqx_parameter::Clear() {
   _impl_.pubspeedtopic_.ClearToEmpty();
   _impl_.subposetopic_.ClearToEmpty();
   _impl_.subspeedtopic_.ClearToEmpty();
+  _impl_.pubclampliftertopic_.ClearToEmpty();
   _impl_.port_ = 0;
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
@@ -611,6 +634,17 @@ const char* AgvEmqx_parameter::_InternalParse(const char* ptr, ::_pbi::ParseCont
           goto handle_unusual;
         }
         continue;
+      // string pubClampLifterTopic = 7;
+      case 7:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 58)) {
+          auto str = _internal_mutable_pubclampliftertopic();
+          ptr = ::_pbi::InlineGreedyStringParser(str, ptr, ctx);
+          CHK_(ptr);
+          CHK_(::_pbi::VerifyUTF8(str, "NavParameter.AgvEmqx_parameter.pubClampLifterTopic"));
+        } else {
+          goto handle_unusual;
+        }
+        continue;
       default:
         goto handle_unusual;
     }  // switch
@@ -687,6 +721,14 @@ failure:
     target = stream->WriteStringMaybeAliased(6, _s, target);
   }
 
+  // string pubClampLifterTopic = 7;
+  if (!this->_internal_pubclampliftertopic().empty()) {
+    const std::string& _s = this->_internal_pubclampliftertopic();
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
+        _s.data(), static_cast<int>(_s.length()), ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE, "NavParameter.AgvEmqx_parameter.pubClampLifterTopic");
+    target = stream->WriteStringMaybeAliased(7, _s, target);
+  }
+
   if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
     target = ::_pbi::WireFormat::InternalSerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
@@ -733,6 +775,12 @@ failure:
                                     this->_internal_subspeedtopic());
   }
 
+  // string pubClampLifterTopic = 7;
+  if (!this->_internal_pubclampliftertopic().empty()) {
+    total_size += 1 + ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+                                    this->_internal_pubclampliftertopic());
+  }
+
   // int32 port = 3;
   if (this->_internal_port() != 0) {
     total_size += ::_pbi::WireFormatLite::Int32SizePlusOne(
@@ -772,6 +820,9 @@ void AgvEmqx_parameter::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, cons
   if (!from._internal_subspeedtopic().empty()) {
     _this->_internal_set_subspeedtopic(from._internal_subspeedtopic());
   }
+  if (!from._internal_pubclampliftertopic().empty()) {
+    _this->_internal_set_pubclampliftertopic(from._internal_pubclampliftertopic());
+  }
   if (from._internal_port() != 0) {
     _this->_internal_set_port(from._internal_port());
   }
@@ -804,6 +855,8 @@ void AgvEmqx_parameter::InternalSwap(AgvEmqx_parameter* other) {
                                        &other->_impl_.subposetopic_, rhs_arena);
   ::_pbi::ArenaStringPtr::InternalSwap(&_impl_.subspeedtopic_, lhs_arena,
                                        &other->_impl_.subspeedtopic_, rhs_arena);
+  ::_pbi::ArenaStringPtr::InternalSwap(&_impl_.pubclampliftertopic_, lhs_arena,
+                                       &other->_impl_.pubclampliftertopic_, rhs_arena);
 
   swap(_impl_.port_, other->_impl_.port_);
 }

MPC/parameter.pb.h

@@ -230,6 +230,7 @@ class AgvEmqx_parameter final :
     kPubSpeedTopicFieldNumber = 4,
     kSubPoseTopicFieldNumber = 5,
     kSubSpeedTopicFieldNumber = 6,
+    kPubClampLifterTopicFieldNumber = 7,
     kPortFieldNumber = 3,
   };
   // string NodeId = 1;
@@ -331,6 +332,26 @@ class AgvEmqx_parameter final :
       const std::string& value);
   std::string* _internal_mutable_subspeedtopic();
 
+  public:
+  // string pubClampLifterTopic = 7;
+  void clear_pubclampliftertopic() ;
+  const std::string& pubclampliftertopic() const;
+
+
+
+
+  template <typename Arg_ = const std::string&, typename... Args_>
+  void set_pubclampliftertopic(Arg_&& arg, Args_... args);
+  std::string* mutable_pubclampliftertopic();
+  PROTOBUF_NODISCARD std::string* release_pubclampliftertopic();
+  void set_allocated_pubclampliftertopic(std::string* ptr);
+
+  private:
+  const std::string& _internal_pubclampliftertopic() const;
+  inline PROTOBUF_ALWAYS_INLINE void _internal_set_pubclampliftertopic(
+      const std::string& value);
+  std::string* _internal_mutable_pubclampliftertopic();
+
   public:
   // int32 port = 3;
   void clear_port() ;
@@ -355,6 +376,7 @@ class AgvEmqx_parameter final :
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr pubspeedtopic_;
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr subposetopic_;
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr subspeedtopic_;
+    ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr pubclampliftertopic_;
     ::int32_t port_;
     mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
@@ -1928,6 +1950,53 @@ inline void AgvEmqx_parameter::set_allocated_subspeedtopic(std::string* value) {
   // @@protoc_insertion_point(field_set_allocated:NavParameter.AgvEmqx_parameter.subSpeedTopic)
 }
 
+// string pubClampLifterTopic = 7;
+inline void AgvEmqx_parameter::clear_pubclampliftertopic() {
+  _impl_.pubclampliftertopic_.ClearToEmpty();
+}
+inline const std::string& AgvEmqx_parameter::pubclampliftertopic() const {
+  // @@protoc_insertion_point(field_get:NavParameter.AgvEmqx_parameter.pubClampLifterTopic)
+  return _internal_pubclampliftertopic();
+}
+template <typename Arg_, typename... Args_>
+inline PROTOBUF_ALWAYS_INLINE void AgvEmqx_parameter::set_pubclampliftertopic(Arg_&& arg,
+                                                     Args_... args) {
+  ;
+  _impl_.pubclampliftertopic_.Set(static_cast<Arg_&&>(arg), args..., GetArenaForAllocation());
+  // @@protoc_insertion_point(field_set:NavParameter.AgvEmqx_parameter.pubClampLifterTopic)
+}
+inline std::string* AgvEmqx_parameter::mutable_pubclampliftertopic() {
+  std::string* _s = _internal_mutable_pubclampliftertopic();
+  // @@protoc_insertion_point(field_mutable:NavParameter.AgvEmqx_parameter.pubClampLifterTopic)
+  return _s;
+}
+inline const std::string& AgvEmqx_parameter::_internal_pubclampliftertopic() const {
+  return _impl_.pubclampliftertopic_.Get();
+}
+inline void AgvEmqx_parameter::_internal_set_pubclampliftertopic(const std::string& value) {
+  ;
+
+
+  _impl_.pubclampliftertopic_.Set(value, GetArenaForAllocation());
+}
+inline std::string* AgvEmqx_parameter::_internal_mutable_pubclampliftertopic() {
+  ;
+  return _impl_.pubclampliftertopic_.Mutable( GetArenaForAllocation());
+}
+inline std::string* AgvEmqx_parameter::release_pubclampliftertopic() {
+  // @@protoc_insertion_point(field_release:NavParameter.AgvEmqx_parameter.pubClampLifterTopic)
+  return _impl_.pubclampliftertopic_.Release();
+}
+inline void AgvEmqx_parameter::set_allocated_pubclampliftertopic(std::string* value) {
+  _impl_.pubclampliftertopic_.SetAllocated(value, GetArenaForAllocation());
+  #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+        if (_impl_.pubclampliftertopic_.IsDefault()) {
+          _impl_.pubclampliftertopic_.Set("", GetArenaForAllocation());
+        }
+  #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  // @@protoc_insertion_point(field_set_allocated:NavParameter.AgvEmqx_parameter.pubClampLifterTopic)
+}
+
 // -------------------------------------------------------------------
 
 // Emqx_parameter

MPC/rotate_mpc.cpp

@@ -124,9 +124,10 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = 5.0 / 180 * M_PI  / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
-    double min_angular_velocity = 1 / 180 * M_PI;
-    double max_angular_acceleration_velocity = mpc_param.acc_angular; //30.0 / 180 * M_PI;//
+    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+    double min_angular_velocity = min_velocity_ ;
+    double max_angular_acceleration_velocity = mpc_param.acc_angular;
+    //double max_angular_acceleration_velocity=0.44;///(1+exp(-36.*(angular_velocity-0.15)))+0.085;
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;
     if (angular_velocity < -max_angular_velocity) angular_velocity = -max_angular_velocity;

controller.cpp

@@ -26,6 +26,11 @@ int main(int argc,char* argv[])
     printf(" read proto parameter failed:%s\n",parameter_file.c_str());
     return -2;
   }
+//  NavParameter::Version nav_version;
+//  if(proto_tool::get_instance_pointer()->read_proto_param(parameter_file,nav_version)==false) {
+//      return -2;
+//  }
+//  printf(" version: %s\n",nav_version.version().c_str());
   Navigation* g_navigator= nullptr;
   if(parameter.main_agv()==true)
     g_navigator=new NavigationMain();
@@ -35,7 +40,7 @@ int main(int argc,char* argv[])
   if(g_navigator->Init(parameter))
     printf(" navigation inited\n");
   else{
-    printf(" navigation init failed");
+    printf(" navigation init failed\n");
     return -1;
   }
   //启动rpc server

message.proto

@@ -17,6 +17,8 @@ message AgvStatu {
     int32 clamp_other = 4; //另外一节
     int32 lifter = 5;   //提升机构 0中间状态，1上升到位， 2下降到位
     int32 lifter_other = 6;     //另外一节
+    repeated int32 zcb = 7; //载车板
+    int32 door = 8; // 门控
 }
 
 message ToAgvCmd {
@@ -32,9 +34,10 @@ message ToAgvCmd {
     float L1 = 10;  //轴距
     int32 P1 = 11; //车位编号
     float D1 = 12; //距目标点距离
-    float Y1=13;    //前车小w
-    float Y2=14;    //后车小w
-    int32 end = 15;
+    float Y1=13;    //主车相对前车的dy
+    float Y2=14;    //副车相对前车角度偏差纠偏的小w
+    int32 CL=15;
+    int32 end = 16;
 }
 
 message Pose2d

parameter.proto

@@ -60,7 +60,3 @@ message Navigation_parameter
   string rpc_ipport=10;
 }
 
-message Version
-{
-  string version=1;
-}