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

2.mpc过程添加打印两次实时v/w值
3.巡线非行进方向精度由0.2扩大到0.25m，入库角度偏差由0.3度放大到0.5度
4.mpc步数由15改为19（提前减速）

MPC/loaded_mpc.cpp

@@ -172,7 +172,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double wmg = statu[4];
 
 
-    size_t N=15;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
+    size_t N=19;//int(10./(1+exp(-16.*(fabs(line_velocity)-0.5)))+10);
     size_t nx = 0;
     size_t ny = nx + N;
     size_t nth = ny + N;

MPC/navigation.cpp

@@ -639,6 +639,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
                    node.x(), node.y(), node.l(), node.w(), node.theta());
         } else {
             retry_count++;
+            printf("MpcToTarget failed,ret:%d\n", ret);
             if (retry_count > retry_count_limit)
                 return false;
         }
@@ -677,13 +678,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
                 node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
                 node.set_l(0.03);
-                node.set_w(0.20);
+                node.set_w(0.25);
             } else {
                 NavMessage::PathNode befor = action.pathnodes(i - 1);
                 Pose2d vec(node.x() - befor.x(), node.y() - befor.y(), 0);
                 node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
                 node.set_l(0.02);
-                node.set_w(0.2);
+                node.set_w(0.25);
             }
             int directions = Direction::eForward;
             if (anyDirect)
@@ -808,7 +809,7 @@ Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        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;
@@ -902,7 +903,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        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;
@@ -1749,37 +1750,38 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             return eMpcFailed;
         }
 
-        //出入库实时判断门控或者载车板
-        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;
-                        }
-                    }
-                }
-            }
-        }
+//        //出入库实时判断门控或者载车板
+//        float door_no_1_y = -5;
+//        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;
+//                        }
+//                    }
+//                }
+//            }
+//        }
 
         //判断是否到达终点
         if (IsArrived(node)) {
@@ -1792,6 +1794,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
             }
         }
 
+        float v_before_compute = timedV_.Get();
+        float a_before_compute = timedA_.Get();
         //一次变速
         std::vector<double> out;
 //        std::vector<double> diff_yaw; //两车分别与整车的相对角度
@@ -1891,6 +1895,10 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
                         std::to_string(cur_Bro_pose.theta());
         log_inf_line += " ";
 
+        log_inf_line += std::to_string(v_before_compute) + " " + std::to_string(a_before_compute);//计算时反馈速度
+        log_inf_line += " ";
+        log_inf_line += std::to_string(timedV_.Get()) + " " + std::to_string(timedA_.Get());//下发时反馈速度
+        log_inf_line += " ";
         log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
         log_inf_line += " ";
         log_inf_line += std::to_string(out[2]) + " " + std::to_string(out[3]);//下发速度

MPC/navigation_main.h

@@ -29,8 +29,10 @@ public:
     virtual bool SwitchMode(int mode, float wheelBase) {
         printf("change mode to:%d\n", mode);
         wheelBase_ = wheelBase;
-        if (move_mode_ == mode)
+        if (move_mode_ == mode){
             return true;
+        }
+
         if (mode == eDouble) {
             if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
                 std::cout << "Brother/self pose is timeout can not set MainAGV pose" << std::endl;