1.降低到库精度要求

2.车位点强制可到达(临时处理雷达波动导致PossibleToTarget()返回false)

MPC/monitor/terminator_emqx.cpp

@@ -54,7 +54,6 @@ bool Terminator_emqx::AddCallback(std::string topic,Callback callback,void* cont
   }
 }
 
-
 Terminator_emqx::Terminator_emqx(std::string nodeId)
     :client_(nullptr),nodeId_(nodeId),connected_(false)
 {

MPC/navigation.cpp

@@ -628,8 +628,8 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
-    target.set_l(0.05);
-    target.set_w(0.10);
+    target.set_l(0.10);
+    target.set_w(0.05);
     target.set_id(space.id());
 
     //整车协调的时候，保持前后与车位一致即可
@@ -1106,6 +1106,10 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         return false;
     }
     Pose2d current = timedPose_.Get();
+    if(targetNode.id().find('S')){
+        //车位点强制可到达
+        return true;
+    }
 
     double tx = targetNode.x();
     double ty = targetNode.y();

MPC/rotate_mpc.cpp

@@ -210,7 +210,7 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 //    } else{
 //
 //    }
-    printf("target: %f\n",targetAngularInAGV.theta()/M_PI*180);
+//    printf("target: %f\n",targetAngularInAGV.theta()/M_PI*180);
 
     Eigen::VectorXd condition(4);
     condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;