1.自己区分前后车（未测试），并调整角度

MPC/navigation.cpp

@@ -714,7 +714,8 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
     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.)
+    printf(" front__:%d  theta:%f\n",isFront,rotated.theta());
+    if(rotated.theta()<0.)
         isFront=!isFront;
 
     //前车先到,后车进入2点,保持与前车一致的朝向
@@ -727,9 +728,12 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
         }
     } else { //当后车先到,倒车入库
         printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
-        rotated.mutable_theta() = space.theta();
-        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
+        //rotated.mutable_theta() = space.theta();
+        //rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
+    if(rotated.theta()<0.)
+        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
+
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
@@ -809,7 +813,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
 
         Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
                    0);
-        action.mutable_streetnode()->set_l(0.05);
+        action.mutable_streetnode()->set_l(0.02);
         action.mutable_streetnode()->set_w(0.05);
         action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
         /*
@@ -850,7 +854,15 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
 
         }
         //入库
-        if(isFront_.Get()==false && move_mode_==eSingle){
+        Pose2d rotated = timedPose_.Get();
+        double x = new_target.x();
+        double y = new_target.y();
+        Pose2d vecSpace(x - rotated.x(), y - rotated.y(), 0);
+        rotated.mutable_theta() = Pose2d::vector2yaw(vecSpace.x(), vecSpace.y());
+        bool isFront=isFront_.Get();
+        if(rotated.theta()<0.)
+            isFront=!isFront;
+        if(isFront==false && move_mode_==eSingle){
             while(timedBrotherNavStatu_.Get().in_space()==false){
                 if(cancel_){
                     return false;

MPC/navigation_main.cpp

@@ -225,7 +225,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     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.)
+    if(rotated.theta()<0.)
         isFront=!isFront;
 
     //前车先到,后车进入2点,保持与前车一致的朝向
@@ -238,8 +238,8 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
     } else { //当后车先到,倒车入库
         printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
-        rotated.mutable_theta() = space.theta();
-        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
+        //rotated.mutable_theta() = space.theta();
+
     }
 
 
@@ -248,6 +248,8 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         y -= wheelbase/2 * sin(rotated.theta());
         printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
     }
+    if(rotated.theta()<0.)
+        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
 
     target.set_x(x);
     target.set_y(y);