1.修改入库点基准，由载车板中心位置基准改为载车板靠墙侧车轮挡板位置为基准

2.添加预入库动作，提高入库精度

MPC/navigation.cpp

@@ -610,36 +610,36 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     double dt = 0.1;
     Pose2d rotated = timedPose_.Get();
 
-    //前车先到,当前车进入2点,保持与前车一致的朝向
+    double x = space.x();
+    double y = space.y();
+    //前车先到,后车进入2点,保持与前车一致的朝向
     if (brother.in_space() && brother.space_id() == space.id()) {
         rotated.mutable_theta() = brother.odom().theta();
-    } else { //当前车先到（后车）,倒车入库
+        if (move_mode_ == eSingle) {
+            x -= wheelbase * cos(rotated.theta());
+            y -= wheelbase * sin(rotated.theta());
+            printf("确定车位点：eSingle\n");
+        }
+    } else { //当后车先到,倒车入库
         rotated.mutable_theta() = space.theta();
         rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
 
-    double x = space.x();
-    double y = space.y();
-    if (move_mode_ == eSingle) {
-        x -= wheelbase / 2 * cos(rotated.theta());
-        y -= wheelbase / 2 * sin(rotated.theta());
-        printf("确定车位点：eSingle\n");
-    }
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
-    target.set_l(0.10);
+    target.set_l(0.05);
     target.set_w(0.05);
     target.set_id(space.id());
 
-    //整车协调的时候，保持前后与车位一致即可
-    if (move_mode_ == eDouble) {
-        double yawDiff1 = (rotated - timedPose_.Get()).theta();
-        double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
-        if (fabs(yawDiff1) < fabs(yawDiff2)) {
-            rotated = rotated + Pose2d(0, 0, M_PI);
-        }
-    }
+//    //整车协调的时候，保持前后与车位一致即可
+//    if (move_mode_ == eDouble) {
+//        double yawDiff1 = (rotated - timedPose_.Get()).theta();
+//        double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
+//        if (fabs(yawDiff1) < fabs(yawDiff2)) {
+//            rotated = rotated + Pose2d(0, 0, M_PI);
+//        }
+//    }
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -696,7 +696,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     if (action.type() == 1) {
         Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
                    0);
-        action.mutable_streetnode()->set_l(0.2);
+        action.mutable_streetnode()->set_l(0.05);
         action.mutable_streetnode()->set_w(0.05);
         action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) {
@@ -712,13 +712,65 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
             return false;
         }
+
         //进库前提升机构上升
         if (lifter_rise() == false){
-            printf(" Enter space | lifter rise failed, line:%d\n",__LINE__);
+            printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
+        }
+
+        //移动到预入库点位，较高精度
+        NavMessage::PathNode pre_target;
+        bool jump_preenter = false;
+        if (move_mode_ == eSingle){
+            double x = action.spacenode().x();
+            double y = action.spacenode().y();
+            double theta = action.spacenode().theta();
+
+            double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
+            Pose2d current = timedPose_.Get();
+            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
+            if (diff < back_distance) jump_preenter = true;
+            x -= back_distance* cos(theta);
+            y -= back_distance* sin(theta);
+
+            pre_target.set_x(x);
+            pre_target.set_y(y);
+            pre_target.set_theta(theta);
+            pre_target.set_l(0.03);
+            pre_target.set_w(0.03);
+            pre_target.set_id("R_0");
+        }
+        else if (move_mode_ == eDouble){
+            double x = action.spacenode().x();
+            double y = action.spacenode().y();
+            double theta = action.spacenode().theta();
+
+            double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
+            Pose2d current = timedPose_.Get();
+            double diff = Pose2d::distance(current, Pose2d(x,y,theta));
+            if (diff < back_distance) jump_preenter = true;
+            x -= back_distance* cos(theta);
+            y -= back_distance* sin(theta);
+
+            pre_target.set_x(x);
+            pre_target.set_y(y);
+            pre_target.set_theta(theta);
+            pre_target.set_l(0.03);
+            pre_target.set_w(0.03);
+            pre_target.set_id("R_0");
+        }
+        else{ return printf("PreEnter space failed. move_mode_ error!. line: %d\n", __LINE__);}
+        if (!jump_preenter) {
+            if (MoveToTarget(pre_target, limit_v, limit_w, true, false) == false) {
+                printf(" PreEnter space:%s failed. type:%d. line:%d\n", action.spacenode().id().data(), action.type(),
+                       __LINE__);
+                return false;
+            }
         }
 
+        //入库
         if (MoveToTarget(new_target, limit_v, limit_w, true, false) == false) {
-            printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
+            printf(" Enter space:%s failed. type:%d\n",action.spacenode().id().data(), action.type());
             return false;
         }
     } else if (action.type() == 2) {

MPC/navigation_main.cpp

@@ -62,7 +62,6 @@ void NavigationMain::SendMoveCmd(int mode, ActionType type,
     if (monitor_) {
         monitor_->set_speed(mode, type, v, angular, wheelBase_);
     }
-
 }
 
 bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
@@ -204,28 +203,38 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     double acc_angular = 25 * M_PI / 180.0;
     double dt = 0.1;
     Pose2d rotated = Pose2d(space.x(), space.y(), space.theta());
-    target.set_l(0.05);
-    target.set_w(0.10);
-    target.set_id(space.id());
-    //后车先到,当前车进入2点,保持与后车一致的朝向
+
+    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();
+        if (move_mode_ == eSingle) {
+            x += wheelbase * cos(rotated.theta());
+            y += wheelbase * sin(rotated.theta());
+            printf("确定车位点：eSingle\n");
+        }
     } else { //当前车先到,正向
         printf("RotateBeforeEnterSpace | 该车先到, __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = space.theta();
+        if (move_mode_ == eSingle) {
+            printf("确定车位点：eSingle\n");
+        }
     }
-    std::cout << "===============================> RotateBeforeEnterSpace ,target:" << rotated << std::endl;
-    double x = space.x();
-    double y = space.y();
-    if (move_mode_ == eSingle) {
-        x += wheelbase / 2 * cos(rotated.theta());
-        y += wheelbase / 2 * sin(rotated.theta());
-        printf("确定车位点：eSingle\n");
+
+    if (move_mode_ == eDouble){
+        x -= wheelbase/2 * cos(rotated.theta());
+        y -= wheelbase/2 * sin(rotated.theta());
+        printf("确定车位点：eDouble\n");
     }
+    std::cout << "===============================> RotateBeforeEnterSpace ,target:" << rotated << std::endl;
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
+    target.set_l(0.05);
+    target.set_w(0.05);
+    target.set_id(space.id());
 
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));