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

2.调整巡线，旋转速度加速度曲线。
3.添加载车板，门控通信（协议，解析，流程判断）
4.动态调整MPC的N，dt，参考速度，加速度
5.双车出库并行夹持杆提升机构动作
6.添加由车位id解算载车板区域+区域内编号方法。及其对应配置文件

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>
@@ -127,13 +128,201 @@ enum eClamLiftActionType{
 
 //////////////////////////////////////////////////
 /// plc区域+编号载车板 对应 车位表
-/*enum eCarryNo2SpaceNo{
-    eByteCLampClose = 1,
-    eByteClampHalfOpen=2,
-    eByteClampFullyOpen=4,
-    eByteLifterDown=8,
-    eByteLifterUp=16
-};*/
+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<unsigned short,unsigned short > Region_Carry;
+typedef std::map<unsigned short ,Region_Carry> SpaceNo2Region_Carry;
+//class SpacesNo2Region_Carry{
+//public:
+//    SpaceNo2Region_Carry spacesNo2Region_Carry;
+//public:
+//    SpacesNo2Region_Carry(){
+//        spacesNo2Region_Carry[0] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[1] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[2] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[3] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[4] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[5] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[6] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[7] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[8] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[9] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[10] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[11] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[12] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[13] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[14] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[15] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[16] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[17] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[18] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[19] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[20] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[21] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[22] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[23] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[24] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[25] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[26] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[27] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[28] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[29] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[30] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[31] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[32] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[33] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[34] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[35] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[36] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[37] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[38] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[39] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[40] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[41] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[42] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[43] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[44] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[45] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[46] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[47] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[48] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[49] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[50] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[51] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[52] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[53] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[54] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[55] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[56] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[57] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[58] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[59] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[60] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[61] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[62] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[63] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[64] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[65] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[66] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[67] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[68] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[69] = Region_Carry(eRegion0,eCarrier0);
+//
+//        // --------------------------------------------
+//        spacesNo2Region_Carry[100] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[101] = Region_Carry(eRegion2,eCarrier1);
+//        spacesNo2Region_Carry[102] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[103] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[104] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[105] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[106] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[107] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[108] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[109] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[110] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[111] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[112] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[113] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[114] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[115] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[116] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[117] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[118] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[119] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[120] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[121] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[122] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[123] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[124] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[125] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[126] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[127] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[128] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[129] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[130] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[131] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[132] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[133] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[134] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[135] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[136] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[137] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[138] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[139] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[140] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[141] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[142] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[143] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[144] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[145] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[146] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[147] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[148] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[149] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[150] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[151] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[152] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[153] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[154] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[155] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[156] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[157] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[158] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[159] = Region_Carry(eRegion0,eCarrier0);
+//
+//        spacesNo2Region_Carry[160] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[161] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[162] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[163] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[164] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[165] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[166] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[167] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[168] = Region_Carry(eRegion0,eCarrier0);
+//        spacesNo2Region_Carry[169] = Region_Carry(eRegion0,eCarrier0);
+//
+//    }
+//};
+
 //////////////////////////////////////////////////
 
 #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 = 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=20;//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));
@@ -195,15 +216,17 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
 
-    double dt = mpc_param.dt;
+    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= 1.3/(1+exp(-9.*(fabs(line_velocity)-0.5)))+0.3;
+    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 = max_velocity_/(1+exp(-4.*(dis-1)));
+    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)
@@ -319,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;

MPC/navigation.cpp

@@ -150,10 +150,9 @@ void Navigation::HandleAgvStatu(const MqttMsg &msg) {
             ResetStatu(speed.v(), speed.w());
             ResetClamp((ClampStatu) speed.clamp());
             ResetLifter((LifterStatus) speed.lifter());
-
         }
     }
-    ResetInsideDoor(speed.door());
+    ResetDoor(speed.door());
     std::vector<int32_t> vecStatus;
     for(int i=0;i<speed.zcb_size();++i){
         vecStatus.push_back(speed.zcb(i));
@@ -361,6 +360,8 @@ Navigation::Navigation() {
     obs_h_ = 4.0;
     obs_w_ = 2.0;
     timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);
+
+    LoadSpaceNo2CarrierNo();
 }
 
 void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) {
@@ -722,6 +723,10 @@ bool Navigation::execute_nodes(NavMessage::NewAction action, int space_id) {
 Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
     if (move_mode_ != eDouble)
         return eMpcSuccess;
+    int clampLifterAction=0;
+    if(move_mode_==eDouble){
+        clampLifterAction |= eLifterDown;
+    }
     Pose2d init = timedPose_.Get();
     double diff1 = fabs(M_PI / 2.0 - init.theta());
     double diff2 = fabs(-M_PI / 2.0 - init.theta());
@@ -747,12 +752,15 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
         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();
+            if(WaitClampLifterStatu(clampLifterAction)==false){
+                return eMpcFailed;
+            }
             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);
+            SendMoveCmd(clampLifterAction, 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);
@@ -931,6 +939,8 @@ 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 ");
@@ -997,7 +1007,8 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         //去出入口，等内门打开
         if (TargetIsExport(action.spacenode()) || TargetIsEntrance(action.spacenode())) {
             printf(" Wait Inside door opened.......\n");
-            if (WaitInsideDoorOpen(action.spacenode().id()) == false) {
+            int port_id = GetPortIDBySpace(action.spacenode());
+            if (WaitInsideDoorCompleted(port_id, eDoorOpened) == false ) {
                 return false;
             }
             printf(" Wait Inside door opened completed!!!\n");
@@ -1005,16 +1016,23 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             //去车位点
             if (IsUperSpace(action.spacenode())) {
                 printf(" Wait Carrier down.......\n");
-                if (WaitCarrierCompleted(0/*待修改*/, eCarrierDown) == false) {
+                if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierDown) == false) {
                     return false;
                 }
                 printf(" Wait Carrier down completed!!!\n");
             } else {
-                printf(" Wait Carrier up.......\n");
-                if (WaitCarrierCompleted(0/*待修改*/, eCarrierUp) == false) {
-                    return false;
+                if(!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode())) {
+                    printf(" Wait Carrier up.......\n");
+                    if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierUp) == false) {
+                        return false;
+                    }
+                    printf(" Wait Carrier up completed!!!\n");
+                }else{
+                    printf(" Wait inside door open.......\n");
+                    if(WaitInsideDoorCompleted(GetSpaceId(action.spacenode()),eDoorOpened)==false)
+                        return false;
+                    printf("inside door opened\n");
                 }
-                printf(" Wait Carrier up completed!!!\n");
             }
         }
 
@@ -1024,6 +1042,13 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             }
         }
 
+        // 单车进库必须全打开
+        if (move_mode_ == eSingle){
+            if (clamp_fully_open() == false) {
+                return false;
+            }
+        }
+
         //入库
         Pose2d rotated = timedPose_.Get();
         double x = new_target.x();
@@ -1039,7 +1064,9 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             while (true) {
                 if (cancel_) {
                     return false;
-                }
+                }if(move_mode_==eDouble){
+            clampLifterAction |= eLifterDown;
+        }
                 Pose2d current = timedPose_.Get();
                 Pose2d brother_pose = timedBrotherPose_.Get();
                 Pose2d vecCurent2T = current - target;
@@ -1101,12 +1128,9 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
         isInSpace_ = false;
-        if (!lifter_down())
-            return false;
         //摆正
         if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
             return false;
-
     }
     return true;
 }
@@ -1509,39 +1533,42 @@ bool Navigation::lifter_down() {
     return false;
 }
 
-bool Navigation::WaitInsideDoorOpen(std::string doorID) {
-    return true;
+bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) {
     while (cancel_ == false) {
-        if (timed_insideDoor_.timeout()) {
+        if (timed_Door_.timeout()) {
             return false;
         }
-
-        InsideDoorStatu current = timed_insideDoor_.Get();
-        /*
-         *
-         */
-
+        if (timed_Door_.Get()[doorID].second == status) {
+            return true;
+        }
         usleep(1000 * 100);
+        printf("waiting door_id(ins):%d,  to:%d...\n",doorID, status);
     }
     if (cancel_ == true) {
         return false;
     }
+    return false;
 }
 
-bool Navigation::WaitCarrierCompleted(int carrierID, CarrierStatu statu) {
-    return true;
+bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
     while (cancel_ == false) {
-        if (timed_Carrier_.timeout()) {
+        int region_id=-1, carrier_no=-1;
+        SpaceNo2CarrierNo(spaceID, region_id, carrier_no);
+        if (timed_Carrier_[region_id].timeout()) {
             return false;
         }
-//        CarrierStatu current = timed_Carrier_.Get();
-        /*   */
 
+        CarrierStatu cur_ = GetCarrierStatusBySpaceID(spaceID);
+        if ( cur_ == statu){
+            return true;
+        }
         usleep(1000 * 100);
+        printf("waiting space_id:%d, carrier_id[%d,%d] to:%d, cur:%d...\n",spaceID, region_id, carrier_no, statu, cur_);
     }
     if (cancel_ == true) {
         return false;
     }
+    return false;
 }
 
 bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
@@ -1928,7 +1955,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 7) {
-            if (this->clamp_half_open() == false) {
+            if (this->clamp_fully_open() == false) {
                 printf("打开夹持 failed...\n");
                 break;
             }
@@ -2035,57 +2062,169 @@ 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::ResetDoor(int32_t status) {
+//    printf("---------------receive status:%d\n",status);
+    DoorStatusMap doorStatusMap;
+    anasisDoorStatu(status, doorStatusMap);
+
+//    for (auto it = doorStatusMap.begin() ; it != doorStatusMap.end() ; ++it) {
+//        printf("doorID:%d, out:%d, ins:%d\n",it->first, it->second.first, it->second.second);
+//    }
+
+    timed_Door_.reset(doorStatusMap, 3);
+}
+
+void Navigation::anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status) {
+//    int bytesize = sizeof(int32_t)*8;
+    m_door_status.clear();
+    for (int i = 1; i >= 0; --i) {
+        int statu = i_door_status>>(i*16);
+        int door_id = (statu&0xFF00)>>8;
+        int outside_door_status = statu&0x00FF;
+        int inside_door_status = outside_door_status>>2;
+
+        if ((outside_door_status & eDoorOpened) == eDoorOpened){
+            outside_door_status = eDoorOpened;
+        } else if ((outside_door_status & eDoorClosed) == eDoorClosed){
+            outside_door_status = eDoorClosed;
+        } else{
+            outside_door_status = eDoorInvalid;
+        }
+
+        if ((inside_door_status & eDoorOpened) == eDoorOpened){
+            inside_door_status = eDoorOpened;
+        } else if ((inside_door_status & eDoorClosed) == eDoorClosed){
+            inside_door_status = eDoorClosed;
+        } else{
+            inside_door_status = eDoorInvalid;
+        }
+
+        m_door_status[door_id] = DoorStatusInOnePort((DoorStatu)outside_door_status, (DoorStatu)inside_door_status);
     }
 }
 
+#define SWAP_BYTES 0
 void Navigation::anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes){
+    int32_t reverseBytes=bytes;
+    if(SWAP_BYTES){
+        reverseBytes = (bytes>>24)| ((bytes&0xFF0000)>>8)|((bytes&0xFF00)<<8)|((bytes&0xFF)<<24);
+    }
     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);
+        int statu =(reverseBytes>>(i*2) & 0x03);
 
         if ((statu & eCarrierUp) == eCarrierUp) {
-            status[i]=eCarrierUp;
+            status[i+1]=eCarrierUp;
         }
         else if ((statu & eCarrierDown) == eCarrierDown){
-            status[i]=eCarrierDown;
+            status[i+1]=eCarrierDown;
         }
         else {
-            status[i]=eCarrierInvalid;
+            status[i+1]=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_[regionID].reset(region,5);
     }
-    timed_Carrier_.reset(regionStatus);
 
-    printf("---------------------------------------\n");
-    for(std::map<int,CarrierStatuRegionMap>::iterator it=regionStatus.begin();
-        it!=regionStatus.end();it++){
+    /*printf("---------------------------------------\n");
+    for(std::map<int,TimedLockData<CarrierStatuRegionMap>>::iterator it=timed_Carrier_.begin();
+        it!=timed_Carrier_.end();it++){
         int regionID=it->first;
+        if(it->second.timeout())
+            continue;
         printf(">>>>>>> Region ID :%d\n",regionID);
-        for(std::map<int,CarrierStatu>::iterator it1=it->second.begin();
-            it1!=it->second.end();++it1){
+        std::map<int,CarrierStatu> region=it->second.Get();
+        for(std::map<int,CarrierStatu>::iterator it1=region.begin();
+            it1!=region.end();++it1){
             int id=it1->first;
             CarrierStatu sta=it1->second;
             printf("\t \tcarrier id :%d  statu:%d\n",id,sta);
         }
+    }*/
+
+}
+
+void Navigation::Stringsplit(const std::string& str, const std::string splits, std::vector<std::string>& res){
+    res.clear();
+    if(str.empty())
+        return;
+    std::string strs = str+splits;
+    size_t pos = strs.find(splits);
+    int step = splits.size();
+
+    while (pos!= std::string::npos){
+        std::string temp = strs.substr(0, pos);
+        res.push_back(temp);
+        strs = strs.substr(pos+step,strs.size());
+        pos = strs.find(splits);
+    }
+}
+
+bool Navigation::LoadSpaceNo2CarrierNo(){
+    std::string file_name = "../config/SpaceNO2CarrarierNO.txt";
+    std::ifstream in(file_name);
+    std::string line;
+    std::vector<std::string> data_in_one_line;
+    if (in){
+        short space_id, region_id, carrier_no;
+        while(std::getline(in,line)){
+            Stringsplit(line, " ",data_in_one_line);
+            space_id = std::stoi(data_in_one_line[0]);
+            region_id = std::stoi(data_in_one_line[1]);
+            carrier_no = std::stoi(data_in_one_line[2]);
+            spaceNo_2_region_carrier_excel_[space_id] = Region_Carry(region_id,carrier_no);
+        }
+        return true;
+    } else{
+        printf("no SpaceNO2CarrarierNO.txt in config!\n");
     }
+    return false;
+}
 
+bool Navigation::SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no) {
+    Region_Carry region_carry = spaceNo_2_region_carrier_excel_[space_no];
+    region_id = region_carry.first;
+    carrier_no = region_carry.second;
+    return true;
 }
 
+Navigation::CarrierStatu Navigation::GetCarrierStatusBySpaceID(int space_id) {
+    int region_id;
+    int carrier_no;
+    if (SpaceNo2CarrierNo(space_id,region_id,carrier_no) == false){
+        printf("no matched carrier!\n");
+        return eCarrierInvalid;
+    }
+    printf("space_id:%d, region_id:%d, carrier_no:%d\n",space_id,region_id,carrier_no);
+    if (timed_Carrier_[region_id].timeout()){
+        printf("carrier status is timeout!\n");
+        return eCarrierInvalid;
+    }
+    else{
+        return timed_Carrier_[region_id].Get()[carrier_no];
+    }
+    return eCarrierInvalid;
+}
+
+int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
+    int port_id = GetSpaceId(node);
+    if (port_id == 1101){
+        return 1;
+    }
+    else if (port_id == 1102){
+        return 2;
+    }
+}
+
+
+
 

MPC/navigation.h

@@ -45,19 +45,20 @@ public:
 
     typedef std::map<int,CarrierStatu> CarrierStatuRegionMap;
 
-    enum InsideDoorStatu{
-        eInsideDoorInvalid=0,
-        eInsideDoorOpened=1,
-        eInsideDoorClosed=2
+    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();
 
@@ -66,6 +67,16 @@ 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);
+
+    void 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);
@@ -76,7 +87,7 @@ public:
 
     virtual void ResetLifter(LifterStatus status);
 
-    virtual void ResetInsideDoor(int32_t status);
+    virtual void ResetDoor(int32_t status);
 
     virtual void ResetCarrier(std::vector<int32_t> status);
 
@@ -107,12 +118,14 @@ public:
 
     bool PoseTimeout();
     bool WaitClampLifterStatu(int clampLifterAction);
-    bool WaitInsideDoorOpen(std::string doorID);
-    bool WaitCarrierCompleted(int carrierID,CarrierStatu statu);
+    bool WaitInsideDoorCompleted(int doorID, DoorStatu status);
+    bool WaitCarrierCompleted(int spaceID,CarrierStatu statu);
 
     bool TargetIsEntrance(NavMessage::PathNode node);
     bool TargetIsExport(NavMessage::PathNode node);
 
+    int GetPortIDBySpace(NavMessage::PathNode node);
+
 
 protected:
     static void anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes);
@@ -223,6 +236,8 @@ protected:
 
     virtual void publish_statu(NavMessage::NavStatu &statu);
 
+
+
 protected:
     std::mutex mtx_;
     bool exit_ = false;
@@ -239,8 +254,8 @@ protected:
     TimedLockData<double> timedV_;              //底盘数据
     TimedLockData<double> timedA_;
 
-    TimedLockData<InsideDoorStatu> timed_insideDoor_;     //内门
-    TimedLockData<std::map<int,CarrierStatuRegionMap>> timed_Carrier_;     //载车板
+    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_;
@@ -267,6 +282,8 @@ protected:
     TimedLockData<Trajectory> selected_traj_;
     TimedLockData<Trajectory> predict_traj_;
 
+    SpaceNo2Region_Carry spaceNo_2_region_carrier_excel_;
+
     NavParameter::Navigation_parameter parameter_;
     RWheelPosition RWheel_position_;
 
@@ -283,4 +300,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

@@ -117,20 +117,16 @@ void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
             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());
+    ResetDoor(speed.door());
     std::vector<int32_t> vecStatus;
     for(int i=0;i<speed.zcb_size();++i){
         vecStatus.push_back(speed.zcb(i));

config/SpaceNO2CarrarierNO.txt

@@ -0,0 +1,68 @@
+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
 }

message.proto

@@ -18,7 +18,7 @@ message AgvStatu {
     int32 lifter = 5;   //提升机构 0中间状态，1上升到位， 2下降到位
     int32 lifter_other = 6;     //另外一节
     repeated int32 zcb = 7; //载车板
-    int32 door = 8; // 门控
+    int32 door = 8; // 门控(1门+状态+2门+状态)
 }
 
 message ToAgvCmd {