完成原动作兼容，横移巡线待测试

MPC/loaded_mpc.cpp

@@ -144,8 +144,8 @@ LoadedMPC::LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity){
 }
 LoadedMPC::~LoadedMPC(){}
 
-bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
-                                         std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory)
+bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,float r,std::vector<double>& out,
+                                         Trajectory& select_traj,Trajectory& optimize_trajectory)
 {
   auto start=std::chrono::steady_clock::now();
   // State vector holds all current values neede for vars below
@@ -155,7 +155,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
 
   //纠正角速度/线速度，使其满足最小转弯半径
   double angular=wmg;
-  double radius=20.0*1.3 * (1.0/sqrt(line_velocity*line_velocity+1e-10));
+  double radius=r * (1.0/sqrt(line_velocity*line_velocity+1e-10));
   if( (line_velocity*line_velocity)/(wmg*wmg+1e-9) < (radius*radius)) {
     angular = fabs(line_velocity) / radius;
     if (wmg < 0) angular = -angular;
@@ -393,6 +393,22 @@ bool LoadedMPC::filte_Path(const Pose2d& point,Pose2d target,Trajectory trajecto
       poses.push_back(pose);
     }
   }
+  int size=poses.size();
+  if(size<point_num)
+  {
+    //一个点都没有，则以当前点开始，反向取点
+    float dl=-0.1;
+    Pose2d last=point;
+    if(size>=1){
+      //从最后点到最后一点均匀选取
+      last=poses[size-1]; //
+      dl=0.1;
+    }
+    float dx=cos(last.theta())*dl;
+    float dy=sin(last.theta())*dl;
+    for(int i=1;i<point_num-size+1;++i)
+      poses.push_back(Pose2d(last.x()+dx*i,last.y()+dy*i,last.theta()));
+  }
   return true;
 }
 

MPC/loaded_mpc.h

@@ -13,7 +13,7 @@ class LoadedMPC
  public:
     LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity);
     virtual ~LoadedMPC();
-    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,
+    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,float r,
                        std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
 
  protected:

MPC/navigation.cpp

@@ -172,20 +172,9 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
     statu.set_statu(actionType_); //
     //printf(" statu:%d\n",actionType_);
     statu.set_move_mode(move_mode_);
-    NavMessage::Action *current_action = nullptr;
     if (running_)
     {
       statu.set_key(global_navCmd_.key());
-      /*std::queue<NavMessage::Action> actios = unfinished_cations_;
-      while (actios.empty() == false) {
-        //保存当前动作
-        NavMessage::Action *act = statu.add_unfinished_actions();
-        current_action = new NavMessage::Action(*act);
-
-        act->CopyFrom(actios.front());
-        actios.pop();
-
-      }*/
     }
     //发布MPC信息
       //发布mpc 预选点
@@ -266,7 +255,7 @@ void Navigation::ResetPose(const Pose2d& pose)
 
 bool Navigation::Start(const NavMessage::NavCmd& cmd)
 {
-  if(unfinished_cations_.empty()==false)  //正在运行中，上一次指令未完成
+  if(newUnfinished_cations_.empty()==false)  //正在运行中，上一次指令未完成
   {
     if(pause_)
     {
@@ -285,14 +274,9 @@ bool Navigation::Start(const NavMessage::NavCmd& cmd)
 
     //add 先检查当前点与起点的距离
     global_navCmd_=cmd;
-  if(cmd.action()==6)
-  {
+
     for (int i = 0; i < cmd.newactions_size(); ++i)
       newUnfinished_cations_.push(cmd.newactions(i));
-  }else {
-    for (int i = 0; i < cmd.actions_size(); ++i)
-      unfinished_cations_.push(cmd.actions(i));
-  }
     thread_=new std::thread(&Navigation::navigatting,this);
     return true;
 
@@ -420,7 +404,9 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       break;
     }
     //判断是否到达目标点
-    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,anyDitect)) {
+    int direct=0;
+    if(anyDitect) direct=2;
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,direct)) {
       if(Stop()) {
         printf("adjust completed anyDitect :%d\n",anyDitect);
         return true;
@@ -451,29 +437,31 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       double acc_angular=20.0*M_PI/180.0;
       double dt=0.1;
       if (action == 1) {
-        if (Pose2d::abs(diff).theta() > thresh.theta()) {
-          float diff_yaw=diff.theta();
-          if(anyDitect==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;
+        float minYawDiff=diff.theta();
+        if(anyDitect){
+          Pose2d p0=timedPose_.Get();
+          float yawdiff[4]={0};
+          yawdiff[0]=diff.theta();
+          yawdiff[1]=Pose2d::relativePose(target,p0-Pose2d(0,0,M_PI/2)).theta();  ////使用减法，保证角度在【-pi pi】
+          yawdiff[2]=Pose2d::relativePose(target,p0-Pose2d(0,0,M_PI)).theta();
+          yawdiff[3]=Pose2d::relativePose(target,p0-Pose2d(0,0,3*M_PI/2)).theta();
+          for(int i=1;i<4;++i){
+            if(fabs(yawdiff[i])<fabs(minYawDiff)){
+              minYawDiff=yawdiff[i];
             }
           }
-          double theta = limit_gause(diff_yaw,limit_rotate.min,limit_rotate.max);
+
+        }
+        if (fabs(minYawDiff) > thresh.theta()) {
+
+          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,diff_yaw,anyDitect);
+                 timedA_.Get(),angular,limit_angular,minYawDiff,anyDitect);
           continue;
         }
       }
@@ -569,6 +557,75 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
   return false;
 }
 
+bool Navigation::execute_InOut_space(NavMessage::NewAction action)
+{
+  if(action.type()!=1 && action.type()!=2){
+    printf(" Inout_space failed : msg action type must ==1\n ");
+    return false;
+  }
+  if(!action.has_inoutvlimit() || !action.has_spacenode()||
+     !action.has_streetnode()) {
+    std::cout << "execute enter_space failed ,Action miss some infos:" << action.DebugString() << std::endl;
+    return false;
+  }
+  if(timedPose_.timeout()==true){
+    printf(" inout_space failed type:%d : current pose is timeout\n",action.type());
+    return false;
+  }
+  Pose2d begin,target,begin_accuracy,target_accuracy;
+  if(action.type()==1)  //入库,起点为streetNode
+  {
+    begin = Pose2d(action.streetnode().pose().x(), action.streetnode().pose().y(), action.streetnode().pose().theta());
+    begin_accuracy=Pose2d(action.streetnode().accuracy().x(),
+                                 action.streetnode().accuracy().y(),
+                                 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());
+  }else{  //出库起点终点对调
+    Pose2d mid=target;
+    Pose2d mid_accuracy=target_accuracy;
+    target=begin;
+    target_accuracy=begin_accuracy;
+    begin=mid;
+    begin_accuracy=mid_accuracy;
+  }
+  //给起点赋予朝向,保证agv以正对车位的方向行进
+  double yaw=Pose2d::vector2yaw(target.x()-begin.x(),target.y()-begin.y());
+  if(action.type()==2)  //出库
+    yaw=Pose2d::vector2yaw(begin.x()-target.x(),begin.y()-target.y());
+  begin.mutable_theta()=yaw;
+
+  //严格调整到起点
+  stLimit adjust_x={ 0.05,0.2};
+  stLimit adjust_h={ 0.05,0.2};
+  stLimit adjust_angular={ 2*M_PI/180.0, 20*M_PI/180.0 };
+  stLimit mpc_velocity={0.05,0.3 };
+
+  //判断当前点与起点的距离
+  Pose2d distance(0.5,0.5,M_PI*2);
+  if(action.type()==2) {  //出库要求起点距离当前点非常近
+    distance = Pose2d(0.05, 0.05, 1 * M_PI / 180.0);
+    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) ){
+    printf(" Inout space failed type:%d: current pose too far from begin node\n",action.type());
+    return false;
+  }
+
+  if(execute_adjust_action(begin,begin_accuracy,adjust_x,adjust_h,adjust_angular,false))
+    return false;
+
+  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");
@@ -607,7 +664,9 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   }
   bool ret = false;
   LoadedMPC MPC(relative, limit_v.min, limit_v.max);
-  ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
+  float r=20.0*1.3; //最小转弯半径
+  if(directY) r=3*2.45;
+  ret = MPC.solve(traj, traj[traj.size() - 1], statu,r, out, selected_trajectory, optimize_trajectory);
 
   //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
   predict_traj_.reset(optimize_trajectory, 1);
@@ -616,32 +675,40 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
 }
 
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
-                           const Pose2d& target,const Pose2d& diff,bool nearest_yaw)
+                           const Pose2d& target,const Pose2d& diff,int direct)
 {
   //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);
-  if(nearest_yaw==false) {
-    if (absDiff < diff) {
-      if (fabs(velocity) < 0.05 && fabs(angular) < 10 * M_PI / 180.0)
+
+  std::cout<<" arrive distanc:"<<distance<<",direct:"<<direct<<std::endl;
+  if(direct==1){ //纵向MPC
+    if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
+    {
+      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.03 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
-  }else{
-    if(absDiff.x()<diff.x() && absDiff.y()<diff.x())
+  }else if(direct==2){    //横向MPC
+    if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
     {
-      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)
+      float diffYaw=absDiff.theta();  //[0-pi)
+      float diffYaw1=absDiff.theta()-M_PI/2;
+      float diffYaw2=absDiff.theta()-M_PI;
+      float diffYaw3=absDiff.theta()-3*M_PI/2;
+      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)
+        return true;
+    }
+  }else{  //原地调整
+    if (absDiff < diff) {
+      if (fabs(velocity) < 0.03 && fabs(angular) < 5 * M_PI / 180.0)
         return true;
     }
-
   }
   //std::cout<<"distance:"<<distance<<"  diff:"<<diff<<"  v:"<<velocity<<"  a:"<<angular<<std::endl;
     return false;
@@ -657,6 +724,7 @@ void Navigation::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionT
 bool Navigation::clamp_close()
 {
   if(monitor_) {
+    printf("Clamp closing...\n");
     monitor_->clamp_close(move_mode_);
     actionType_=eClampClose;
     while (cancel_ == false) {
@@ -676,6 +744,7 @@ bool Navigation::clamp_close()
 }
 bool Navigation::clamp_open() {
   if(monitor_) {
+    printf("Clamp openning...\n");
     monitor_->clamp_open(move_mode_);
     actionType_=eClampOpen;
     while(cancel_==false)
@@ -714,8 +783,12 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
   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;
+    if( fabs(target_relative.y())>fabs(target_relative.x())) {  //目标轨迹点在当前点Y轴上
+      std::cout<<" MPC Y axis  target_relative:"<<target_relative<<std::endl;
+      directY = true;
+    }else{
+      std::cout<<" MPC X axis  target_relative:"<<target_relative<<std::endl;
+    }
   }
   printf(" exec along autoDirect:%d\n",autoDirect);
   while (cancel_ == false) {
@@ -729,7 +802,9 @@ 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,directY)) {
+    /*int direct=1;
+    if(directY) direct=2;*/
+    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,2)) {
       if (Stop()) {
         printf(" exec along completed !!!\n");
         return true;
@@ -780,7 +855,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     /*
      * 判断车辆是否在终点附近徘徊，无法到达终点
      */
-    if (mpc_possible_to_end(target, target_diff) == false) {
+    if (mpc_possible_to_end(target, target_diff,directY) == false) {
       printf(" --------------MPC  imposible to target -----------------------\n");
       Stop();
       //调整到目标点
@@ -802,20 +877,32 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
   return false;
 }
 
-bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff)
+bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff,bool directY)
 {
   if(timedPose_.timeout()==false) {
-    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
+    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 diff = Pose2d::relativePose(target, current);
     if (fabs(diff.y()) < target_diff.y() && fabs(diff.x()) < target_diff.x()) { //xy在精度内
       if (timedV_.timeout() == true)
         return false;
-      if(fabs(timedV_.Get())<0.05 && fabs(diff.theta())>target_diff.theta())
-        return false;
+      /*if(fabs(timedV_.Get())<0.05 && fabs(diff.theta())>target_diff.theta())
+        return false;*/
       return true;
     } else {  //xy不在精度内
-      if (fabs(diff.x()) < 1e-3)
-        return false; //x已到达，Y未到达，则不可能到达目标点
+      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;
@@ -831,138 +918,73 @@ bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_d
   return false;
 }
 
-void Navigation::navigatting()
-{
-  if(inited_==false) {
+void Navigation::navigatting() {
+  if (inited_ == false) {
     printf(" navigation has not inited\n");
-    return ;
+    return;
   }
-  pause_=false;
-  cancel_=false;
-  running_=true;
-  actionType_=eReady;
+  pause_ = false;
+  cancel_ = false;
+  running_ = true;
+  actionType_ = eReady;
   printf(" navigation beg...\n");
 
-  if(global_navCmd_.action()==6)
-  {
-    printf("exec new nav cmd ...\n");
-
-    while(newUnfinished_cations_.empty()==false && cancel_==false)
-    {
-      std::cout<<"unfinished size:"<<newUnfinished_cations_.size()<<std::endl;
-      NavMessage::NewAction act=newUnfinished_cations_.front();
-      if(act.type()==5)  {//夹持
-        if(this->clamp_close()==false){
-          printf("夹持failed ...\n");
-          break;
-        }
-      }else if(act.type()==6){
-        if(this->clamp_open()==false){
-          printf("打开夹持 failed...\n");
-          break;
-        }
-      }else if(act.type()==3 || act.type()==4){
-        if(!execute_nodes(act))
-          break;
-      }else{
-        printf(" action type 1/2 not handled !!\n");
-      }
-      newUnfinished_cations_.pop();
-    }
-    actionType_=eReady;
-    Stop();
-    if(cancel_==true) {
-      printf(" navigation canceled\n");
-      while(newUnfinished_cations_.empty()==false)
-        newUnfinished_cations_.pop();
-    }else {
-      if (newUnfinished_cations_.empty())
-        printf("navigation completed!!!\n");
-      else{
-        printf(" navigation Failed\n");
-        while(newUnfinished_cations_.empty()==false)
-          newUnfinished_cations_.pop();
-      }
-    }
-    running_=false;
-    return ;
-  }
-
-  //---------------------
-  while(unfinished_cations_.empty()==false && cancel_==false)
-  {
-    NavMessage::Action act=unfinished_cations_.front();
-    Pose2d target(act.target().x(),act.target().y(),act.target().theta());
-    Pose2d target_diff(act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
-    if(act.type()==1)
-    {
-      printf(" adjust to (%.3f %.3f %.3f) diff:(%.3f,%.3f,%.3f)\n",
-             act.target().x(),act.target().y(),act.target().theta(),
-             act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
-      stLimit limit_angular,limit_velocity,limint_horize;
-      limit_velocity.min=act.velocity_limit().min();
-      limit_velocity.max=act.velocity_limit().max();
-      limit_angular.min=act.angular_limit().min();
-      limit_angular.max=act.angular_limit().max();
-      limint_horize.min=act.horize_limit().min();
-      limint_horize.max=act.horize_limit().max();
-      if(execute_adjust_action(target,target_diff,limit_velocity,limint_horize,limit_angular,false)==false)
-      {
-        printf("execute adjust action failed\n");
+  while (newUnfinished_cations_.empty() == false && cancel_ == false) {
+    std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
+    NavMessage::NewAction act = newUnfinished_cations_.front();
+    if(act.type()==1){  //入库
+      if(!execute_InOut_space(act)){
+        printf(" In space failed\n");
         break;
       }
     }
-    else if(act.type()==2)
-    {
-      Pose2d begin(act.begin().x(),act.begin().y(),act.begin().theta());
-      stLimit limit_velocity;
-      limit_velocity.min=act.velocity_limit().min();
-      limit_velocity.max=act.velocity_limit().max();
-      printf(" MPC to (%.3f %.3f %.3f) diff:(%.3f,%.3f,%.3f)\n",
-             act.target().x(),act.target().y(),act.target().theta(),
-             act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
-      if(execute_along_action(begin,target,target_diff,limit_velocity,false)==false) {
-        printf("execute along action failed\n");
+    else if(act.type()==2){  //出库
+      if(!execute_InOut_space(act)){
+        printf(" out space failed\n");
         break;
       }
-    }
-    else if(act.type()==3)  //夹持
-    {
-      if(this->clamp_close()==false)
-      {
+    }else if (act.type() == 3 || act.type() == 4) { //马路导航
+      if (!execute_nodes(act))
+        break;
+    }else if(act.type()==5){
+      printf(" 汽车模型导航....\n");
+
+    }else if (act.type() == 6) {//夹持
+      if (this->clamp_close() == false) {
         printf("夹持failed ...\n");
         break;
       }
-    }
-    else if(act.type()==4)
-    {
-      if(this->clamp_open()==false)
-      {
+    } else if (act.type() == 7) {
+      if (this->clamp_open() == false) {
         printf("打开夹持 failed...\n");
         break;
       }
-    }
-    else
-    {
-      printf(" unknow action type:%d\n",act.type());
+    } else if(act.type()==8){ //切换模式
+      SwitchMode(Monitor_emqx::ActionMode(act.changedmode()),act.wheelbase());
+    }else{
+      printf(" action type invalid not handled !!\n");
       break;
     }
-    unfinished_cations_.pop();
+    newUnfinished_cations_.pop();
   }
-  actionType_=eReady;
+  actionType_ = eReady;
   Stop();
-  if(cancel_==true) {
+  if (cancel_ == true) {
     printf(" navigation canceled\n");
-    while(unfinished_cations_.empty()==false)
-      unfinished_cations_.pop();
-  }else {
-    if (unfinished_cations_.empty())
+    while (newUnfinished_cations_.empty() == false)
+      newUnfinished_cations_.pop();
+  } else {
+    if (newUnfinished_cations_.empty())
       printf("navigation completed!!!\n");
-    else{
+    else {
       printf(" navigation Failed\n");
-      while(unfinished_cations_.empty()==false)
-        unfinished_cations_.pop();
+      while (newUnfinished_cations_.empty() == false)
+        newUnfinished_cations_.pop();
     }
   }
-  running_=false;
+  running_ = false;
+}
+
+void Navigation::SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
+    printf("Child AGV can not Switch Mode\n");
 }

MPC/navigation.h

@@ -61,6 +61,7 @@ public:
     void Cancel();
     void Pause();
     bool Stop();
+    virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase);
 
 
  protected:
@@ -74,8 +75,11 @@ public:
     static void NavCmdCallback(const MqttMsg& msg,void* context);
     static void BrotherAgvStatuCallback(const MqttMsg& msg,void* context);
 
+    /*
+     * direct : 0:严格朝向一致，1:x轴一致 2:xy轴任意朝向一致
+     */
     static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,
-                          const Pose2d& diff,bool nearest_yaw=false);
+                          const Pose2d& diff,int direct);
     /*
      * autoDirect:自动选择 纵向还是横向巡线
      */
@@ -90,10 +94,15 @@ public:
      * 按照路径点导航
      */
     bool execute_nodes(NavMessage::NewAction action);
+    /*
+     * 进库
+     */
+    bool execute_InOut_space(NavMessage::NewAction action);
+
     virtual bool clamp_close();
     virtual bool clamp_open();
     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);
+    bool mpc_possible_to_end(const Pose2d& target,const Pose2d& diff,bool directY);
     void navigatting();
 
     virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
@@ -124,7 +133,6 @@ public:
     TimedLockData<double> timedBrotherA_;
 
     NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
-    std::queue<NavMessage::Action> unfinished_cations_; //未完成的动作
     std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
     std::thread* thread_= nullptr;
     bool running_=false;

MPC/navigation_main.cpp

@@ -95,9 +95,8 @@ void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::Act
 
 bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
 {
-  NavMessage::RobotStatu agvStatu;
-  if(Navigation::CreateRobotStatuMsg(agvStatu)) {
-    agvStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
+  if(Navigation::CreateRobotStatuMsg(robotStatu)) {
+    robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
     return true;
   }
   //std::cout<<agvStatu.DebugString()<<std::endl;

controller.cpp

@@ -37,8 +37,11 @@ int main(int argc,char* argv[])
     return -1;
   }
 
-  while(1)
+
+  printf(" input q to quite\n");
+  while(getchar()!='q')
   {
+    printf(" input q to quite\n");
     usleep(1000*500);
   }
   delete g_navigator;

message.proto

@@ -50,25 +50,13 @@ message Trajectory
   repeated Pose2d poses=1;
 }
 
-message Action
-{
-  int32 type = 1; // 1:原地调整，2：巡线,3,夹持，4松夹持
-  Pose2d begin = 2;
-  Pose2d target = 3;
-  Pose2d target_diff = 4;
-  SpeedLimit velocity_limit=5;
-  SpeedLimit angular_limit=6;
-  SpeedLimit horize_limit=7;
-}
-
-
 //----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
-  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，5，夹持，6，松夹持，7，切换模式
-  PathNode begNode = 2; //进出库起点
+  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6:夹持，7:松夹持，8:切换模式
+  PathNode spaceNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
-  PathNode targetNode = 4; //进出库终点
+  PathNode streetNode = 4; //进出库终点
   repeated PathNode pathNodes=5;//导航路径点
   SpeedLimit InOutVLimit=6; //进出库速度
   SpeedLimit NodeVelocityLimit=7; //马路点MPC速度限制
@@ -82,10 +70,8 @@ message NewAction //进库，出库，轨迹导航，夹持，松夹持
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,6,新导航模式
+  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
   string key=2;
-  float wheelbase=3;		//轴距
-  repeated Action actions=4;
   repeated NewAction newActions=5;
 }
 
@@ -95,7 +81,6 @@ message NavStatu
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，1：single，2：双车
   string key = 4; // 任务唯一码
-  repeated Action unfinished_actions = 5;  //未完成的动作，第一个即为当前动作
   Trajectory selected_traj = 6;
   Trajectory predict_traj = 7;