1.进出库（包括出入口），实时判断载车板或者门控信号

MPC/navigation.cpp

@@ -616,7 +616,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
         Pose2d target(node.x(), node.y(), 0);
         Pose2d targetInCurrent = Pose2d::relativePose(target, current);
 
-        if (GetCurrentAction().type() != 1 && GetCurrentAction().type() != 2){
+        if (GetCurrentAction().type() != 1 && GetCurrentAction().type() != 2) {
             if (enableTotarget == false) {
                 int directions = Direction::eForward;
                 if (anyDirect)
@@ -1748,9 +1748,37 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             ofs.close();//关闭文件
             return eMpcFailed;
         }
-        //出入库实时判断门控或者载车板
-        if(GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2){
 
+        //出入库实时判断门控或者载车板
+        NavMessage::NewAction current_action = GetCurrentAction();
+        if (current_action.type() == 1 || current_action.type() == 2) {
+            //去出入口，判断内门是否打开
+            if (TargetIsExport(current_action.spacenode()) || TargetIsEntrance(current_action.spacenode())) {
+                int port_id = GetPortIDBySpace(current_action.spacenode());
+                if (timed_Door_.timeout() || timed_Door_.Get()[port_id].second != eDoorOpened) {
+                    printf(" Inside door is not opened.......| line: %d\n", __LINE__);
+                    return eMpcFailed;
+                }
+            } else {
+                //去车位点
+                if (IsUperSpace(current_action.spacenode())) {
+                    if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierDown) {
+                        printf(" Carrier is not downed.......| line: %d\n", __LINE__);
+                        return eMpcFailed;
+                    }
+                } else {
+                    int region_id = 0, carrier_no = 0;
+                    SpaceNo2CarrierNo(GetSpaceId(current_action.spacenode()), region_id, carrier_no);
+                    if (region_id < 0) {// 子母车位，且不在载车板下方的车位
+
+                    } else {
+                        if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierUp) {
+                            printf(" Carrier is not up.......| line: %d\n", __LINE__);
+                            return eMpcFailed;
+                        }
+                    }
+                }
+            }
         }
 
         //判断是否到达终点
@@ -1824,7 +1852,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         //添加车位号,距目标点距离信息
         double distance = 0;
         int space_id = -1;
-        if (GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2){
+        if (GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2) {
             space_id = GetSpaceId(GetCurrentAction().spacenode());
         }
         if (space_id > 0) {