1.添加MPC计算失败重试，连续三次计算失败才结束MPC Move。

2.出入库时无法达到目标点，禁止任意方向调整（AdjustReferToTarget）
3.出库时，雷达反馈当前位置与车位点相对位置限制改为（x相对0.15m，y0.1m）
4.出库时摆正动作优化为，如果是最后一个动作，不做摆正动作
5.mpc_once,巡线最大角速度固定0.1745
6.入库最后0.8m不纠偏，出库最开始1.4m不纠偏
7.出入库时才下发车位id和据目标距离信息
8.添加获取当前action函数，剩余动作数量函数

MPC/loaded_mpc.cpp

@@ -172,7 +172,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double wmg = statu[4];
 
 
-    size_t N=18;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
+    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;

MPC/navigation.cpp

@@ -154,7 +154,7 @@ void Navigation::HandleAgvStatu(const MqttMsg &msg) {
     }
     ResetDoor(speed.door());
     std::vector<int32_t> vecStatus;
-    for(int i=0;i<speed.zcb_size();++i){
+    for (int i = 0; i < speed.zcb_size(); ++i) {
         vecStatus.push_back(speed.zcb(i));
     }
     ResetCarrier(vecStatus);
@@ -350,7 +350,7 @@ bool Navigation::SlowlyStop() {
 
 
 Navigation::Navigation() {
-    is_master_AGV=false;
+    is_master_AGV = false;
     wheelBase_ = 0;
     isInSpace_ = false; //是否在车位或者正在进入车位
     RWheel_position_ = eUnknow;
@@ -450,7 +450,7 @@ bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate,
 }
 */
 
-bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate,int directions) {
+bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate, int directions) {
     std::cout << "Adjust to : " << target << std::endl;
     if (inited_ == false) {
         printf(" MPC_rotate has not inited\n");
@@ -592,6 +592,8 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         return true;
     }
     bool already_in_mpc = false;
+    int retry_count = 0;
+    int retry_count_limit = 3;
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         if (PoseTimeout()) {
@@ -614,13 +616,16 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         Pose2d target(node.x(), node.y(), 0);
         Pose2d targetInCurrent = Pose2d::relativePose(target, current);
 
-        if (enableTotarget == false) {
-            int directions = Direction::eForward;
-            if(anyDirect)
-                 directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
-            bool adjustRet = AdjustReferToTarget(target, limit_w,directions);
-            if (adjustRet == false)
-                return false;
+        if (GetCurrentAction().type() != 1 && GetCurrentAction().type() != 2){
+            if (enableTotarget == false) {
+                int directions = Direction::eForward;
+                if (anyDirect)
+                    directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+                bool adjustRet = AdjustReferToTarget(target, limit_w, directions);
+                if (adjustRet == false)
+                    return false;
+            }
+
         }
 
         already_in_mpc = true;
@@ -633,7 +638,9 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
             printf(" MPC to target:%f %f l:%f,w:%f theta:%f  impossible ,retry ...\n",
                    node.x(), node.y(), node.l(), node.w(), node.theta());
         } else {
-            return false;
+            retry_count++;
+            if (retry_count > retry_count_limit)
+                return false;
         }
     }
     if (cancel_) {
@@ -659,13 +666,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         int clampLifterAction = 0;
         if (move_mode_ == eSingle) {
             clampLifterAction |= eByteClampHalfOpen;
-        }else{
+        } else {
             clampLifterAction |= eByteLifterDown;
         }
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
             NavMessage::PathNode node = action.pathnodes(i);
-            if (i  < action.pathnodes_size()-1) {
+            if (i < action.pathnodes_size() - 1) {
                 NavMessage::PathNode next = action.pathnodes(i + 1);
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
                 node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
@@ -728,8 +735,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
 Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
     if (move_mode_ != eDouble)
         return eMpcSuccess;
-    int clampLifterAction=0;
-    if(move_mode_==eDouble){
+    int clampLifterAction = 0;
+    if (move_mode_ == eDouble) {
         clampLifterAction |= eLifterDown;
     }
     Pose2d init = timedPose_.Get();
@@ -757,7 +764,7 @@ 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){
+            if (WaitClampLifterStatu(clampLifterAction) == false) {
                 return eMpcFailed;
             }
             return eMpcSuccess;
@@ -917,7 +924,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 
 bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
     int space_id = GetSpaceId(spaceNode);
-    if ( space_id < 0)
+    if (space_id < 0)
         return false;
     if (space_id > 99 && space_id < 1000) {
         return true;
@@ -926,11 +933,11 @@ bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
 }
 
 int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
-    if(spaceNode.has_id()==false){
+    if (spaceNode.has_id() == false) {
         return -1;
     }
     std::string space_flag = "S_";
-    if (spaceNode.id().find(space_flag) >= 0) {
+    if (spaceNode.id().find(space_flag) >= 0 && spaceNode.id().size() > 2) {
         std::string id = spaceNode.id().substr(space_flag.size(), spaceNode.id().length() - 1);
         int space_id = stoi(id);
         return space_id;
@@ -940,16 +947,15 @@ int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
 
 bool Navigation::TargetIsEntrance(NavMessage::PathNode node) {
     int space_id = GetSpaceId(node);
-    return space_id==1101 || space_id==1102;
+    return space_id == 1101 || space_id == 1102;
 }
 
 bool Navigation::TargetIsExport(NavMessage::PathNode node) {
     int space_id = GetSpaceId(node);
-    return space_id==1101 || space_id==1102;
+    return space_id == 1101 || space_id == 1102;
 }
 
 
-
 bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     printf("execute_InOut_space\n");
     if (action.type() != 1 && action.type() != 2) {
@@ -988,7 +994,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampHalfOpen;
-        
+
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -997,7 +1003,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf("inout ----------------------------------------\n");
 
         //取车流程，送车到出口前旋转180
-        if (move_mode_ == eDouble && TargetIsExport(action.spacenode())) {
+        if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
             Pose2d current_pose = timedPose_.Get();
             double current_theta = current_pose.theta();
             if (RotateBeforeIntoExport() != eMpcSuccess) {
@@ -1020,7 +1026,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (TargetIsExport(action.spacenode()) || TargetIsEntrance(action.spacenode())) {
             printf(" Wait Inside door opened.......\n");
             int port_id = GetPortIDBySpace(action.spacenode());
-            if (WaitInsideDoorCompleted(port_id, eDoorOpened) == false ) {
+            if (WaitInsideDoorCompleted(port_id, eDoorOpened) == false) {
                 return false;
             }
             printf(" Wait Inside door opened completed!!!\n");
@@ -1033,16 +1039,16 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
                 }
                 printf(" Wait Carrier down completed!!!\n");
             } else {
-                if(!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode())) {
+                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{
+                } else {
                     printf(" Wait inside door open.......\n");
-                    if(WaitInsideDoorCompleted(GetSpaceId(action.spacenode()),eDoorOpened)==false)
+                    if (WaitInsideDoorCompleted(GetSpaceId(action.spacenode()), eDoorOpened) == false)
                         return false;
                     printf("inside door opened\n");
                 }
@@ -1050,14 +1056,14 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
 
         if (IsUperSpace(action.spacenode())) {
-            if(lifter_rise()== false){
+            if (lifter_rise() == false) {
                 return false;
             }
         }
 
-        // 单车进库必须全打开
-        if (move_mode_ == eSingle){
-            if (clamp_fully_open() == false) {
+        // 单车进库必须全打开(先半开，行走时全开)
+        if (move_mode_ == eSingle) {
+            if (clamp_half_open() == false) {
                 return false;
             }
         }
@@ -1077,9 +1083,10 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             while (true) {
                 if (cancel_) {
                     return false;
-                }if(move_mode_==eDouble){
-            clampLifterAction |= eLifterDown;
-        }
+                }
+                if (move_mode_ == eDouble) {
+                    clampLifterAction |= eLifterDown;
+                }
                 Pose2d current = timedPose_.Get();
                 Pose2d brother_pose = timedBrotherPose_.Get();
                 Pose2d vecCurent2T = current - target;
@@ -1109,11 +1116,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             clampLifterAction |= eByteCLampClose;
 
         printf(" clampLifter_statu2 :%d\n", clampLifterAction);
-        if(!TargetIsExport(action.spacenode())&& !TargetIsEntrance(action.spacenode()))
-            limit_w={0.01,0.05}; //进库纠偏角速度限制
-        else{
+        if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode()))
+            limit_w = {0.01, 0.05}; //进库纠偏角速度限制
+        else {
             // 整车进 出入口速度调整为与巡线一致
-            if (move_mode_==eDouble){
+            if (move_mode_ == eDouble) {
                 limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
                 limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
             }
@@ -1135,11 +1142,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         Pose2d current = timedPose_.Get();
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
         // 整车从出入口出库速度调整为与巡线一致
-        if (move_mode_==eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))){
+        if (move_mode_ == eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))) {
             limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
             limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
         }
-        if (diff.x() < 0.05 || diff.y() < 0.10) {
+        if (fabs(diff.x()) < 0.15 || fabs(diff.y()) < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
             action.mutable_streetnode()->set_theta(theta);
             action.mutable_streetnode()->set_l(0.03);
@@ -1156,8 +1163,10 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         }
         isInSpace_ = false;
         //摆正
-        if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
-            return false;
+        if (GetRemainActionsCount() > 1) {
+            if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
+                return false;
+        }
     }
     return true;
 }
@@ -1280,8 +1289,9 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
 
     MpcError ret = failed;
     LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max);
+    // 巡线最大角速度固定0.1745
     LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
-                                          mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
+                                          mpc_parameter.acc_velocity(), 0.1745};
     //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
     //       mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
     ret = MPC.solve(traj, traj[traj.size() - 1], statu, parameter,
@@ -1568,7 +1578,7 @@ bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) {
             return true;
         }
         usleep(1000 * 100);
-        printf("waiting door_id(ins):%d,  to:%d...\n",doorID, status);
+        printf("waiting door_id(ins):%d,  to:%d...\n", doorID, status);
     }
     if (cancel_ == true) {
         return false;
@@ -1578,22 +1588,23 @@ bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) {
 
 bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
     while (cancel_ == false) {
-        int region_id=0, carrier_no=0;
+        int region_id = 0, carrier_no = 0;
         SpaceNo2CarrierNo(spaceID, region_id, carrier_no);
-        if(region_id<0){
+        if (region_id < 0) {
             return true;
         }
         if (timed_Carrier_[region_id].timeout()) {
-            printf("spaceID:%d, timed_Carrier_[%d].timeout  carrier_no id:%d\n",spaceID,region_id,carrier_no);
+            printf("spaceID:%d, timed_Carrier_[%d].timeout  carrier_no id:%d\n", spaceID, region_id, carrier_no);
             return false;
         }
 
         CarrierStatu cur_ = GetCarrierStatusBySpaceID(spaceID);
-        if ( cur_ == statu){
+        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_);
+        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;
@@ -1603,7 +1614,7 @@ bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
 
 bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
     int currentStatu = 0;
-    if (move_mode_ == eSingle){
+    if (move_mode_ == eSingle) {
         while (cancel_ == false) {
             if (timed_clamp_.timeout() || timed_lifter_.timeout()) {
                 return false;
@@ -1624,8 +1635,8 @@ bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
                 currentStatu |= eByteLifterDown;
             }
             printf(" current statu:%d   action status:%d  last:%d\n",
-                   currentStatu,clampLifterAction,currentStatu&clampLifterAction);
-            if((currentStatu&clampLifterAction) == clampLifterAction)
+                   currentStatu, clampLifterAction, currentStatu & clampLifterAction);
+            if ((currentStatu & clampLifterAction) == clampLifterAction)
                 return true;
 
             double down_v[3] = {0, 0, 0};//下发线速度0
@@ -1634,11 +1645,11 @@ bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
             usleep(1000 * 100);
         }
 
-    } else if (move_mode_ == eDouble){
+    } else if (move_mode_ == eDouble) {
 
         while (cancel_ == false) {
             if (timed_clamp_.timeout() || timed_lifter_.timeout() ||
-                    timed_other_clamp_.timeout() || timed_other_lifter_.timeout()) {
+                timed_other_clamp_.timeout() || timed_other_lifter_.timeout()) {
                 return false;
             }
             if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
@@ -1657,8 +1668,8 @@ bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
                 currentStatu |= eByteLifterDown;
             }
             printf(" current statu:%d   action status:%d  last:%d\n",
-                   currentStatu,clampLifterAction,currentStatu&clampLifterAction);
-            if((currentStatu&clampLifterAction) == clampLifterAction)
+                   currentStatu, clampLifterAction, currentStatu & clampLifterAction);
+            if ((currentStatu & clampLifterAction) == clampLifterAction)
                 return true;
 
             double down_v[3] = {0, 0, 0};//下发线速度0
@@ -1737,6 +1748,11 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             ofs.close();//关闭文件
             return eMpcFailed;
         }
+        //出入库实时判断门控或者载车板
+        if(GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2){
+
+        }
+
         //判断是否到达终点
         if (IsArrived(node)) {
             if (Stop()) {
@@ -1771,19 +1787,19 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             target_in_agv = target_in_agv.rotate(M_PI / 2.0);
         }
 
-            for (int i = 0; i < down_count; ++i) {
-                if (down_v[i] >= 0 && down_v[i] < limit_v.min)
-                    down_v[i] = limit_v.min;
-                if (down_v[i] < 0 && down_v[i] > -limit_v.min)
-                    down_v[i] = -limit_v.min;
-            }
+        for (int i = 0; i < down_count; ++i) {
+            if (down_v[i] >= 0 && down_v[i] < limit_v.min)
+                down_v[i] = limit_v.min;
+            if (down_v[i] < 0 && down_v[i] > -limit_v.min)
+                down_v[i] = -limit_v.min;
+        }
 
         //放大角速度
 //        out[1] *= 10;
 
         //入库，行进方向上最后一米不纠偏
         if (newUnfinished_cations_.front().type() == 1 && GetSpaceId(node) > 0) {//入库
-            if (fabs(target_in_agv.x()) < 0.5) {
+            if (fabs(target_in_agv.x()) < 0.8) {
                 printf("target_in_agv: %f", target_in_agv.x());
                 for (double &i: down_w) {
                     i = 0.0;
@@ -1795,7 +1811,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
                             Pose2d(newUnfinished_cations_.front().spacenode().x(),
                                    newUnfinished_cations_.front().spacenode().y(), 0),
                             timedPose_.Get());
-            if (fabs(agv_in_space.x()) < 0.3) {
+            if (fabs(agv_in_space.x()) < 1.4) {
                 printf("agv_in_space: %f", agv_in_space.x());
                 for (double &i: down_w) {
                     i = 0.0;
@@ -1806,8 +1822,14 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         //下发速度
         //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         //添加车位号,距目标点距离信息
-        double distance = Pose2d::distance(target_in_agv, Pose2d(0, 0, 0));
-        int space_id = GetSpaceId(node);
+        double distance = 0;
+        int space_id = -1;
+        if (GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2){
+            space_id = GetSpaceId(GetCurrentAction().spacenode());
+        }
+        if (space_id > 0) {
+            distance = Pose2d::distance(target_in_agv, Pose2d(0, 0, 0));
+        }
         double Y1 = 0.0, Y2 = 0.0;
         if (move_mode_ == eDouble) {
             double dy = -timeYawDiff1_.Get();
@@ -1972,7 +1994,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     running_ = true;
     actionType_ = eReady;
     printf("Navigation beg...\n");
-    bool ret=false;
+    bool ret = false;
     while (newUnfinished_cations_.empty() == false && cancel_ == false) {
         std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
         NavMessage::NewAction act = newUnfinished_cations_.front();
@@ -1982,7 +2004,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             space_id_ = act.spacenode().id();
             obs_w_ = 1.1;
             obs_h_ = 1.87;
-            ret=execute_InOut_space(act);
+            ret = execute_InOut_space(act);
             if (!ret) {
                 printf(" In space failed\n");
                 isInSpace_ = false;
@@ -1992,7 +2014,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             printf("出库\n");
             obs_w_ = 1.25;
             obs_h_ = 1.87;
-            ret=execute_InOut_space(act);
+            ret = execute_InOut_space(act);
             if (!ret) {
                 printf(" out space failed\n");
                 break;
@@ -2003,7 +2025,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             printf("马路导航\n");
             obs_w_ = 1.1;
             obs_h_ = 1.87;
-            ret=execute_nodes(act);
+            ret = execute_nodes(act);
             if (!ret) {
                 printf(" street nav failed\n");
                 break;
@@ -2012,8 +2034,8 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             printf(" 汽车模型导航....\n");
 
         } else if (act.type() == 6) {//夹持
-            ret=clamp_close();
-            if (ret== false) {
+            ret = clamp_close();
+            if (ret == false) {
                 printf("夹持failed ...\n");
                 break;
             }
@@ -2024,15 +2046,15 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
             }
         } else if (act.type() == 8) { //切换模式
             SwitchMode(act.changedmode(), act.wheelbase());
-            ret=true;
+            ret = true;
         } else if (act.type() == 9) { //提升机构提升
-            ret=lifter_rise();
+            ret = lifter_rise();
             if (ret == false) {
                 printf("提升failed ...\n");
                 break;
             }
         } else if (act.type() == 10) { //提升机构下降
-            ret=lifter_down();
+            ret = lifter_down();
             if (ret == false) {
                 printf("下降failed ...\n");
                 break;
@@ -2045,14 +2067,13 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     }
     actionType_ = eReady;
     Stop();
-    if(ret==false){
+    if (ret == false) {
         response.set_ret(-4);
         response.set_info("navigation ret false");
         printf(" navigation ret false\n");
         while (newUnfinished_cations_.empty() == false)
             newUnfinished_cations_.pop();
-    }
-    else if (cancel_ == true) {
+    } else if (cancel_ == true) {
         response.set_ret(-3);
         response.set_info("navigation canceled");
         printf(" navigation canceled\n");
@@ -2148,65 +2169,64 @@ void Navigation::ResetDoor(int32_t status) {
     timed_Door_.reset(doorStatusMap, 3);
 }
 
-void Navigation::anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status) {
+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;
+        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){
+        if ((outside_door_status & eDoorOpened) == eDoorOpened) {
             outside_door_status = eDoorOpened;
-        } else if ((outside_door_status & eDoorClosed) == eDoorClosed){
+        } else if ((outside_door_status & eDoorClosed) == eDoorClosed) {
             outside_door_status = eDoorClosed;
-        } else{
+        } else {
             outside_door_status = eDoorInvalid;
         }
 
-        if ((inside_door_status & eDoorOpened) == eDoorOpened){
+        if ((inside_door_status & eDoorOpened) == eDoorOpened) {
             inside_door_status = eDoorOpened;
-        } else if ((inside_door_status & eDoorClosed) == eDoorClosed){
+        } else if ((inside_door_status & eDoorClosed) == eDoorClosed) {
             inside_door_status = eDoorClosed;
-        } else{
+        } else {
             inside_door_status = eDoorInvalid;
         }
 
-        m_door_status[door_id] = DoorStatusInOnePort((DoorStatu)outside_door_status, (DoorStatu)inside_door_status);
+        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);
+
+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 =(reverseBytes>>(i*2) & 0x03);
+    int bytesize = sizeof(int32_t) * 8;
+    regionID = (bytes >> ((bytesize - 1) * 8));
+    for (int i = 0; i < (bytesize - 8) / 2; ++i) {
+        int statu = (reverseBytes >> (i * 2) & 0x03);
 
         if ((statu & eCarrierUp) == eCarrierUp) {
-            status[i+1]=eCarrierUp;
-        }
-        else if ((statu & eCarrierDown) == eCarrierDown){
-            status[i+1]=eCarrierDown;
-        }
-        else {
-            status[i+1]=eCarrierInvalid;
+            status[i + 1] = eCarrierUp;
+        } else if ((statu & eCarrierDown) == eCarrierDown) {
+            status[i + 1] = eCarrierDown;
+        } else {
+            status[i + 1] = eCarrierInvalid;
         }
     }
 }
 
 void Navigation::ResetCarrier(std::vector<int32_t> status) {
-    for(int32_t statu : status){
+    for (int32_t statu: status) {
         int regionID;
         CarrierStatuRegionMap region;
-        anasisCarierStatu(regionID,region,statu);
+        anasisCarierStatu(regionID, region, statu);
 
-        timed_Carrier_[regionID].reset(region,5);
+        timed_Carrier_[regionID].reset(region, 5);
     }
 
     /*printf("---------------------------------------\n");
@@ -2227,20 +2247,20 @@ void Navigation::ResetCarrier(std::vector<int32_t> status) {
 
 }
 
-bool Navigation::Stringsplit(const std::string& str, const std::string splits, std::vector<std::string>& res){
+bool Navigation::Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res) {
     res.clear();
-    if(str.empty()){
+    if (str.empty()) {
         return false;
     }
-    std::string strs = str+splits;
+    std::string strs = str + splits;
     size_t pos = strs.find(splits);
     int step = splits.size();
 
-    while (pos!= std::string::npos){
+    while (pos != std::string::npos) {
         std::string temp = strs.substr(0, pos);
         if (temp.size() > 0)
             res.push_back(temp);
-        strs = strs.substr(pos+step,strs.size());
+        strs = strs.substr(pos + step, strs.size());
         pos = strs.find(splits);
     }
 //    for (int i = 0; i < res.size(); ++i) {
@@ -2249,17 +2269,17 @@ bool Navigation::Stringsplit(const std::string& str, const std::string splits, s
     return true;
 }
 
-bool Navigation::LoadSpaceNo2CarrierNo(){
+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){
+    if (in) {
         int space_id, region_id, carrier_no;
-        while(std::getline(in,line)){
-            if (!Stringsplit(line, " ",data_in_one_line) || line.find("//") != std::string::npos)
+        while (std::getline(in, line)) {
+            if (!Stringsplit(line, " ", data_in_one_line) || line.find("//") != std::string::npos)
                 continue;
-            if (data_in_one_line.size()==3) {
+            if (data_in_one_line.size() == 3) {
                 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]);
@@ -2267,17 +2287,17 @@ bool Navigation::LoadSpaceNo2CarrierNo(){
             }
         }
         //打印
-        for (auto & i : spaceNo_2_region_carrier_excel_) {
-            printf("space_id: %d, region_id: %d, carrier_no: %d\n",i.first,i.second.first,i.second.second);
+        for (auto &i: spaceNo_2_region_carrier_excel_) {
+            printf("space_id: %d, region_id: %d, carrier_no: %d\n", i.first, i.second.first, i.second.second);
         }
         return true;
-    } else{
+    } else {
         printf("no SpaceNO2CarrarierNO.txt in config!\n");
     }
     return false;
 }
 
-bool Navigation::SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no) {
+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;
@@ -2287,16 +2307,15 @@ bool Navigation::SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no
 Navigation::CarrierStatu Navigation::GetCarrierStatusBySpaceID(int space_id) {
     int region_id;
     int carrier_no;
-    if (SpaceNo2CarrierNo(space_id,region_id,carrier_no) == false){
+    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("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{
+    } else {
         return timed_Carrier_[region_id].Get()[carrier_no];
     }
     return eCarrierInvalid;
@@ -2304,16 +2323,22 @@ Navigation::CarrierStatu Navigation::GetCarrierStatusBySpaceID(int space_id) {
 
 int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
     int port_id = GetSpaceId(node);
-    if (port_id == 1101){
+    if (port_id == 1101) {
         return 1;
-    }
-    else if (port_id == 1102){
+    } else if (port_id == 1102) {
         return 2;
     }
 
     return -1;
 }
 
+int Navigation::GetRemainActionsCount() {
+    return newUnfinished_cations_.size();
+}
+
+NavMessage::NewAction Navigation::GetCurrentAction() {
+    return newUnfinished_cations_.front();
+}
 
 
 

MPC/navigation.h

@@ -126,6 +126,9 @@ public:
 
     int GetPortIDBySpace(NavMessage::PathNode node);
 
+    // 获取当前未完成动作数量
+    virtual int GetRemainActionsCount();
+    virtual NavMessage::NewAction GetCurrentAction();
 
 protected:
     static void anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes);
@@ -196,6 +199,7 @@ protected:
 
     virtual Navigation::MpcResult RotateBeforeIntoExport();
 
+    /// 双车出库摆正
     virtual Navigation::MpcResult DoubleOutSpaceCorrectTheta();
 
     virtual bool clamp_close();