去掉原地旋转，自由方向需要原地旋转

MPC/loaded_mpc.cpp

@@ -7,7 +7,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 
-size_t N = 20;                  //优化考虑后面多少步
+size_t N = 25;                  //优化考虑后面多少步
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -388,7 +388,7 @@ bool LoadedMPC::filte_Path(const Pose2d& point,Pose2d target,Trajectory trajecto
 
   double theta=gradient2theta(gradient,target.x()-point.x()>=0);
 
-  if(trajectory.size() < point_num)
+  if(trajectory.size() < 2)
     return false;
 
   poses.clear();

MPC/monitor/monitor_emqx.cpp

@@ -60,12 +60,13 @@ void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,d
     return;
   }
   double w=fabs(a)>0.001?a:0.0;
+  double velocity=fabs(v)>0.001?v:0;
   MqttMsg msg;
   NavMessage::ToAgvCmd speed;
   heat_=(heat_+1)%255;
   speed.set_h(heat_);
   speed.set_t(type);
-  speed.set_v(v);
+  speed.set_v(velocity);
   speed.set_w(w);
   speed.set_l(L);
 

MPC/navigation.cpp

@@ -403,7 +403,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     return false;
   }
 
-  Pose2d accuracy=target_diff.rotate(timedPose_.Get().theta());
+  Pose2d accuracy=Pose2d::abs(target_diff.rotate(timedPose_.Get().theta()));
   Pose2d thresh=Pose2d::abs(accuracy);
 
   int action=0; //1 原地旋转，2横移，3前进
@@ -508,6 +508,70 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
 
 }
 
+bool Navigation::RotateReferToTarget(const Pose2d& target,stLimit limit_rotate,bool anyDirect) {
+
+  while(cancel_==false) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    if (timedPose_.timeout()) {
+      printf(" RotateBytarget failed  pose timeout\n");
+      return false;
+    }
+    if (timedA_.timeout()) {
+      printf(" RotateBytarget failed  wmg timeout\n");
+      return false;
+    }
+    Pose2d targetInPose = Pose2d::relativePose(target, timedPose_.Get());
+    float yawDiff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y());
+    float minYawdiff = yawDiff;
+
+    if (anyDirect) {
+      Pose2d p0 = timedPose_.Get();
+      float yawdiff[4] = {0};
+      yawdiff[0] = yawDiff;
+
+      Pose2d relativePose;
+      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI / 2));
+      yawdiff[1] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());  ////使用减法，保证角度在【-pi pi】
+      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI));
+      yawdiff[2] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
+      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, 3 * M_PI / 2));
+      yawdiff[3] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
+      for (int i = 1; i < 4; ++i) {
+        if (fabs(yawdiff[i]) < fabs(minYawdiff)) {
+          minYawdiff = yawdiff[i];
+          targetInPose=Pose2d::relativePose(target,p0-Pose2d(0,0,i*M_PI/2.0));
+        }
+      }
+    }
+    std::cout<<" Rotate refer to target:"<<target<<",targetInPose:"
+        <<targetInPose<<",anyDirect:"<<anyDirect<<std::endl;
+    //以当前点为原点，想方向经过目标点的二次曲线曲率>0.25,则需要调整 y=a x*x
+
+    float cuv=compute_cuv(targetInPose);
+    float dt=0.1;
+    float acc_angular=15*M_PI/180.0;
+    if (cuv>2*M_PI/180.0) {
+      double theta = limit_gause(minYawdiff, limit_rotate.min, limit_rotate.max);
+      double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
+      double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
+
+      SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
+      actionType_ = eRotation;
+      printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f anyDitect:%d\n",
+             timedA_.Get(), angular, limit_angular, minYawdiff, anyDirect);
+
+      continue;
+    }else{
+      if(fabs(timedA_.Get())<5*M_PI/180.0) {
+        printf(" Rotate refer target completed,cuv:%f\n", cuv);
+        return true;
+      }
+      continue;
+    }
+  }
+  return false;
+}
+
 
 bool Navigation::execute_nodes(NavMessage::NewAction action)
 {
@@ -540,28 +604,36 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
     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;*/
-    for(int i=1;i< action.pathnodes_size();++i)
-    {
-        NavMessage::PathNode node = action.pathnodes(i);
-        /// 巡线到目标点，判断巡线方向
-
-        Pose2d target(node.pose().x(), node.pose().y(), node.pose().theta());
-        Pose2d accuracy(node.accuracy().x(), node.accuracy().y(), node.accuracy().theta());
-
-        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;
-          }
+    for(int i=0;i< action.pathnodes_size();++i) {
+      NavMessage::PathNode node = action.pathnodes(i);
+      /// 巡线到目标点，判断巡线方向
+
+      Pose2d target(node.pose().x(), node.pose().y(), node.pose().theta());
+      Pose2d accuracy(node.accuracy().x(), node.accuracy().y(), node.accuracy().theta());
+
+      while (cancel_ == false) {
+        if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, Pose2d::abs(accuracy))) {
+          break;
+        }
+        if (RotateReferToTarget(target, adjust_angular, anyDirect) == false) {
+          std::cout << " Rotate refer to target failed,target:" << target <<
+                    "  anyDirect:" << anyDirect << std::endl;
+          return false;
         }
-        if(cancel_){
+
+        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 << "  impossible to target,retry" << std::endl;
+        } else {
           return false;
         }
+      }
+      if (cancel_) {
+        return false;
+      }
     }
     return true;
   }
@@ -718,7 +790,7 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   Pose2d relative = Pose2d::relativePose(brother, pose);
   if(directY)
     relative = Pose2d::relativePose(brother.rotate(brother.x(),brother.y(),M_PI/2), pose.rotate(pose.x(),pose.y(),M_PI/2));//计算另一节在当前小车坐标系下的坐标
-  std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
+  //std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
   if (move_mode_ == Monitor_emqx::ActionMode::eMain)
     relative = Pose2d(-1e8, -1e8, 0);
 
@@ -757,7 +829,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.06 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.06 && fabs(angular) < 10 * M_PI / 180.0)
         return true;
     }
   }else if(direct==2){    //横向MPC
@@ -770,18 +842,35 @@ 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.06 && fabs(angular) < 5 * M_PI / 180.0)
+      if(yawArrived && fabs(velocity) < 0.06 && fabs(angular) < 10 * M_PI / 180.0)
         return true;
     }
   }else{  //原地调整
     if (absDiff < diff) {
-      if (fabs(velocity) < 0.03 && fabs(angular) < 5 * M_PI / 180.0)
+      if (fabs(velocity) < 0.06 && fabs(angular) < 10 * M_PI / 180.0)
         return true;
     }
   }
   //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
     return false;
 }
+bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
+                           const Pose2d& target,const Pose2d& diff)
+{
+  //std::cout<<cur<<","<<target<<","<<nearest_yaw<<",v"<<velocity<<"a:"<<angular<<"diff:"<<diff<<std::endl;
+  Pose2d distance=Pose2d::relativePose(target,cur);
+  Pose2d absDiff=Pose2d::abs(distance);
+
+  //std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
+
+    if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
+    {
+      if(fabs(velocity) < 0.06 && fabs(angular) < 5 * M_PI / 180.0)
+        return true;
+    }
+  //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
+  return false;
+}
 
 void Navigation::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                              double v,double angular){
@@ -878,7 +967,7 @@ Navigation::MpcResult Navigation::execute_along_action(const Pose2d& begin,const
     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 (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, Pose2d::abs(target_accuracy),2)) {
       if (Stop()) {
         printf(" exec along completed !!!\n");
         return eMpcSuccess;
@@ -945,6 +1034,7 @@ bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_d
 {
 
   if(timedPose_.timeout()==false) {
+
     Pose2d current = timedPose_.Get();
     Pose2d targetInPose = Pose2d::relativePose(target, current);
     //std::cout<<" target in pose:"<<targetInPose<<", target diff:"<<target_diff<<std::endl;
@@ -957,6 +1047,12 @@ bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_d
       else
         return true;
     } else {
+      float y=fabs(targetInPose.y())-fabs(target_diff.y());
+      if(y<0) y=0;
+      Pose2d minTarget(targetInPose.x(),y,0);
+      float cuv=compute_cuv(minTarget);
+      if(cuv>15*M_PI/180.0)
+        return false;
       return true;
     }
   }else{
@@ -1034,4 +1130,13 @@ void Navigation::navigatting() {
 
 void Navigation::SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
     printf("Child AGV can not Switch Mode\n");
+}
+
+float Navigation::compute_cuv(const Pose2d& target)
+{
+  float x=fabs(target.x());
+  float y=fabs(target.y());
+  float cuv= atan(y/(x+1e-8));
+  printf(" ---------------------- cuv：%f\n",cuv);
+  return cuv;
 }

MPC/navigation.h

@@ -92,6 +92,8 @@ public:
      */
     static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,
                           const Pose2d& diff,int direct);
+    static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,
+                          const Pose2d& diff);
     /*
      * autoDirect:自动选择 纵向还是横向巡线
      */
@@ -101,7 +103,12 @@ public:
      * 调整到目标点，nearest_yaw:xy轴线任一朝向对准目标朝向即可
      */
     bool execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
-                            stLimit limit_h,stLimit limit_rotate,bool anyDitect);
+                            stLimit limit_h,stLimit limit_rotate,bool anyDirect);
+    /*
+     * 旋转摆正，朝向目标点,
+     * anyDirect: 任意一个朝向对准目标点
+     */
+    virtual bool RotateReferToTarget(const Pose2d& target,stLimit limit_rotate,bool anyDirect);
     /*
      * 按照路径点导航
      */
@@ -122,7 +129,10 @@ public:
     void navigatting();
 
     virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
-
+    /*
+     * 计算以当前点到目标点的曲率
+     */
+    float compute_cuv(const Pose2d& target);
     /*
      * 发布导航模块状态
      */

MPC/pose2d.cpp

@@ -38,10 +38,16 @@ float Pose2d::vector2yaw(float x,float y)
   if(r<1e-8)
     return 0;
   float yaw=asin(y/r);
-  if(x>=0)  //1,4象限
-    return yaw;
+
   if (x<0 ) //2 3象限
-    return M_PI-yaw;
+    yaw= M_PI-yaw;
+
+  if(yaw>M_PI)
+    yaw-=M_PI*2;
+  if(yaw<=-M_PI)
+    yaw+=M_PI*2;
+
+  return yaw;
 
 }
 

MPC/pose2d.h

@@ -111,7 +111,7 @@ class Pose2d
 
     static float distance(const Pose2d& pose1,const Pose2d& pose2);
     static Pose2d relativePose(const Pose2d& world_pose,const Pose2d& axis_pose);
-    //返回值[0,2PI)
+    //返回值(-PI,PI]
     static float vector2yaw(float x,float y);
 
     static std::vector<Pose2d> generate_rectangle_vertexs(const Pose2d& pose,float w,float h){

MPC/trajectory.cpp

@@ -209,13 +209,13 @@ Trajectory Trajectory::create_line_trajectory(const Pose2d& start,const Pose2d&
 
   Trajectory trajectory;
   double angle =gen_axis_angle(start,end);
-
+  trajectory.push_point(Pose2d(start.x(),start.y(),angle));
   int point_count=int(Pose2d::distance(start,end)/dt);
   //生成离散点,间距
   Pose2d diff=end-start;
   float solution_x=diff.x()/float(point_count);
   float solution_y=diff.y()/float(point_count);
-  for(int i=0;i<point_count;++i)
+  for(int i=1;i<point_count;++i)
   {
     double x=start.x()+i*solution_x;
     double y=start.y()+i*solution_y;

config/webots/navigation.prototxt

@@ -4,7 +4,7 @@ main_agv:false
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"192.168.225.224"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"
@@ -14,7 +14,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-child-nav"
-        ip:"192.168.225.224"
+        ip:"127.0.0.1"
         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:"192.168.225.224"
+	ip:"127.0.0.1"
 	port:1883
 	subBrotherStatuTopic:"lidar_odom/main"
 }

config/webots/navigation_main.prototxt

@@ -3,7 +3,7 @@ main_agv:true
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"192.168.225.224"
+	ip:"127.0.0.1"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -13,7 +13,7 @@ Agv_emqx
 Terminal_emqx
 {
 	NodeId:"agv-main-nav"
-	ip:"192.168.225.224"
+	ip:"127.0.0.1"
 	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:"192.168.225.224"
+	ip:"127.0.0.1"
 	port:1883
 	subBrotherStatuTopic:"lidar_odom/child"
 }