1.自己区分前后车（未测试）

MPC/navigation_main.cpp

@@ -218,23 +218,31 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
     double acc_angular = 25 * M_PI / 180.0;
     double dt = 0.1;
-    Pose2d rotated = Pose2d(space.x(), space.y(), space.theta());
 
+    Pose2d rotated = timedPose_.Get();
     double x = space.x();
     double y = space.y();
-    //后车先到,前车进入2点,保持与后车一致的朝向
-    if (brother.in_space() && brother.space_id() == space.id()) {
-        printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
-        rotated.mutable_theta() = brother.odom().theta();
+    Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
+    rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
+    bool isFront=isFront_.Get();
+    if(rotated.theta()>0.)
+        isFront=!isFront;
+
+    //前车先到,后车进入2点,保持与前车一致的朝向
+    if (isFront==false ) {
+        printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
         if (move_mode_ == eSingle) {
-            x += wheelbase * cos(rotated.theta());
-            y += wheelbase * sin(rotated.theta());
+            x -= wheelbase * cos(rotated.theta());
+            y -= wheelbase * sin(rotated.theta());
+            printf("确定车位点：eSingle\n");
         }
-    } else { //当前车先到,正向
-        printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
+    } else { //当后车先到,倒车入库
+        printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = space.theta();
+        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
 
+
     if (move_mode_ == eDouble){
         x -= wheelbase/2 * cos(rotated.theta());
         y -= wheelbase/2 * sin(rotated.theta());