修复判断是否可到目标点bug

MPC/navigation.cpp

@@ -1009,8 +1009,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode,bool directY)
   double minYaw=5*M_PI/180.0;
   if(move_mode_==Monitor_emqx::eMain){
     L=1.3+2.7;
-    if(directY)
-      minYaw=2*M_PI/180.0;
+
   }
   if(directY){
     current=current.rotate(current.x(),current.y(),M_PI/2);
@@ -1018,7 +1017,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode,bool directY)
     W=L;
     L=t;
   }
-  double minR = W / 2 + L / tan(minYaw * M_PI / 180.0);
+  double minR = W / 2 + L / tan(minYaw);
   //printf("l:%f,w:%f\n",l,w);
   std::vector<Pose2d> poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l*2, w*2);
 
@@ -1039,9 +1038,9 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode,bool directY)
     }else if(nagY&&posiY){
       return true;
     }
-    /*printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n",directY,
+    printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n",directY,
            relative.x(),relative.y(),targetNode.l(),targetNode.w(),targetNode.theta(),
-           minR,ax,ay);*/
+           minR,ax,ay);
   }