新增自动选择MPC方向，（未完待续）

MPC/loaded_mpc.cpp

@@ -139,7 +139,7 @@ LoadedMPC::LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity){
 }
 LoadedMPC::~LoadedMPC(){}
 
-bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
+bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
                                          std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory)
 {
   auto start=std::chrono::steady_clock::now();
@@ -356,7 +356,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   return true;
 }
 
-bool LoadedMPC::filte_Path(const Pose2d& point,const Pose2d& target,const Trajectory& trajectory,
+bool LoadedMPC::filte_Path(const Pose2d& point,Pose2d target,Trajectory trajectory,
                             std::vector<Pose2d>& poses,int point_num)
 {
   double gradient=0;
@@ -412,4 +412,37 @@ Eigen::VectorXd LoadedMPC::fit_path(const std::vector<Pose2d>& trajectory)
   auto Q = A.householderQr();
   auto result = Q.solve(yvals);
   return result;
-}
+}
+
+
+MPC_Y::MPC_Y(const Pose2d& obs,double min_velocity,double max_velocity)
+  :LoadedMPC(obs,min_velocity,max_velocity) {
+}
+MPC_Y::~MPC_Y(){}
+bool MPC_Y::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
+                   std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory){
+  //将轨迹点，绕当前点旋转-pi/2 即可
+  Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
+  Trajectory newTraj;
+  for(int i=0;i<trajectory.size();++i)
+  {
+    Pose2d rotated=trajectory[i].rotate(pose_agv.x(),pose_agv.y(),-M_PI/2.0);
+    newTraj.push_point(rotated);
+  }
+  Pose2d newTaget=target.rotate(pose_agv.x(),pose_agv.y(),-M_PI/2.0);
+  if(LoadedMPC::solve(newTraj,newTaget,statu,out,select_traj,optimize_trajectory))
+  {
+    //将选中的轨迹和优化预测轨迹绕当前点旋转回来
+    for(int i=0;i<select_traj.size();++i)
+    {
+      select_traj[i]=select_traj[i].rotate(pose_agv.x(),pose_agv.y(),M_PI/2.0);
+    }
+    for(int i=0;i<optimize_trajectory.size();++i)
+    {
+      optimize_trajectory[i]=optimize_trajectory[i].rotate(pose_agv.x(),pose_agv.y(),M_PI/2.0);
+    }
+    return true;
+  }
+  return false;
+}
+

MPC/loaded_mpc.h

@@ -12,8 +12,8 @@ class LoadedMPC
 {
  public:
     LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity);
-    ~LoadedMPC();
-    virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
+    virtual ~LoadedMPC();
+    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
                        std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
 
  protected:
@@ -21,8 +21,8 @@ class LoadedMPC
      * 根据当前点和轨迹，选择前方 point_num
      * point:当前点，target：目标点
      */
-    static bool filte_Path(const Pose2d& point,const Pose2d& target,
-                           const Trajectory& trajectory,std::vector<Pose2d>& poses,int point_num=10);
+    static bool filte_Path(const Pose2d& point,Pose2d target,
+                           Trajectory trajectory,std::vector<Pose2d>& poses,int point_num=10);
     /*
      * 根据路径点，拟合曲线
      */
@@ -33,5 +33,15 @@ class LoadedMPC
 
 };
 
+class MPC_Y:public LoadedMPC
+{
+public:
+    MPC_Y(const Pose2d& obs,double min_velocity,double max_velocity);
+    virtual ~MPC_Y();
+    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
+                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
+
+protected:
+};
 
 #endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_

MPC/navigation.cpp

@@ -389,7 +389,7 @@ void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
 
 
 bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
-                                    stLimit limit_v,stLimit limit_h,stLimit limit_rotate)
+                                    stLimit limit_v,stLimit limit_h,stLimit limit_rotate,bool nearest_yaw)
 {
   std::cout<<" adjust tarhet:"<<target<<"  diff:"<<target_diff<<std::endl;
   if(inited_==false) {
@@ -411,9 +411,9 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       break;
     }
     //判断是否到达目标点
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,nearest_yaw)) {
       if(Stop()) {
-        printf("adjust completed\n");
+        printf("adjust completed nearest yaw:%d\n",nearest_yaw);
         return true;
       }
       return false;
@@ -443,13 +443,28 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       double dt=0.1;
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
-          double theta = limit_gause(diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
+          float diff_yaw=diff.theta();
+          if(nearest_yaw==true)
+          {
+            int n=int(diff_yaw/(M_PI/4.0));
+            if (n%2==1)
+            {
+              if(n>0)
+                diff_yaw=diff_yaw-float((n+1)/2)*M_PI/2.0;
+              else
+                diff_yaw=diff_yaw-float((n-1)/2)*M_PI/2.0;
+            }else{
+              diff_yaw=diff_yaw-float(n/2)*M_PI/2.0;
+            }
+          }
+          double theta = limit_gause(diff_yaw,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
           double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
           double limit_angular=limit(angular,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
 
           SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, limit_angular);
           actionType_=eRotation;
-          printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f\n",timedA_.Get(),angular,limit_angular,theta);
+          printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f NearestYaw:%d\n",
+                 timedA_.Get(),angular,limit_angular,theta,nearest_yaw);
           continue;
         }
       }
@@ -483,7 +498,50 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
 
 }
 
-bool Navigation::mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out)
+
+bool Navigation::execute_nodes(NavMessage::NewAction action)
+{
+  if(action.type()==3 || action.type()==4) //最优动作导航 or 导航中保证朝向严格一致
+  {
+    if(!action.has_nodevelocitylimit() || !action.has_nodeangularlimit()||
+        !action.has_adjustvelocitylimit()|| !action.has_adjusthorizonlimit()
+        || action.pathnodes_size()==0) {
+      std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
+      return false;
+    }
+    bool autoDirect=(action.type()==3);
+    //原地调整
+    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() };
+    //目标，精度
+    Pose2d target(first.pose().x(),first.pose().y(),first.pose().theta());
+    Pose2d accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
+    execute_adjust_action(target,accuracy,adjust_x,adjust_h,adjust_angular,autoDirect);
+    NavMessage::PathNode last_node=first;
+
+    for(int i=1;i< action.pathnodes_size();++i)
+    {
+      NavMessage::PathNode node=action.pathnodes(i);
+      /// 巡线到目标点，判断巡线方向
+      Pose2d begin(last_node.pose().x(),last_node.pose().y(),last_node.pose().theta());
+      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(begin,target,accuracy,mpc_velocity,autoDirect)==false)
+        return false;
+      last_node=node;
+    }
+    return true;
+  }
+  printf("execute PathNode failed ,action type is not 3/4  :%d\n",action.type());
+  return false;
+}
+
+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)
   {
@@ -504,7 +562,7 @@ bool Navigation::mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<dou
   Pose2d relative=Pose2d::relativePose(brother,pose);//计算另一节在当前小车坐标系下的坐标
   if(move_mode_==Monitor_emqx::ActionMode::eMain)
     relative=Pose2d(-1e8,-1e8,0);
-  LoadedMPC MPC(relative,limit_v.min, limit_v.max);
+
   Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
 
   statu[0]=pose.x();
@@ -515,21 +573,47 @@ bool Navigation::mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<dou
 
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
-  bool ret= MPC.solve(traj,traj[traj.size()-1],statu,out,selected_trajectory,optimize_trajectory);
+
+  bool ret=false;
+  if(directY==false) {
+    LoadedMPC MPC(relative, limit_v.min, limit_v.max);
+    ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
+  }else{
+    MPC_Y MPC(relative, limit_v.min, limit_v.max);
+    ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
+  }
   predict_traj_.reset(optimize_trajectory,1);
   selected_traj_.reset(selected_trajectory,1);
   return ret;
 }
 
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
-                           const Pose2d& target,const Pose2d& diff)
+                           const Pose2d& target,const Pose2d& diff,bool nearest_yaw)
 {
   Pose2d distance=Pose2d::relativePose(target,cur);
-    if(Pose2d::abs(distance)<diff)
+  Pose2d absDiff=Pose2d::abs(distance);
+  if(nearest_yaw==false) {
+    if (absDiff < diff) {
+      if (fabs(velocity) < 0.05 && fabs(angular) < 10 * M_PI / 180.0)
+        return true;
+    }
+  }else{
+    if(absDiff.x()<diff.x() && absDiff.y()<diff.x())
     {
-        if(fabs(velocity)<0.05 && fabs(angular)< 10*M_PI/180.0)
-            return true;
+      double theta=absDiff.theta();
+      int n=int(theta/(M_PI/4.0));
+      if (n%2==1)
+      {
+        theta=float((n+1)/2)*M_PI/2.0-theta;
+      }else{
+        theta=theta-float(n/2)*M_PI/2.0;
+      }
+
+      if(theta<diff.theta() && fabs(velocity) < 0.05 && fabs(angular) < 10 * M_PI / 180.0)
+        return true;
     }
+
+  }
   //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
     return false;
 }
@@ -583,7 +667,8 @@ bool Navigation::clamp_open() {
   return false;
 }
 
-bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v) {
+bool 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;
@@ -596,6 +681,13 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
   Trajectory traj = Trajectory::create_line_trajectory(begin, target);
   if (traj.size() == 0)
     return true;
+  //判断 巡线方向
+  bool directY=false;
+  if(autoDirect){
+    Pose2d target_relative=Pose2d::relativePose(target,timedPose_.Get());
+    if( fabs(target_relative.y()>fabs(target_relative.x())))  //目标轨迹点在当前点Y轴上
+      directY=true;
+  }
 
   while (cancel_ == false) {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -608,7 +700,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       return false;
     }
     //判断是否到达终点
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff)) {
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,directY)) {
       if (Stop()) {
         printf(" exec along completed !!!\n");
         return true;
@@ -645,7 +737,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     std::vector<double> out;
     bool ret;
     TimerRecord::Execute([&, this]() {
-        ret = mpc_once(traj, limit_v, out);
+        ret = mpc_once(traj, limit_v, out,directY);
     }, "mpc_once");
     if (ret == false) {
       Stop();

MPC/navigation.h

@@ -75,13 +75,24 @@ public:
     static void BrotherAgvStatuCallback(const MqttMsg& msg,void* context);
 
     static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,
-                          const Pose2d& diff);
-    bool execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v);
-    bool execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
-                            stLimit limit_v,stLimit limit_h,stLimit limit_rotate);
+                          const Pose2d& diff,bool nearest_yaw=false);
+    /*
+     * autoDirect:自动选择 纵向还是横向巡线
+     */
+    bool execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,
+                              stLimit limit_v,bool autoDirect=false);
+    /*
+     * 调整到目标点，nearest_yaw:xy轴线任一朝向对准目标朝向即可
+     */
+    bool execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
+                            stLimit limit_h,stLimit limit_rotate,bool nearest_yaw=false);
+    /*
+     * 按照路径点导航
+     */
+    bool execute_nodes(NavMessage::NewAction action);
     virtual bool clamp_close();
     virtual bool clamp_open();
-    bool mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out);
+    bool mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY=false);
     bool mpc_possible_to_end(const Pose2d& target,const Pose2d& diff);
     void navigatting();
 

MPC/pose2d.cpp

@@ -102,3 +102,10 @@ Pose2d Pose2d::rotate(float theta)const
 
   return Pose2d(x,y,new_theta);
 }
+
+Pose2d Pose2d::rotate(float ox,float oy,float theta)const{
+  Pose2d translate(x()-ox,y()-oy,theta);//平移
+  Pose2d rotated=rotate(theta);
+  Pose2d ret(rotated.x()+x(),rotated.y()+y(),rotated.theta());
+  return ret;
+}

MPC/pose2d.h

@@ -96,6 +96,8 @@ class Pose2d
      * 将点顺时针旋转theta
      */
     Pose2d rotate(float theta)const;
+    //绕xy旋转
+    Pose2d rotate(float ox,float oy,float theta)const;
     friend std::ostream& operator<<(std::ostream &out,const Pose2d& point);
 
 

MPC/trajectory.cpp

@@ -14,7 +14,7 @@ Trajectory::Trajectory(const std::vector<Pose2d>& points)
   m_pose_vector=points;
 }
 
-Pose2d Trajectory::operator[](int index) const
+Pose2d& Trajectory::operator[](int index)
 {
   return m_pose_vector[index];
 }

MPC/trajectory.h

@@ -17,7 +17,7 @@ class Trajectory
     Trajectory(const std::vector<Pose2d>& points);
     Trajectory(const Trajectory& other)= default;
     Trajectory& operator=(const Trajectory& other)= default;
-    Pose2d operator[](int index) const;
+    Pose2d& operator[](int index);
     Trajectory& operator+(const Trajectory& other);
     unsigned int size()const{return m_pose_vector.size();}
     void clear(){m_pose_vector.clear();}

message.proto

@@ -34,9 +34,15 @@ message SpeedLimit
 
 message Pose2d
 {
-    float x=1;
-    float y=2;
-    float theta=3;
+  float x=1;
+  float y=2;
+  float theta=3;
+}
+
+message PathNode  //导航路径点及精度
+{
+  Pose2d pose=1;    //路径点
+  Pose2d accuracy=2; //要求精度
 }
 
 message Trajectory
@@ -50,9 +56,24 @@ message Action
   Pose2d begin = 2;
   Pose2d target = 3;
   Pose2d target_diff = 4;
-    SpeedLimit velocity_limit=5;
-    SpeedLimit angular_limit=6;
-    SpeedLimit horize_limit=7;
+  SpeedLimit velocity_limit=5;
+  SpeedLimit angular_limit=6;
+  SpeedLimit horize_limit=7;
+}
+
+message NewAction //进库，出库，轨迹导航，夹持，松夹持
+{
+  int32 type =1; //1,进库，2，出库，3，马路最优动作导航，4，马路严格保证agv朝前导航，5，夹持，6，松夹持，7，切换模式
+  PathNode begNode = 2; //进出库起点
+  PathNode passNode=3; //进出库途径点
+  PathNode targetNode = 4; //进出库终点
+  repeated PathNode pathNodes=5;//导航路径点
+  SpeedLimit InOutVLimit=6; //进出库速度
+  SpeedLimit NodeVelocityLimit=7; //马路点MPC速度限制
+  SpeedLimit NodeAngularLimit=8;  //马路点原地旋转速度限制
+  SpeedLimit adjustVelocitylimit=9; //马路点原地调整x速度
+  SpeedLimit adjustHorizonLimit=10;  //马路点原地横移速度限制
+  float wheelbase=11;		//切换模式，轴距信息
 }