去掉车位点调整，允许重载横移巡线

MPC/navigation.cpp

@@ -250,7 +250,7 @@ void Navigation::ResetClamp(ClampStatu statu){
 
 void Navigation::ResetPose(const Pose2d& pose)
 {
-  timedPose_.reset(pose,0.5);
+  timedPose_.reset(pose,1.0);
 }
 
 bool Navigation::Start(const NavMessage::NavCmd& cmd)
@@ -433,8 +433,8 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     }
     else
     {
-      double acc_v=2.0;
-      double acc_angular=20.0*M_PI/180.0;
+      double acc_v=0.6;
+      double acc_angular=10.0*M_PI/180.0;
       double dt=0.1;
       if (action == 1) {
         float minYawDiff=diff.theta();
@@ -507,8 +507,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
       return false;
     }
     bool anyDirect=(action.type()==3);
-    if (move_mode_==Monitor_emqx::eMain)  //双车模式静止自由方向做MPC
-      anyDirect=false;
+//    if (move_mode_==Monitor_emqx::eMain)  //双车模式静止自由方向做MPC
+//      anyDirect=false;
     //原地调整起点
     NavMessage::PathNode first=action.pathnodes(0);
     //速度限制
@@ -613,9 +613,9 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
                           action.streetnode().accuracy().theta());
   target = Pose2d(action.spacenode().pose().x(), action.spacenode().pose().y(), action.spacenode().pose().theta());
   //目标点精度
-  target_accuracy = Pose2d(action.spacenode().accuracy().x(),
-                           action.spacenode().accuracy().y(),
-                           action.spacenode().accuracy().theta());
+  target_accuracy = Pose2d(action.spacenode().accuracy().x()*2,
+                           action.spacenode().accuracy().y()*2,
+                           action.spacenode().accuracy().theta()*2);
   double yaw = Pose2d::vector2yaw(target.x() - begin.x(), target.y() - begin.y());
   if (action.type() == 2) {  //出库起点终点对调
     Pose2d mid = target;
@@ -637,7 +637,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
   stLimit mpc_velocity = {0.05, 0.3};
 
   //判断当前点与起点的距离
-  Pose2d distance(0.5, 0.5, M_PI * 2);
+  Pose2d distance(1, 1, M_PI * 2);
   if (action.type() == 2) {  //出库要求起点距离当前点非常近
     distance = Pose2d(1, 0.1, 3 * M_PI);
     adjust_x = {0.03, 0.1};
@@ -651,18 +651,24 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     return false;
   }
 
-  if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
-    return false;
+  if (action.type() == 1) //1,进库
+    if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
+      return false;
 
-  if (execute_along_action(begin, target, target_accuracy, mpc_velocity, false) == false)
-    return false;
+  //if (action.type() == 2) //2，出库
+    if (execute_along_action(begin, target, target_accuracy, mpc_velocity, false) == false)
+      return false;
   return true;
 }
 
 
 bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY) {
-  if (timedPose_.timeout() == true && timedV_.timeout() == false && timedA_.timeout() == false) {
-    printf(" MPC once Error:Pose/V/A is timeout \n");
+  if (timedPose_.timeout() == true) {
+    printf(" MPC once Error:Pose is timeout \n");
+    return false;
+  }
+  if(timedV_.timeout() == true || timedA_.timeout() == true){
+    printf(" MPC once Error:V/A is timeout \n");
     return false;
   }
   if (traj.size() == 0) {
@@ -726,7 +732,7 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
       float diffYaw=absDiff.theta();  //[0-pi)
       float diffYaw2=absDiff.theta()-M_PI;
       bool yawArrived=( fabs(diffYaw)<diff.theta() || fabs(diffYaw2)<diff.theta() );
-      if(yawArrived && fabs(velocity) < 0.05 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.06 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
   }else if(direct==2){    //横向MPC
@@ -739,7 +745,7 @@ bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
       printf(" diff 123 :%f  %f  %f %f\n",diffYaw,diffYaw1,diffYaw2,diffYaw3);
       bool yawArrived=( fabs(diffYaw)<diff.theta() || fabs(diffYaw1)<diff.theta()
           || fabs(diffYaw2)<diff.theta() ||fabs(diffYaw3)<diff.theta() );
-      if(yawArrived && fabs(velocity) < 0.05 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.06 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
   }else{  //原地调整
@@ -835,8 +841,12 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       //发送暂停消息
       continue;
     }
-    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
-      printf(" navigation Error:Pose/v/a is timeout \n");
+    if (timedPose_.timeout()) {
+      printf(" navigation Error:Pose is timeout \n");
+      return false;
+    }
+    if(timedV_.timeout() || timedA_.timeout()){
+      printf(" navigation Error:v/a is timeout \n");
       return false;
     }
     //判断是否到达终点
@@ -852,7 +862,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
      * 判断周边是否有障碍物，半径r，当障碍物进入r内，停下等待，当障碍物远离 1.2*r，恢复
      * */
     if(move_mode_==Monitor_emqx::eSingle) {
-      double r = 2.8;
+      double r = 2.4;
       static bool obs_stoped = false;
       Pose2d obs_relative = Pose2d::relativePose(timedBrotherPose_.Get(), timedPose_.Get());
       double distance = sqrt(obs_relative.x() * obs_relative.x() + obs_relative.y() * obs_relative.y());

config/navigation.prototxt

@@ -39,8 +39,8 @@ x_mpc_parameter
 }
 y_mpc_parameter
 {
-	shortest_radius:7.35
+	shortest_radius:24.5
 	dt:0.1
 	acc_velocity:1
-	acc_angular:20
+	acc_angular:10
 }