MPC/custom_type.h

@@ -6,6 +6,7 @@
 #ifndef NAVIGATION_CUSTOM_TYPE_H
 #define NAVIGATION_CUSTOM_TYPE_H
 
+#include <map>
 //////////////////////////////////////////////////
 /// 封装的enum模板类,解决原生enum无法参与位运算问题
 template<typename Enum>
@@ -103,9 +104,9 @@ enum ActionType {
     eLifterDown =8,
     eClampFullyOpen = 9
 };
-///////////////////////////////////////////////
+//////////////////////////////////////////////////
 
-///////////////////////////////////////////////
+//////////////////////////////////////////////////
 /// 提升机构运动状态
 enum LifterStatus {
     eInvalid = 0,
@@ -114,4 +115,52 @@ enum LifterStatus {
 };
 //////////////////////////////////////////////////
 
+//////////////////////////////////////////////////
+/// 并行夹持和提升机构运作
+enum eClamLiftActionType{
+    eByteCLampClose = 1,
+    eByteClampHalfOpen=2,
+    eByteClampFullyOpen=4,
+    eByteLifterDown=8,
+    eByteLifterUp=16
+};
+//////////////////////////////////////////////////
+
+//////////////////////////////////////////////////
+/// plc区域+编号载车板 对应 车位表
+enum eRegionId{
+    eRegion0 = 0,
+    eRegion1 = 1,
+    eRegion2,
+    eRegion3,
+    eRegion4,
+    eRegion5,
+    eRegion6,
+    eRegion7,
+    eRegion8,
+    eRegion9,
+    eRegion10,
+    eRegion11
+};
+
+enum eCarrierIdInRegionId{
+    eCarrier0 = 0,
+    eCarrier1 = 1,
+    eCarrier2,
+    eCarrier3,
+    eCarrier4,
+    eCarrier5,
+    eCarrier6,
+    eCarrier7,
+    eCarrier8,
+    eCarrier9,
+    eCarrier10,
+    eCarrier11
+};
+
+typedef std::pair<short, short > Region_Carry;
+typedef std::map<int ,Region_Carry> SpaceNo2Region_Carry;
+
+//////////////////////////////////////////////////
+
 #endif //NAVIGATION_CUSTOM_TYPE_H

MPC/loaded_mpc.cpp

@@ -7,26 +7,35 @@
 #include <cppad/cppad.hpp>
 #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;
-size_t ny = nx + N;
-size_t nth = ny + N;
-size_t nv = nth + N;
-size_t ndlt = nv + N;
-
-size_t nobs = ndlt + N;
-
 class FG_eval_half_agv {
 public:
     // Fitted polynomial coefficients
     Eigen::VectorXd m_coeffs;                 //曲线方程
     Eigen::VectorXd m_statu;                  //当前状态
     Eigen::VectorXd m_condition;              //搜索条件参数
+    size_t delay;
+    size_t down_count = 3;//下发前多少步
+    size_t N;
+    size_t nx = 0;
+    size_t ny = nx;
+    size_t nth = ny ;
+    size_t nv = nth ;
+    size_t ndlt = nv ;
+    size_t nobs = ndlt ;
     bool directY_;
-    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition,bool directY) {
+    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition,bool directY,int steps) {
+        //优化考虑后面多少步
+        N=steps;
+        nx = 0;
+        ny = nx + N;
+        nth = ny + N;
+        nv = nth + N;
+        ndlt = nv + N;
+        nobs = ndlt + N;
+
         m_coeffs = coeffs;
         m_statu = statu;
         m_condition = condition;
@@ -154,12 +163,24 @@ LoadedMPC::~LoadedMPC() {}
 MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
                           std::vector<double> &out, Trajectory &select_traj,
                           Trajectory &optimize_trajectory,bool directY) {
+
+
     auto start = std::chrono::steady_clock::now();
     // State vector holds all current values neede for vars below
     Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
     double line_velocity = statu[3];
     double wmg = statu[4];
 
+
+    size_t N=15;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
+    size_t nx = 0;
+    size_t ny = nx + N;
+    size_t nth = ny + N;
+    size_t nv = nth + N;
+    size_t ndlt = nv + N;
+
+    size_t nobs = ndlt + N;
+
     //纠正角速度/线速度，使其满足最小转弯半径
     double angular = wmg;
     double radius = mpc_param.shortest_radius * (1.0 / sqrt(line_velocity * line_velocity + 1e-10));
@@ -191,22 +212,26 @@ 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
 
+    double pdt = mpc_param.dt;
+    double dt=fabs(pdt-0.1)/(1+exp(-25.*(fabs(line_velocity)-0.8)))+0.1;
+
+    //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
+    double max_dlt = max_wmg;//5*M_PI/180.0;
+    double acc_sigmoid= (max_velocity_+1.3)/(1+exp(-4.*(fabs(line_velocity)-0.7)))+0.3;
+    double max_acc_line_velocity = mpc_param.acc_velocity*acc_sigmoid;
+    double max_acc_dlt = mpc_param.acc_angular;
+
+    double ref_velocity = 0.5*dis*dis+min_velocity_;//max_velocity_/(1+exp(-4.*(dis-0.8)))+min_velocity_;
+
     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;
 
-    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;
-
     size_t n_vars = N * 5;
 
     Dvector vars(n_vars);
@@ -317,7 +342,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     Eigen::VectorXd condition(6);
     condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
-    FG_eval_half_agv fg_eval(coef, statu_velocity, condition,directY);
+    FG_eval_half_agv fg_eval(coef, statu_velocity, condition,directY,N);
 
     // options for IPOPT solver
     std::string options;
@@ -368,14 +393,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_)*/{}} {}
@@ -132,7 +143,9 @@ PROTOBUF_ATTRIBUTE_NO_DESTROY PROTOBUF_CONSTINIT
 template <typename>
 PROTOBUF_CONSTEXPR PathNode::PathNode(
     ::_pbi::ConstantInitialized): _impl_{
-    /*decltype(_impl_.id_)*/ {
+    /*decltype(_impl_._has_bits_)*/{}
+  , /*decltype(_impl_._cached_size_)*/{}
+  , /*decltype(_impl_.id_)*/ {
     &::_pbi::fixed_address_empty_string, ::_pbi::ConstantInitialized {}
   }
 
@@ -145,8 +158,7 @@ PROTOBUF_CONSTEXPR PathNode::PathNode(
   , /*decltype(_impl_.w_)*/ 0
 
   , /*decltype(_impl_.theta_)*/ 0
-
-  , /*decltype(_impl_._cached_size_)*/{}} {}
+} {}
 struct PathNodeDefaultTypeInternal {
   PROTOBUF_CONSTEXPR PathNodeDefaultTypeInternal() : _instance(::_pbi::ConstantInitialized{}) {}
   ~PathNodeDefaultTypeInternal() {}
@@ -351,6 +363,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 +385,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_),
@@ -383,7 +400,7 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.x_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.y_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::Pose2d, _impl_.theta_),
-    ~0u,  // no _has_bits_
+    PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_._has_bits_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _internal_metadata_),
     ~0u,  // no _extensions_
     ~0u,  // no _oneof_case_
@@ -397,6 +414,12 @@ const ::uint32_t TableStruct_message_2eproto::offsets[] PROTOBUF_SECTION_VARIABL
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.w_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.theta_),
     PROTOBUF_FIELD_OFFSET(::NavMessage::PathNode, _impl_.id_),
+    ~0u,
+    ~0u,
+    ~0u,
+    ~0u,
+    ~0u,
+    0,
     ~0u,  // no _has_bits_
     PROTOBUF_FIELD_OFFSET(::NavMessage::Trajectory, _internal_metadata_),
     ~0u,  // no _extensions_
@@ -508,16 +531,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, 78, -1, sizeof(::NavMessage::PathNode)},
+        { 84, -1, -1, sizeof(::NavMessage::Trajectory)},
+        { 93, 108, -1, sizeof(::NavMessage::NewAction)},
+        { 115, -1, -1, sizeof(::NavMessage::NavCmd)},
+        { 126, -1, -1, sizeof(::NavMessage::NavResponse)},
+        { 136, -1, -1, sizeof(::NavMessage::ManualCmd)},
+        { 147, 164, -1, sizeof(::NavMessage::NavStatu)},
+        { 173, 185, -1, sizeof(::NavMessage::RobotStatu)},
 };
 
 static const ::_pb::Message* const file_default_instances[] = {
@@ -537,49 +560,51 @@ 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\"]\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\017\n\002id\030\006 \001(\tH\000\210\001\001B\005\n\003_id\"/\n\nTrajec"
+    "tory\022!\n\005poses\030\001 \003(\0132\022.NavMessage.Pose2d\""
+    "\345\001\n\tNewAction\022\014\n\004type\030\001 \001(\005\022\'\n\tspaceNode"
+    "\030\002 \001(\0132\024.NavMessage.PathNode\022&\n\010passNode"
+    "\030\003 \001(\0132\024.NavMessage.PathNode\022(\n\nstreetNo"
+    "de\030\004 \001(\0132\024.NavMessage.PathNode\022\'\n\tpathNo"
+    "des\030\005 \003(\0132\024.NavMessage.PathNode\022\021\n\twheel"
+    "base\030\006 \001(\002\022\023\n\013changedMode\030\007 \001(\005\"P\n\006NavCm"
+    "d\022\016\n\006action\030\001 \001(\005\022\013\n\003key\030\002 \001(\t\022)\n\nnewAct"
+    "ions\030\005 \003(\0132\025.NavMessage.NewAction\"(\n\013Nav"
+    "Response\022\013\n\003ret\030\001 \001(\005\022\014\n\004info\030\002 \001(\t\"B\n\tM"
+    "anualCmd\022\013\n\003key\030\001 \001(\t\022\026\n\016operation_type\030"
+    "\002 \001(\005\022\020\n\010velocity\030\003 \001(\002\"\366\001\n\010NavStatu\022\r\n\005"
+    "statu\030\001 \001(\005\022\020\n\010main_agv\030\002 \001(\010\022\021\n\tmove_mo"
+    "de\030\003 \001(\005\022(\n\004odom\030\004 \001(\0132\032.NavMessage.Lida"
+    "rOdomStatu\022\013\n\003key\030\005 \001(\t\022-\n\rselected_traj"
+    "\030\006 \001(\0132\026.NavMessage.Trajectory\022,\n\014predic"
+    "t_traj\030\007 \001(\0132\026.NavMessage.Trajectory\022\020\n\010"
+    "in_space\030\010 \001(\010\022\020\n\010space_id\030\t \001(\t\"Y\n\nRobo"
+    "tStatu\022\t\n\001x\030\001 \001(\002\022\t\n\001y\030\002 \001(\002\022\r\n\005theta\030\003 "
+    "\001(\002\022&\n\010agvStatu\030\004 \001(\0132\024.NavMessage.AgvSt"
+    "atu2\302\001\n\nNavExcutor\0226\n\005Start\022\022.NavMessage"
+    ".NavCmd\032\027.NavMessage.NavResponse\"\000\0227\n\006Ca"
+    "ncel\022\022.NavMessage.NavCmd\032\027.NavMessage.Na"
+    "vResponse\"\000\022C\n\017ManualOperation\022\025.NavMess"
+    "age.ManualCmd\032\027.NavMessage.NavResponse\"\000"
+    "b\006proto3"
 };
 static ::absl::once_flag descriptor_table_message_2eproto_once;
 const ::_pbi::DescriptorTable descriptor_table_message_2eproto = {
     false,
     false,
-    1532,
+    1608,
     descriptor_table_protodef_message_2eproto,
     "message.proto",
     &descriptor_table_message_2eproto_once,
@@ -967,16 +992,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 +1039,8 @@ inline void AgvStatu::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.lifter_other_) { 0 }
 
+    , decltype(_impl_.door_) { 0 }
+
     , /*decltype(_impl_._cached_size_)*/{}
   };
 }
@@ -1003,6 +1056,7 @@ AgvStatu::~AgvStatu() {
 
 inline void AgvStatu::SharedDtor() {
   ABSL_DCHECK(GetArenaForAllocation() == nullptr);
+  _impl_.zcb_.~RepeatedField();
 }
 
 void AgvStatu::SetCachedSize(int size) const {
@@ -1015,9 +1069,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 +1136,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 +1236,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 +1268,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 +1324,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 +1348,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 +1375,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 +1395,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 +1454,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 +1609,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 +1790,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 +1938,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 +2054,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());
   }
@@ -2155,6 +2375,12 @@ void Pose2d::InternalSwap(Pose2d* other) {
 
 class PathNode::_Internal {
  public:
+  using HasBits = decltype(std::declval<PathNode>()._impl_._has_bits_);
+  static constexpr ::int32_t kHasBitsOffset =
+    8 * PROTOBUF_FIELD_OFFSET(PathNode, _impl_._has_bits_);
+  static void set_has_id(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
 };
 
 PathNode::PathNode(::PROTOBUF_NAMESPACE_ID::Arena* arena)
@@ -2166,7 +2392,9 @@ PathNode::PathNode(const PathNode& from)
   : ::PROTOBUF_NAMESPACE_ID::Message() {
   PathNode* const _this = this; (void)_this;
   new (&_impl_) Impl_{
-      decltype(_impl_.id_) {}
+      decltype(_impl_._has_bits_){from._impl_._has_bits_}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.id_) {}
 
     , decltype(_impl_.x_) {}
 
@@ -2177,15 +2405,14 @@ PathNode::PathNode(const PathNode& from)
     , decltype(_impl_.w_) {}
 
     , decltype(_impl_.theta_) {}
-
-    , /*decltype(_impl_._cached_size_)*/{}};
+  };
 
   _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
   _impl_.id_.InitDefault();
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
         _impl_.id_.Set("", GetArenaForAllocation());
   #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
-  if (!from._internal_id().empty()) {
+  if ((from._impl_._has_bits_[0] & 0x00000001u) != 0) {
     _this->_impl_.id_.Set(from._internal_id(), _this->GetArenaForAllocation());
   }
   ::memcpy(&_impl_.x_, &from._impl_.x_,
@@ -2197,7 +2424,9 @@ PathNode::PathNode(const PathNode& from)
 inline void PathNode::SharedCtor(::_pb::Arena* arena) {
   (void)arena;
   new (&_impl_) Impl_{
-      decltype(_impl_.id_) {}
+      decltype(_impl_._has_bits_){}
+    , /*decltype(_impl_._cached_size_)*/{}
+    , decltype(_impl_.id_) {}
 
     , decltype(_impl_.x_) { 0 }
 
@@ -2209,7 +2438,6 @@ inline void PathNode::SharedCtor(::_pb::Arena* arena) {
 
     , decltype(_impl_.theta_) { 0 }
 
-    , /*decltype(_impl_._cached_size_)*/{}
   };
   _impl_.id_.InitDefault();
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
@@ -2241,15 +2469,20 @@ void PathNode::Clear() {
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  _impl_.id_.ClearToEmpty();
+  cached_has_bits = _impl_._has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    _impl_.id_.ClearNonDefaultToEmpty();
+  }
   ::memset(&_impl_.x_, 0, static_cast<::size_t>(
       reinterpret_cast<char*>(&_impl_.theta_) -
       reinterpret_cast<char*>(&_impl_.x_)) + sizeof(_impl_.theta_));
+  _impl_._has_bits_.Clear();
   _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
 }
 
 const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx) {
 #define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
   while (!ctx->Done(&ptr)) {
     ::uint32_t tag;
     ptr = ::_pbi::ReadTag(ptr, &tag);
@@ -2299,7 +2532,7 @@ const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
           goto handle_unusual;
         }
         continue;
-      // string id = 6;
+      // optional string id = 6;
       case 6:
         if (PROTOBUF_PREDICT_TRUE(static_cast<::uint8_t>(tag) == 50)) {
           auto str = _internal_mutable_id();
@@ -2326,6 +2559,7 @@ const char* PathNode::_InternalParse(const char* ptr, ::_pbi::ParseContext* ctx)
     CHK_(ptr != nullptr);
   }  // while
 message_done:
+  _impl_._has_bits_.Or(has_bits);
   return ptr;
 failure:
   ptr = nullptr;
@@ -2394,8 +2628,9 @@ failure:
         5, this->_internal_theta(), target);
   }
 
-  // string id = 6;
-  if (!this->_internal_id().empty()) {
+  cached_has_bits = _impl_._has_bits_[0];
+  // optional string id = 6;
+  if (cached_has_bits & 0x00000001u) {
     const std::string& _s = this->_internal_id();
     ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::VerifyUtf8String(
         _s.data(), static_cast<int>(_s.length()), ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::SERIALIZE, "NavMessage.PathNode.id");
@@ -2418,8 +2653,9 @@ failure:
   // Prevent compiler warnings about cached_has_bits being unused
   (void) cached_has_bits;
 
-  // string id = 6;
-  if (!this->_internal_id().empty()) {
+  // optional string id = 6;
+  cached_has_bits = _impl_._has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
     total_size += 1 + ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
                                     this->_internal_id());
   }
@@ -2487,7 +2723,7 @@ void PathNode::MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTO
   ::uint32_t cached_has_bits = 0;
   (void) cached_has_bits;
 
-  if (!from._internal_id().empty()) {
+  if ((from._impl_._has_bits_[0] & 0x00000001u) != 0) {
     _this->_internal_set_id(from._internal_id());
   }
   static_assert(sizeof(::uint32_t) == sizeof(float), "Code assumes ::uint32_t and float are the same size.");
@@ -2544,6 +2780,7 @@ void PathNode::InternalSwap(PathNode* other) {
   auto* lhs_arena = GetArenaForAllocation();
   auto* rhs_arena = other->GetArenaForAllocation();
   _internal_metadata_.InternalSwap(&other->_internal_metadata_);
+  swap(_impl_._has_bits_[0], other->_impl_._has_bits_[0]);
   ::_pbi::ArenaStringPtr::InternalSwap(&_impl_.id_, lhs_arena,
                                        &other->_impl_.id_, rhs_arena);
   ::PROTOBUF_NAMESPACE_ID::internal::memswap<

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_;
   };
@@ -1153,7 +1224,8 @@ class PathNode final :
     kWFieldNumber = 4,
     kThetaFieldNumber = 5,
   };
-  // string id = 6;
+  // optional string id = 6;
+  bool has_id() const;
   void clear_id() ;
   const std::string& id() const;
 
@@ -1231,13 +1303,14 @@ class PathNode final :
   typedef void InternalArenaConstructable_;
   typedef void DestructorSkippable_;
   struct Impl_ {
+    ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
     ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr id_;
     float x_;
     float y_;
     float l_;
     float w_;
     float theta_;
-    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
   };
   union { Impl_ _impl_; };
   friend struct ::TableStruct_message_2eproto;
@@ -2943,6 +3016,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 +3324,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;
 }
@@ -3375,9 +3572,14 @@ inline void PathNode::_internal_set_theta(float value) {
   _impl_.theta_ = value;
 }
 
-// string id = 6;
+// optional string id = 6;
+inline bool PathNode::has_id() const {
+  bool value = (_impl_._has_bits_[0] & 0x00000001u) != 0;
+  return value;
+}
 inline void PathNode::clear_id() {
   _impl_.id_.ClearToEmpty();
+  _impl_._has_bits_[0] &= ~0x00000001u;
 }
 inline const std::string& PathNode::id() const {
   // @@protoc_insertion_point(field_get:NavMessage.PathNode.id)
@@ -3386,7 +3588,7 @@ inline const std::string& PathNode::id() const {
 template <typename Arg_, typename... Args_>
 inline PROTOBUF_ALWAYS_INLINE void PathNode::set_id(Arg_&& arg,
                                                      Args_... args) {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
   _impl_.id_.Set(static_cast<Arg_&&>(arg), args..., GetArenaForAllocation());
   // @@protoc_insertion_point(field_set:NavMessage.PathNode.id)
 }
@@ -3399,20 +3601,33 @@ inline const std::string& PathNode::_internal_id() const {
   return _impl_.id_.Get();
 }
 inline void PathNode::_internal_set_id(const std::string& value) {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
 
 
   _impl_.id_.Set(value, GetArenaForAllocation());
 }
 inline std::string* PathNode::_internal_mutable_id() {
-  ;
+  _impl_._has_bits_[0] |= 0x00000001u;
   return _impl_.id_.Mutable( GetArenaForAllocation());
 }
 inline std::string* PathNode::release_id() {
   // @@protoc_insertion_point(field_release:NavMessage.PathNode.id)
-  return _impl_.id_.Release();
+  if ((_impl_._has_bits_[0] & 0x00000001u) == 0) {
+    return nullptr;
+  }
+  _impl_._has_bits_[0] &= ~0x00000001u;
+  auto* released = _impl_.id_.Release();
+  #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  _impl_.id_.Set("", GetArenaForAllocation());
+  #endif  // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  return released;
 }
 inline void PathNode::set_allocated_id(std::string* value) {
+  if (value != nullptr) {
+    _impl_._has_bits_[0] |= 0x00000001u;
+  } else {
+    _impl_._has_bits_[0] &= ~0x00000001u;
+  }
   _impl_.id_.SetAllocated(value, GetArenaForAllocation());
   #ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
         if (_impl_.id_.IsDefault()) {

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.h

@@ -37,13 +37,28 @@ public:
         eFullyOpened = 3 //全开位
     };
 
+    enum CarrierStatu{
+        eCarrierInvalid=0,
+        eCarrierUp=1,
+        eCarrierDown=2
+    };
+
+    typedef std::map<int,CarrierStatu> CarrierStatuRegionMap;
+
+    enum DoorStatu{
+        eDoorInvalid=0,
+        eDoorOpened=1,
+        eDoorClosed=2
+    };
+
+    typedef std::pair<DoorStatu,DoorStatu> DoorStatusInOnePort;//<out, ins>
+    typedef std::map<int,DoorStatusInOnePort> DoorStatusMap;
     enum MpcResult {
         eMpcSuccess = 0,
         eMpcFailed,
         eImpossibleToTarget
     };
 
-
 public:
     Navigation();
 
@@ -52,6 +67,18 @@ public:
 
     bool Init(const NavParameter::Navigation_parameter &parameter);
 
+    bool LoadSpaceNo2CarrierNo();
+
+    bool SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no);
+
+    CarrierStatu GetCarrierStatusBySpaceID(int space_id);
+
+    bool Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res);
+
+    static void anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status);
+
+    virtual bool IsMasterAGV();
+
     virtual void ResetPose(const Pose2d &pose);
 
     void ResetStatu(double v, double a);
@@ -60,6 +87,10 @@ public:
 
     virtual void ResetLifter(LifterStatus status);
 
+    virtual void ResetDoor(int32_t status);
+
+    virtual void ResetCarrier(std::vector<int32_t> status);
+
     TimedLockData<Pose2d> &RealTimePose() {
         return timedPose_;
     }
@@ -83,37 +114,34 @@ public:
 
     bool SlowlyStop();
 
-    virtual void SwitchMode(int mode, float wheelBase);
+    virtual bool SwitchMode(int mode, float wheelBase);
 
     bool PoseTimeout();
+    bool WaitClampLifterStatu(int clampLifterAction);
+    bool WaitInsideDoorCompleted(int doorID, DoorStatu status);
+    bool WaitCarrierCompleted(int spaceID,CarrierStatu statu);
 
-    // 测试日志打印
-    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;
-    }
+    bool TargetIsEntrance(NavMessage::PathNode node);
+    bool TargetIsExport(NavMessage::PathNode node);
+
+    int GetPortIDBySpace(NavMessage::PathNode node);
+
+    // 获取当前未完成动作数量
+    virtual int GetRemainActionsCount();
+    virtual NavMessage::NewAction GetCurrentAction();
 
 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[],
+                             int space_id, double distance,double Y1=0.0,double Y2=0.0);*/
 
-    virtual void SendMoveCmd(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 void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);
 
@@ -131,28 +159,29 @@ 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);
 
     /*
      * 按照路径点导航
      */
 
-    bool execute_nodes(NavMessage::NewAction action,int next_actionTypeID=-1);
+    bool execute_nodes(NavMessage::NewAction action);
 
     /*
      * 进出库
@@ -165,10 +194,13 @@ 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();
 
@@ -182,7 +214,9 @@ protected:
 
     virtual bool singleClamp_fullyOpen_until_inspaceBegin();
 
-    static bool IsUperSpace(NavMessage::PathNode spaceNode);
+    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);
 
@@ -206,6 +240,8 @@ protected:
 
     virtual void publish_statu(NavMessage::NavStatu &statu);
 
+
+
 protected:
     std::mutex mtx_;
     bool exit_ = false;
@@ -221,8 +257,13 @@ protected:
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
-    TimedLockData<ClampStatu> timed_clamp_;     //加持杆
+
+    TimedLockData<DoorStatusMap> timed_Door_;     //门(1外内，2外内)
+    std::map<int,TimedLockData<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 +278,7 @@ protected:
     bool cancel_ = false;
 
     int move_mode_ = eSingle;
+    bool is_master_AGV = false; // 是否是主车
     float wheelBase_;   //两节agv的位姿与单节的位姿变换
 
     bool isInSpace_; //是否在车位或者正在进入车位
@@ -244,6 +286,8 @@ protected:
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
 
+    SpaceNo2Region_Carry spaceNo_2_region_carrier_excel_;
+
     NavParameter::Navigation_parameter parameter_;
     RWheelPosition RWheel_position_;
 
@@ -251,6 +295,7 @@ protected:
     double obs_h_;
     TimedLockData<bool> isFront_;
 
+
 };
 
 double limit_gause(double x, double min, double max);
@@ -259,4 +304,5 @@ double limit(double x, double min, double max);
 
 double next_speed(double speed, double target_speed, double acc, double dt = 0.1);
 
+
 #endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_

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,28 @@ 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());
+            ResetLifter((LifterStatus) speed.lifter());
+        }
+    }
+    ResetDoor(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 +188,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 +224,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 +254,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 +277,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,25 +291,40 @@ 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());
     target.set_l(0.02);
-    target.set_w(0.1);
+    target.set_w(0.5);
     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 +354,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 +376,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

@@ -26,15 +26,15 @@ public:
 
     virtual void HandleAgvStatu(const MqttMsg &msg);
 
-    virtual void SwitchMode(int mode, float wheelBase) {
+    virtual bool SwitchMode(int mode, float wheelBase) {
         printf("change mode to:%d\n", mode);
         wheelBase_ = wheelBase;
         if (move_mode_ == mode)
-            return;
+            return true;
         if (mode == eDouble) {
             if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
                 std::cout << "Brother/self pose is timeout can not set MainAGV pose" << std::endl;
-                return;
+                return false;
             }
             //Pose2d transform(-wheelBase_/2.0,0,0);
             //Navigation::ResetPose(pose * transform);
@@ -42,9 +42,9 @@ public:
             Pose2d pose = timedPose_.Get();
             Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
 
-            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.2 || diff.theta() > 5 * M_PI / 180.0) {
+            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.5 || diff.theta() > 5 * M_PI / 180.0) {
                 std::cout << " distance with two agv is too far diff: " << diff << std::endl;
-                return;
+                return false;
             }
 
             Pose2d self_pose = timedPose_.Get();
@@ -64,15 +64,17 @@ public:
         }
 
         move_mode_ = mode;
+        return true;
     };
 
 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 +88,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;

config/SpaceNO2CarrarierNO.txt

@@ -0,0 +1,151 @@
+// 车位id + “ ” + 区域id + “ ” + 区域内载车板编号
+// 区域id < 0 :与载车板无关。绝对值为所在区域
+01      2   1
+03 2 2
+05 2 3
+07 2 4
+09 2 5
+11 2 6
+
+02 5 6
+04 5 5
+06 5 4
+08 5 3
+10 5 2
+12 5 1
+
+13 7 1
+15 7 2
+17 7 3
+19 7 4
+21 7 5
+23 7 6
+
+14 9 9
+16 9 8
+18 9 7
+20 9 6
+22 9 5
+24 9 4
+
+25 6 9
+27 6 8
+29 6 7
+31 6 6
+33 6 5
+35 6 4
+37 6 3
+39 6 2
+41 6 1
+
+26 8 1
+28 8 2
+30 8 3
+32 8 4
+34 8 5
+36 8 6
+38 8 7
+40 8 8
+42 8 9
+
+43 4 1
+45 4 2
+47 4 3
+49 4 4
+51 4 5
+53 4 6
+
+55 1 1
+57 1 2
+59 1 3
+
+44 3 9
+46 3 8
+48 3 7
+50 3 6
+52 3 5
+54 3 4
+56 3 3
+58 3 2
+60 3 1
+
+
+61 -3 9
+62 -3 8
+63 -3 7
+64 -3 6
+65 -3 5
+66 -3 4
+67 -3 3
+68 -3 2
+69 -3 1
+
+
+101 2 1
+103 2 2
+105 2 3
+107 2 4
+109 2 5
+111 2 6
+
+102 5 6
+104 5 5
+106 5 4
+108 5 3
+110 5 2
+112 5 1
+
+113 7 1
+115 7 2
+117 7 3
+119 7 4
+121 7 5
+123 7 6
+
+114 9 9
+116 9 8
+118 9 7
+120 9 6
+122 9 5
+124 9 4
+
+125 6 9
+127 6 8
+129 6 7
+131 6 6
+133 6 5
+135 6 4
+137 6 3
+139 6 2
+141 6 1
+
+126 8 1
+128 8 2
+130 8 3
+132 8 4
+134 8 5
+136 8 6
+138 8 7
+140 8 8
+142 8 9
+
+143 4 1
+145 4 2
+147 4 3
+149 4 4
+151 4 5
+153 4 6
+
+155 1 1
+157 1 2
+159 1 3
+
+144 3 9
+146 3 8
+148 3 7
+150 3 6
+152 3 5
+154 3 4
+156 3 3
+158 3 2
+160 3 1

config/navigation.prototxt

@@ -7,7 +7,7 @@ Agv_emqx
 	ip:"192.168.0.71"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
-	pubClampLifterTopic:"monitor_child/clampLifter"
+	pubClampLifterTopic:"monitor_child/speedcmd"
 	subPoseTopic:"monitor_child/statu"
 	subSpeedTopic:"monitor_child/speed"
 }
@@ -32,32 +32,31 @@ brother_emqx
 
 x_mpc_parameter
 {
-	shortest_radius:26
+	shortest_radius:10
 	dt:0.1
-	acc_velocity:1.5
+	acc_velocity:1
 	acc_angular:10
 }
 y_mpc_parameter
 {
-	shortest_radius:24.5
+	shortest_radius:10
 	dt:0.1
-	acc_velocity:1.5
+	acc_velocity:1
 	acc_angular:10
 }
 
-
 InOutVLimit
 {
     min:0.03
-    max:0.5
+    max:0.3
 }
 NodeVelocityLimit
 {
-    min:0.03
-    max:1.5
+    min:0.05
+    max:0.5
 }
 NodeAngularLimit
 {
     min:0.0523
-    max:0.4
+    max:0.1
 }

config/navigation_main.prototxt

@@ -7,7 +7,7 @@ Agv_emqx
 	ip:"192.168.0.70"
 	port:1883
 	pubSpeedTopic:"monitor_child/speedcmd"
-	pubClampLifterTopic:"monitor_child/clampLifter"
+	pubClampLifterTopic:"monitor_child/speedcmd"
 	subPoseTopic:"monitor_child/statu"
 	subSpeedTopic:"monitor_child/speed"
 }
@@ -32,31 +32,31 @@ brother_emqx
 
 x_mpc_parameter
 {
-	shortest_radius:26
-	dt:0.1
+	shortest_radius:10
+	dt:0.2
 	acc_velocity:1.0
-	acc_angular:10
+	acc_angular:0.44
 }
 y_mpc_parameter
 {
-	shortest_radius:24.5
-	dt:0.1
-	acc_velocity:1.0
-	acc_angular:10
+	shortest_radius:10
+	dt:0.2
+	acc_velocity:1
+	acc_angular:0.1745
 }
 
 InOutVLimit
 {
-    min:0.03
+    min:0.05
     max:0.5
 }
 NodeVelocityLimit
 {
-    min:0.03
-    max:1.5
+    min:0.05
+    max:1.2
 }
 NodeAngularLimit
 {
-    min:0.0523
-    max:0.4
+    min:0.05
+    max:0.35
 }

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; // 门控(1门+状态+2门+状态)
 }
 
 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
@@ -51,7 +54,7 @@ message PathNode //导航路径点及精度
     float l = 3;    //目标点旋转矩形方程，代表精度范围
     float w = 4;
     float theta = 5;
-    string id = 6;
+    optional string id = 6;
 }
 
 message Trajectory

parameter.proto

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