修复入库角度bug

MPC/loaded_mpc.cpp

@@ -45,12 +45,10 @@ class FG_eval_half_agv {
 
       // Weights for how "important" each cost is - can be tuned
       const double y_cost_weight = 1000;
-      const double th_cost_weight = 4000;
-      const double v_cost_weight = 5000;
-      const double vth_cost_weight = 1000;
+      const double v_cost_weight = 2000;
+      const double vth_cost_weight = 500;
       const double a_cost_weight = 1;
       const double ath_cost_weight=10;
-      const double obs_distance_weight=5000.0;
 
       // Cost for CTE, psi error and velocity
       for (int t = 0; t < N; t++) {

MPC/navigation.cpp

@@ -542,6 +542,7 @@ bool Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelb
     rotated.mutable_theta() = space.theta();
     rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
   }
+  printf("brother spaceid:%s  current space id:%s ,space theta :%f\n",brother.space_id().c_str(),space.id().c_str(),space.theta());
   double x = space.x() - wheelbase/2 * cos(rotated.theta());
   double y = space.y() - wheelbase/2 * sin(rotated.theta());
   target.set_x(x);
@@ -550,7 +551,7 @@ bool Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelb
   target.set_l(0.05);
   target.set_w(0.03);
   target.set_id(space.id());
-
+  printf(" new target :%f,%f, theta:%f,wheelbase:%f\n",target.x(),target.y(),rotated.theta(),wheelbase);
   //整车协调的时候，保持前后与车位一致即可
   if(move_mode_==Monitor_emqx::eMain){
     double yawDiff1 = (rotated - timedPose_.Get()).theta();
@@ -571,8 +572,8 @@ bool Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelb
 
       SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
       actionType_ = eRotation;
-      printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
-             timedA_.Get(), angular, limit_angular, yawDiff);
+      /*printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
+             timedA_.Get(), angular, limit_angular, yawDiff);*/
 
       continue;
     } else {
@@ -885,7 +886,8 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node,stLimit
     if (directY) {
       target_in_agv=target_in_agv.rotate(M_PI / 2.0);
     }
-    if(Pose2d::abs(target_in_agv)<Pose2d(0.2,2,M_PI*2)) {
+    Pose2d absPose=Pose2d::abs(target_in_agv);
+    if(absPose.x()<0.5 && absPose.y()<0.5) {
       if (out[0] >= 0 && out[0] < limit_v.min)
         out[0] = limit_v.min;
       if (out[0] < 0 && out[0] > -limit_v.min)
@@ -928,7 +930,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode,bool directY)
 
   double W = 2.45;
   double L = 1.3;
-  double minYaw=5*M_PI/180.0;
+  double minYaw=3*M_PI/180.0;
   if(move_mode_==Monitor_emqx::eMain){
     L=1.3+2.7;
 

MPC/pose2d.cpp

@@ -109,7 +109,7 @@ Pose2d Pose2d::rotate(float ox,float oy,float theta)const{
 
   Pose2d translate(x()-ox,y()-oy,m_theta);//平移
   Pose2d rotated=translate.rotate(theta);
+
   Pose2d ret(rotated.x()+ox,rotated.y()+oy,rotated.theta());
-  //std::cout<<" self:"<<*this<<" , R:"<<rotated<<",ret:"<<ret<<std::endl;
   return ret;
 }

config/webots/navigation.prototxt

@@ -53,7 +53,7 @@ InOutVLimit
 NodeVelocityLimit
 {
     min:0.03
-    max:3
+    max:2
 }
 NodeAngularLimit
 {

config/webots/navigation_main.prototxt

@@ -52,7 +52,7 @@ InOutVLimit
 NodeVelocityLimit
 {
     min:0.03
-    max:3
+    max:2
 }
 NodeAngularLimit
 {