1.取消取车流程，送车到出口前旋转180动作，添加存车流程离开入口，掉头

2.同步整车判断两车错位条件

MPC/navigation.cpp

@@ -778,6 +778,49 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
     return eMpcFailed;
 }
 
+Navigation::MpcResult Navigation::RotateAfterOutExport() {
+    if (move_mode_ != eDouble)
+        return eMpcSuccess;
+    Pose2d init = timedPose_.Get();
+    double diff1 = fabs(M_PI / 2.0 - init.theta());
+    double diff2 = fabs(-M_PI / 2.0 - init.theta());
+    double target = diff1 > diff2 ? M_PI / 2.0 : -M_PI / 2.0;
+    stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
+    while (cancel_ == false) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(80));
+        Pose2d current = timedPose_.Get();
+        double yawDiff = (target - current.theta());
+
+        //一次变速
+        std::vector<double> out;
+        bool ret;
+        TimerRecord::Execute([&, this]() {
+            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
+        }, "Rotation_mpc_once");
+        if (ret == false) {
+            Stop();
+            return eMpcFailed;
+        }
+
+        //下发速度
+        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+            printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
+            Stop();
+            return eMpcSuccess;
+        } else {
+            const int down_count = 3;
+            double v[down_count] = {0, 0, 0};
+            double w[down_count] = {out[0], out[1], out[2]};
+            SendMoveCmd(0, move_mode_, eRotation, v, w);
+            actionType_ = eRotation;
+//            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+//                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
+        }
+        continue;
+    }
+    return eMpcFailed;
+}
+
 
 Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
     if (move_mode_ != eDouble)
@@ -999,14 +1042,14 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf("inout ----------------------------------------\n");
 
         //取车流程，送车到出口前旋转180
-        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) {
-                printf("Rotate before in entrance failed\n");
-                return false;
-            }
-        }
+//        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) {
+//                printf("Rotate before in entrance failed\n");
+//                return false;
+//            }
+//        }
 
         //计算车位朝向
         action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
@@ -1158,6 +1201,19 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
             return false;
         }
         isInSpace_ = false;
+
+        // 存车流程离开入口，掉头
+        if (GetRemainActionsCount() > 1) {
+            if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
+                Pose2d current_pose = timedPose_.Get();
+                double current_theta = current_pose.theta();
+                if (RotateAfterOutExport() != eMpcSuccess) {
+                    printf("Rotate before in entrance failed\n");
+                    return false;
+                }
+            }
+        }
+
         //摆正
         if (GetRemainActionsCount() > 1) {
             if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)

MPC/navigation.h

@@ -199,6 +199,8 @@ protected:
 
     virtual Navigation::MpcResult RotateBeforeIntoExport();
 
+    virtual Navigation::MpcResult RotateAfterOutExport();
+
     /// 双车出库摆正
     virtual Navigation::MpcResult DoubleOutSpaceCorrectTheta();
 

MPC/navigation_main.cpp

@@ -27,7 +27,7 @@ void NavigationMain::ResetPose(const Pose2d &pose) {
         Pose2d brother = timedBrotherPose_.Get();
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
 
-        if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.3 || diff.theta() > 5 * M_PI / 180.0) {
+        if (diff.x() > 3.6 || diff.x() < 2.2 || 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;
         }

MPC/navigation_main.h

@@ -44,7 +44,7 @@ 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.5 || diff.theta() > 5 * M_PI / 180.0) {
+            if (diff.x() > 3.6 || diff.x() < 2.2 || 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 false;
             }