重载下只允许x方向mpc

MPC/monitor/monitor_emqx.cpp

@@ -77,7 +77,7 @@ void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,d
 }
 void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a)
 {
-  double w=fabs(a)>0.00001?a:0.0;
+  double w=fabs(a)>0.0001?a:0.0;
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;

MPC/navigation.cpp

@@ -507,7 +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;
     //原地调整起点
     NavMessage::PathNode first=action.pathnodes(0);
     //速度限制
@@ -638,14 +639,15 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
   //判断当前点与起点的距离
   Pose2d distance(0.5, 0.5, M_PI * 2);
   if (action.type() == 2) {  //出库要求起点距离当前点非常近
-    distance = Pose2d(0.05, 0.05, 2 * M_PI);
+    distance = Pose2d(1, 0.1, 3 * M_PI);
     adjust_x = {0.03, 0.1};
     adjust_h = {0.03, 0.1};
     adjust_angular = {2 * M_PI / 180.0, 5 * M_PI / 180.0};
   }
-  if (!(Pose2d::abs(begin - timedPose_.Get()) < distance)) {
+  Pose2d relativeDiff=Pose2d::relativePose(begin,timedPose_.Get());
+  if (!(Pose2d::abs(relativeDiff) < distance)) {
     printf(" Inout space failed type:%d: current pose too far from begin node\n", action.type());
-    std::cout << " begin：" << begin << "   current:" << timedPose_.Get() << std::endl;
+    std::cout << " begin：" << begin << "   current:" << timedPose_.Get()<<"  relative:"<<relativeDiff << std::endl;
     return false;
   }
 
@@ -737,7 +739,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.03 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.05 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
   }else{  //原地调整
@@ -893,7 +895,8 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
      */
     if (mpc_possible_to_end(target, target_diff,directY) == false) {
       printf(" --------------MPC  imposible to target -----------------------\n");
-      Stop();
+      if(fabs(timedV_.Get())>0.1 || fabs(timedA_.Get())>5*M_PI/180.0)
+        Stop();
       //调整到目标点
       stLimit limitv, limitR;
       limitv.min = 0.05;

MPC/navigation_main.cpp

@@ -25,7 +25,7 @@ void NavigationMain::ResetPose(const Pose2d& pose){
     Pose2d brother=timedBrotherPose_.Get();
     Pose2d diff=Pose2d::abs(Pose2d::relativePose(brother,pose));
 
-    if(diff.x()>3.6 || diff.x()<2.2 || diff.y()>0.2 || diff.theta()>5*M_PI/180.0)
+    if(diff.x()>3.6 || diff.x()<2.2 || diff.y()>0.3 || diff.theta()>5*M_PI/180.0)
     {
       std::cout<<" distance with two agv is too far diff: "<<diff<<std::endl;
       return;