去掉原地旋转，非自由方向需要原地旋转（待添加）

MPC/loaded_mpc.cpp

@@ -354,7 +354,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
     if (solve_angular < 0) correct_angular = -correct_angular;
   }
   ///-----
-  printf("input : %.4f  %.5f(%.5f)   output : %.4f  %.5f(%.5f)  ref : %.3f obs relative:(%f,%f) time:%.3f\n",
+  printf("input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f obs relative:(%f,%f) time:%.3f\n",
          line_velocity,wmg,angular,solve_velocity,solve_angular,correct_angular,
          ref_velocity,obs_relative_pose_.x(),obs_relative_pose_.y(),time);
 

MPC/navigation.cpp

@@ -307,10 +307,11 @@ bool Navigation::Stop() {
     {
       if(timedV_.timeout()==false && timedA_.timeout()==false)
       {
-        if(fabs(timedV_.Get())<1e-6 && fabs(timedA_.Get())<1e-6)
+        if(fabs(timedV_.Get())<1e-2 && fabs(timedA_.Get())<1e-3)
           return true;
         else
           monitor_->stop();
+        printf("Stoped wait V/A ==0(1e-4,1e-4),V:%f,A:%f\n",fabs(timedV_.Get()),fabs(timedA_.Get()));
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
       }else{
         printf("Stop failed V/A timeout\n");
@@ -318,6 +319,7 @@ bool Navigation::Stop() {
       }
     }
   }
+  printf("stoped ret false.\n");
   return false;
 }
 
@@ -401,7 +403,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     return false;
   }
 
-  Pose2d accuracy=target_diff;
+  Pose2d accuracy=target_diff.rotate(timedPose_.Get().theta());
   Pose2d thresh=Pose2d::abs(accuracy);
 
   int action=0; //1 原地旋转，2横移，3前进
@@ -425,10 +427,9 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       }
       return false;
     }
-
     
     //计算目标点在当前pose下的pose
-    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
+    Pose2d diff = Pose2d::relativePose(target,timedPose_.Get());
     //停止状态
     if (action == 0) {
       //
@@ -493,7 +494,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
           double velocity=limit(next_speed(timedV_.Get(),v,acc_v,dt),limit_v.min,limit_v.max);
           SendMoveCmd(move_mode_,Monitor_emqx::eMPC, velocity, 0);
           actionType_=eVertical;
-          printf(" Vrtical input:%f,out:%f\n",timedV_.Get(),velocity);
+          printf(" Vrtical input:%f,out:%f diff.x:%f\n",timedV_.Get(),velocity,diff.x());
           continue;
         }
       }
@@ -519,15 +520,16 @@ 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);
+
     //速度限制
     stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
     stLimit adjust_h={ action.adjusthorizonlimit().min(),action.adjusthorizonlimit().max() };
     stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
     stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
+
+    /*原地调整起点
+    NavMessage::PathNode first=action.pathnodes(0);
+
     //目标，精度
     double first_yaw=timedPose_.Get().theta();
     if(1<action.pathnodes_size()){
@@ -537,7 +539,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
     Pose2d first_target(first.pose().x(),first.pose().y(),first_yaw);
     Pose2d first_accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
     execute_adjust_action(first_target,first_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
-    Pose2d last_node=first_target;
+    Pose2d last_node=first_target;*/
     for(int i=1;i< action.pathnodes_size();++i)
     {
         NavMessage::PathNode node = action.pathnodes(i);
@@ -546,23 +548,20 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
         Pose2d target(node.pose().x(), node.pose().y(), node.pose().theta());
         Pose2d accuracy(node.accuracy().x(), node.accuracy().y(), node.accuracy().theta());
 
-        if (execute_along_action(last_node, target, accuracy, mpc_velocity, anyDirect) == false)
+        while(cancel_==false){
+          MpcResult ret=execute_along_action(timedPose_.Get(), target, accuracy, mpc_velocity, anyDirect);
+          if(ret==eMpcSuccess){
+            std::cout<<" MPC to target "<<target<<" completed"<<std::endl;
+            break;
+          }else if(ret==eImpossibleToTarget){
+            std::cout<<" MPC to target "<<target<<"  failed,retry"<<std::endl;
+          }else{
+            return false;
+          }
+        }
+        if(cancel_){
           return false;
-        //不是最后一个点，则原地当前点朝向
-        if(i!=action.pathnodes_size()-1)
-        {
-          //目标，精度
-          double adjust_yaw=timedPose_.Get().theta();
-          NavMessage::PathNode next=action.pathnodes(i+1);
-          adjust_yaw=Pose2d::vector2yaw(next.pose().x()-node.pose().x(),next.pose().y()-node.pose().y());
-
-          Pose2d adjust_target(node.pose().x(),node.pose().y(),adjust_yaw);
-          Pose2d adjust_accuracy(node.accuracy().x(),node.accuracy().y(),node.accuracy().theta());
-          execute_adjust_action(adjust_target,adjust_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
-          last_node=adjust_target;
         }
-
-
     }
     return true;
   }
@@ -835,20 +834,20 @@ bool Navigation::clamp_open() {
   return false;
 }
 
-bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
+Navigation::MpcResult Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
                                       stLimit limit_v,bool autoDirect) {
   if (inited_ == false) {
     printf(" navigation has not inited\n");
-    return false;
+    return eMpcFailed;
   }
   if (PoseTimeout()) {
     printf(" navigation Error:Pose is timeout \n");
-    return false;
+    return eMpcFailed;
   }
   //生成轨迹
   Trajectory traj = Trajectory::create_line_trajectory(begin, target);
   if (traj.size() == 0)
-    return true;
+    return eMpcSuccess;
   //判断 巡线方向
   bool directY=false;
   if(autoDirect){
@@ -869,48 +868,22 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     }
     if (PoseTimeout()) {
       printf(" navigation Error:Pose is timeout \n");
-      return false;
+      return eMpcFailed;
     }
     if(timedV_.timeout() || timedA_.timeout()){
       printf(" navigation Error:v/a is timeout \n");
-      return false;
+      return eMpcFailed;
     }
     //判断是否到达终点
-    /*int direct=1;
-    if(directY) direct=2;*/
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,2)) {
+    Pose2d target_accuracy=target_diff;
+    if(directY)
+      target_accuracy=Pose2d::abs(target_diff.rotate(timedPose_.Get().theta()));
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_accuracy,2)) {
       if (Stop()) {
         printf(" exec along completed !!!\n");
-        return true;
+        return eMpcSuccess;
       }
     }
-    /*
-     * 判断周边是否有障碍物，半径r，当障碍物进入r内，停下等待，当障碍物远离 1.2*r，恢复
-     *
-    if(move_mode_==Monitor_emqx::eSingle) {
-      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());
-      if (obs_stoped == false) {
-        if (distance < r && fabs(obs_relative.x()) > 0.2) {
-          std::cout << "find obs stop to wait...." << std::endl;
-          Stop();
-          obs_stoped = true;
-          continue;
-        }
-      } else {
-        //障碍解除
-        if (distance > 1.2 * r) {
-          std::cout << "obs release continue MPC" << std::endl;
-          obs_stoped = false;
-        } else {
-          continue;
-        }
-
-      }
-    }
-     */
 
     //一次变速
     std::vector<double> out;
@@ -920,13 +893,16 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     }, "mpc_once");
     if (ret == false) {
       Stop();
-      return false;
+      return eMpcFailed;
     }
     /*
      * AGV 在终点附近低速巡航时，设置最小速度0.05
      */
     Pose2d target_in_agv=Pose2d::relativePose(target,timedPose_.Get());
-    if(Pose2d::abs(target_in_agv)<Pose2d(0.2,0.2,M_PI*2)) {
+    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)) {
       if (out[0] >= 0 && out[0] < limit_v.min)
         out[0] = limit_v.min;
       if (out[0] < 0 && out[0] > -limit_v.min)
@@ -941,12 +917,13 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     /*
      * 判断车辆是否在终点附近徘徊，无法到达终点
      */
-    if (mpc_possible_to_end(target, target_diff,directY) == false) {
+    if (mpc_possible_to_end(target, target_accuracy,directY) == false) {
       printf(" --------------MPC  imposible to target -----------------------\n");
       if(fabs(timedV_.Get())>0.1 || fabs(timedA_.Get())>5*M_PI/180.0)
         Stop();
+      return eImpossibleToTarget;
       //调整到目标点
-      stLimit limitv, limitR;
+      /*stLimit limitv, limitR;
       limitv.min = 0.05;
       limitv.max = 0.1;
       limitR.min = 2 * M_PI / 180.0;
@@ -955,54 +932,37 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       if (execute_adjust_action(target, accuracy, limitv, limitv, limitR,false))
         return true;
       else
-        return false;
+        return false;*/
     }
 
     //std::this_thread::sleep_for(std::chrono::milliseconds (400));
   }
 
-  return false;
+  return eMpcFailed;
 }
 
 bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff,bool directY)
 {
+
   if(timedPose_.timeout()==false) {
-    Pose2d current=timedPose_.Get();
-    if(directY){
-      float yaw=current.theta();
-      yaw += M_PI / 2;
-      if (yaw > M_PI)
-        yaw -= 2 * M_PI;
-      current.mutable_theta()=yaw;
+    Pose2d current = timedPose_.Get();
+    Pose2d targetInPose = Pose2d::relativePose(target, current);
+    //std::cout<<" target in pose:"<<targetInPose<<", target diff:"<<target_diff<<std::endl;
+    if (directY) {
+      targetInPose=targetInPose.rotate(M_PI / 2.0);
     }
-
-    Pose2d diff = Pose2d::relativePose(target, current);
-    if (fabs(diff.y()) < target_diff.y() && fabs(diff.x()) < target_diff.x()) { //xy在精度内
-      if (timedV_.timeout() == true)
+    if (fabs(targetInPose.x()) < target_diff.x()) {
+      if (fabs(targetInPose.y()) > target_diff.y())
         return false;
-      /*if(fabs(timedV_.Get())<0.05 && fabs(diff.theta())>target_diff.theta())
-        return false;*/
-      return true;
-    } else {  //xy不在精度内
-      if(directY==false) {
-        if (fabs(diff.x()) < 1e-3)
-          return false; //x已到达，Y未到达，则不可能到达目标点
-      }
-      double theta = fabs(atan(diff.y() / diff.x()));
-
-      if (theta > 5 * M_PI / 180.0) {
-        if (timedV_.timeout() == true)
-          return false;
-        if (fabs(timedV_.Get()) < 0.05)
-          return false;
-        else
-          return true;
-      } else {
+      else
         return true;
-      }
+    } else {
+      return true;
     }
+  }else{
+    return false;
   }
-  return false;
+
 }
 
 void Navigation::navigatting() {

MPC/navigation.h

@@ -43,6 +43,11 @@ public:
         eY,
         eXY
     };
+    enum MpcResult{
+        eMpcSuccess=0,
+        eMpcFailed,
+        eImpossibleToTarget
+    };
 
 
  public:
@@ -90,7 +95,7 @@ public:
     /*
      * autoDirect:自动选择 纵向还是横向巡线
      */
-    bool execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
+    MpcResult execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
                               stLimit limit_v,bool autoDirect);
     /*
      * 调整到目标点，nearest_yaw:xy轴线任一朝向对准目标朝向即可

config/webots/navigation.prototxt

@@ -4,7 +4,7 @@ main_agv:false
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"127.0.0.1"
+	ip:"192.168.225.224"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"
@@ -14,7 +14,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-child-nav"
-        ip:"127.0.0.1"
+        ip:"192.168.225.224"
         port:1883
         pubStatuTopic:"agv_child/agv_statu"
         pubNavStatuTopic:"agv_child/nav_statu"
@@ -25,7 +25,7 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-child"
-	ip:"127.0.0.1"
+	ip:"192.168.225.224"
 	port:1883
 	subBrotherStatuTopic:"lidar_odom/main"
 }
@@ -34,13 +34,13 @@ x_mpc_parameter
 {
 	shortest_radius:26
 	dt:0.1
-	acc_velocity:1.5
+	acc_velocity:1.0
 	acc_angular:10
 }
 y_mpc_parameter
 {
 	shortest_radius:24.5
 	dt:0.1
-	acc_velocity:1.5
+	acc_velocity:1.0
 	acc_angular:10
 }

config/webots/navigation_main.prototxt

@@ -3,7 +3,7 @@ main_agv:true
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"127.0.0.1"
+	ip:"192.168.225.224"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -13,7 +13,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-main-nav"
-	ip:"127.0.0.1"
+	ip:"192.168.225.224"
 	port:1883
 	pubStatuTopic:"agv_main/agv_statu"
 	pubNavStatuTopic:"agv_main/nav_statu"
@@ -24,7 +24,7 @@ Terminal_emqx
 brother_emqx
 {
 	NodeId:"agv-brother-main"
-	ip:"127.0.0.1"
+	ip:"192.168.225.224"
 	port:1883
 	subBrotherStatuTopic:"lidar_odom/child"
 }