AGV/MPC/navigation.cpp

//
// Created by zx on 22-12-2.
//

#include "navigation.h"
#include "loaded_mpc.h"
#include "../define/TimerRecord.h"

Navigation::~Navigation()
{
  exit_=true;
  if(terminator_)
    delete terminator_;
  if(brotherEmqx_)
    delete brotherEmqx_;
  if(monitor_)
    delete monitor_;
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
      thread_->join();
    delete thread_;
  }
  if(pubthread_!= nullptr)
  {
    if(pubthread_->joinable())
      pubthread_->join();
    delete pubthread_;
  }
}

/*
 * 高斯函数压缩，x=3
 */
double limit_gause(double x,double min,double max){
  double r=x>=0?1.0:-1.0;
  double delta=max/1.0;
  return (max-(max-min)*exp(-x*x/(delta*delta)))*r;

}

double limit(double x,double min,double max){
  double ret=x;
  if(x>=0){
    if(x>max) ret=max;
    if(x<min) ret=min;
  }else{
    if(x<-max) ret=-max;
    if(x>-min) ret=-min;
  }
  return ret;

}

double next_speed(double speed,double target_speed,double acc,double dt=0.1)
{
  if(fabs(target_speed-speed)/dt<acc)
  {
    return target_speed;
  }
  else
  {
    double r=target_speed-speed>=0.?1.0:-1.0;
    return speed+r*acc*dt;
  }
}



bool Navigation::Init(const Navigation_parameter& parameter)
{
  parameter_=parameter;
  AgvEmqx_parameter agv_p=parameter.agv_emqx();
  if(monitor_== nullptr) {
    monitor_ = new Monitor_emqx(agv_p.nodeid());
    if(monitor_->Connect(agv_p.ip(),agv_p.port())==false)
    {
      printf(" agv emqx connected failed\n");
      return false;
    }
    monitor_->set_speedcmd_topic(agv_p.pubspeedtopic());
    monitor_->AddCallback(agv_p.subposetopic(),Navigation::RobotPoseCallback,this);
    monitor_->AddCallback(agv_p.subspeedtopic(),Navigation::RobotSpeedCallback,this);
  }

  Emqx_parameter terminal_p=parameter.terminal_emqx();
  if(terminator_== nullptr) {
    terminator_ = new Terminator_emqx(terminal_p.nodeid());

    if(terminator_->Connect(terminal_p.ip(),terminal_p.port())==false)
    {
      printf(" terminator emqx connected failed\n");
      return false;
    }

    terminator_->AddCallback(terminal_p.subnavcmdtopic(),Navigation::NavCmdCallback,this);

  }

  if(parameter.has_brother_emqx()) {
    BrotherEmqx brotherEmqx = parameter.brother_emqx();
    if (brotherEmqx_ == nullptr) {
      brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());

      if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
        brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback, this);
      }else{
        printf(" brother emqx connected failed\n");
       // return false;
      }

    }
  }
  inited_=true;

  if(pubthread_!= nullptr)
  {
    exit_=true;
    if(pubthread_->joinable())
      pubthread_->join();
    delete pubthread_;
  }
  exit_=false;
  pubthread_=new std::thread(&Navigation::pubStatuThreadFuc,this);

  return true;

}

void Navigation::RobotPoseCallback(const MqttMsg& msg,void* context)
{
  Navigation* navigator=(Navigation*)context;
  NavMessage::LidarOdomStatu statu;
  if(msg.toProtoMessage(statu))
  {
    navigator->ResetPose(Pose2d(statu.x(),statu.y(),statu.theta()));
  }
}

void Navigation::HandleAgvStatu(const MqttMsg& msg)
{
  NavMessage::AgvStatu speed;
  if(msg.toProtoMessage(speed))
  {
    ResetStatu(speed.v(),speed.w());
    ResetClamp((ClampStatu)speed.clamp());
  }
}

void Navigation::RobotSpeedCallback(const MqttMsg& msg,void* context)
{
  Navigation* navigator=(Navigation*)context;
  navigator->HandleAgvStatu(msg);
}

void Navigation::pubStatuThreadFuc(void* p) {
  Navigation *navogator = (Navigation *) p;

  if (navogator) {
    while (navogator->exit_ == false) {
      std::this_thread::sleep_for(std::chrono::milliseconds(50));
      NavMessage::NavStatu statu;
      statu.set_main_agv(false);
      navogator->publish_statu(statu);
    }
  }
}

void Navigation::publish_statu(NavMessage::NavStatu& statu) {

    //发布nav状态
    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 预选点
      if (selected_traj_.timeout() == false) {
        for (int i = 0; i < selected_traj_.Get().size(); ++i) {
          Pose2d pt = selected_traj_.Get()[i];
          NavMessage::Pose2d *pose = statu.mutable_selected_traj()->add_poses();
          pose->set_x(pt.x());
          pose->set_y(pt.y());
          pose->set_theta(pt.theta());
        }

      }
      //发布 mpc 预测点
      if (predict_traj_.timeout() == false) {
        for (int i = 0; i < predict_traj_.Get().size(); ++i) {
          Pose2d pt = predict_traj_.Get()[i];
          NavMessage::Pose2d *pose = statu.mutable_predict_traj()->add_poses();
          pose->set_x(pt.x());
          pose->set_y(pt.y());
          pose->set_theta(pt.theta());
        }
      }
      //std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;

    MqttMsg msg;
    msg.fromProtoMessage(statu);
    if(terminator_)
      terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);

    //发布位姿  ------robot状态--------------------------
    NavMessage::RobotStatu robot;
    if(CreateRobotStatuMsg(robot)) {
      if (terminator_) {
        MqttMsg msg;
        msg.fromProtoMessage(robot);
        terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
      }
    }
}

bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
{
  //printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout());
  if (timedPose_.timeout() == false) {

    Pose2d pose = timedPose_.Get();
    robotStatu.set_x(pose.x());
    robotStatu.set_y(pose.y());
    robotStatu.set_theta(pose.theta());
    if((timedV_.timeout() == false && timedA_.timeout() == false)) {
      NavMessage::AgvStatu plc;
      plc.set_v(timedV_.Get());
      plc.set_w(timedA_.Get());
      plc.set_clamp(timed_clamp_.Get());
      //printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
      robotStatu.mutable_agvstatu()->CopyFrom(plc);
    }
    return true;
  }
  return false;
}

void Navigation::ResetStatu(double v,double a)
{
  timedV_.reset(v,0.5);
  timedA_.reset(a,0.5);
}

void Navigation::ResetClamp(ClampStatu statu){
  timed_clamp_.reset(statu,1);
}

void Navigation::ResetPose(const Pose2d& pose)
{
  timedPose_.reset(pose,0.5);
}

bool Navigation::Start(const NavMessage::NavCmd& cmd)
{
  if(unfinished_cations_.empty()==false)  //正在运行中，上一次指令未完成
  {
    if(pause_)
    {
      pause_=false;
      return true;
    }
    printf(" navigation is running pls cancel before\n");
    return false;
  }
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
      thread_->join();
    delete thread_;
  }

    //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;

}
void Navigation::Cancel()
{
  cancel_=true;

}

bool Navigation::Stop() {
  if(monitor_)
  {
    monitor_->stop();
    while(cancel_==false)
    {
      if(timedV_.timeout()==false && timedA_.timeout()==false)
      {
        if(fabs(timedV_.Get())<1e-6 && fabs(timedA_.Get())<1e-6)
          return true;
        else
          monitor_->stop();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
      }else{
        printf("Stop failed V/A timeout\n");
        return false;
      }
    }
  }
  return false;
}

void Navigation::Pause()
{
  if(Stop())
    pause_=true;
}

Navigation::Navigation()
{
  move_mode_=Monitor_emqx::eSingle;
  thread_= nullptr;
  monitor_= nullptr;
  terminator_= nullptr;

  timedBrotherPose_.reset(Pose2d(-100,0,0),0.1);
}

void Navigation::BrotherAgvStatuCallback(const MqttMsg& msg,void* context)
{
  Navigation* navigator=(Navigation*)context;

  NavMessage::LidarOdomStatu brother_statu;
  if(msg.toProtoMessage(brother_statu)==false) {
    std::cout<<" msg transform to AGVStatu failed,msg:"<<msg.data()<<std::endl;
    return;
  }
  //std::cout<<brother_nav.DebugString()<<std::endl<<std::endl;
  Pose2d pose(brother_statu.x(),brother_statu.y(),brother_statu.theta());
  navigator->timedBrotherPose_.reset(pose,1);
  navigator->timedBrotherV_.reset(brother_statu.v(),1);
  navigator->timedBrotherA_.reset(brother_statu.vth(),1);
}

void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
{
  if(cmd.action()==3)
  {
    printf(" Nav cancel\n");
    Cancel();
    return ;
  }
  if(cmd.action()==2) {
    printf(" Nav continue\n");
    pause_=false;
    return ;
  }
  if(cmd.action()==1) {
    printf(" Nav pause\n");
    Pause();
    return ;
  }
  if(cmd.action()==0||cmd.action()==6)
  {
    Start(cmd);
  }
  else
    printf(" Invalid action ,action:%d\n",cmd.action());
}

void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
{
  NavMessage::NavCmd cmd;
  if(msg.toProtoMessage(cmd)==false) {
    printf(" msg transform to NavCmd failed ..!!\n");
    return;
  }
  Navigation* navigator=(Navigation*)context;
  navigator->HandleNavCmd(cmd);
}



bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
                                    stLimit limit_h,stLimit limit_rotate,bool anyDitect)
{
  std::cout<<" adjust target:"<<target<<" target diff:"<<target_diff<<std::endl;
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return false;
  }

  Pose2d accuracy=target_diff;
  Pose2d thresh=Pose2d::abs(accuracy);

  int action=0; //1 原地旋转，2横移，3前进
  while(cancel_==false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (pause_ == true) {
      //发送暂停消息
      continue;
    }
    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
      printf(" navigation Error:Pose/v/a is timeout \n");
      break;
    }
    //判断是否到达目标点
    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,anyDitect)) {
      if(Stop()) {
        printf("adjust completed anyDitect :%d\n",anyDitect);
        return true;
      }
      return false;
    }

    
    //计算目标点在当前pose下的pose
    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
    //停止状态
    if (action == 0) {
      //
     if (Pose2d::abs(diff).x() > thresh.x()) {
        //前进
        action = 3;
      }else if (Pose2d::abs(diff).y() > thresh.y()) {
        //横移
        action = 2;
      }
      else if (Pose2d::abs(diff).theta() > thresh.theta()) {
       action = 1; //原地旋转
      }
    }
    else
    {
      double acc_v=2.0;
      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;
            }
          }
          double theta = limit_gause(diff_yaw,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);
          continue;
        }
      }
      if (action == 2) {
        if (Pose2d::abs(diff).y() > thresh.y()) {
          double hv=limit_gause(diff.y(),limit_h.min,limit_h.max);
          double velocity=limit(next_speed(timedV_.Get(),hv,acc_v,dt),limit_h.min,limit_h.max);
          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal,velocity , 0);
          actionType_=eHorizon;
          printf(" Horizontal input:%f out:%f\n",timedV_.Get(),velocity);
          continue;
        }
      }
      if (action == 3) {
        if (Pose2d::abs(diff).x() > thresh.x()) {
          double v=limit_gause(diff.x(),limit_v.min,limit_v.max);
          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);
          continue;
        }
      }
      if(Stop()) {
        printf(" monitor type :%d completed!!! reset action type!!!\n", action);
        action = 0;
      }
    }
  }
  return false;

}


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 anyDirect=(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() };
    //目标，精度
    double first_yaw=timedPose_.Get().theta();
    if(1<action.pathnodes_size()){
      NavMessage::PathNode next=action.pathnodes(1);
      first_yaw=Pose2d::vector2yaw(next.pose().x()-first.pose().x(),next.pose().y()-first.pose().y());
    }
    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;
    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());

        if (execute_along_action(last_node, target, accuracy, mpc_velocity, anyDirect) == false)
          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;
  }
  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) {
    printf(" MPC once Error:Pose/V/A is timeout \n");
    return false;
  }
  if (traj.size() == 0) {
    printf("traj size ==0\n");
    return false;
  }

  Pose2d pose = timedPose_.Get();
  double velocity = timedV_.Get();    //从plc获取状态
  double angular = timedA_.Get();

  Pose2d brother = timedBrotherPose_.Get();
  Pose2d relative = Pose2d::relativePose(brother, pose);//计算另一节在当前小车坐标系下的坐标
  if (move_mode_ == Monitor_emqx::ActionMode::eMain)
    relative = Pose2d(-1e8, -1e8, 0);

  Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态

  statu[0] = pose.x();
  statu[1] = pose.y();
  statu[2] = pose.theta();
  statu[3] = velocity;
  statu[4] = angular;

  Trajectory optimize_trajectory;
  Trajectory selected_trajectory;

  if (directY == true) {
    //将车身朝向旋转90°，车身朝向在(-pi,pi]
    statu[2] += M_PI / 2;
    if (statu[2] > M_PI)
      statu[2] -= 2 * M_PI;
  }
  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);

  //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
  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,bool nearest_yaw)
{
  //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)
        return true;
    }
  }else{
    if(absDiff.x()<diff.x() && absDiff.y()<diff.x())
    {
      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;
}

void Navigation::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                             double v,double angular){
  if(monitor_) {
    monitor_->set_speed(mode, type, v, angular);
  }
}

bool Navigation::clamp_close()
{
  if(monitor_) {
    monitor_->clamp_close(move_mode_);
    actionType_=eClampClose;
    while (cancel_ == false) {
      if (timed_clamp_.timeout()) {
        printf("timed clamp is timeout\n");
        return false;
      }
      if (timed_clamp_.Get() == eClosed)
        return true;
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      monitor_->clamp_close(move_mode_);
      actionType_=eClampClose;
    }
    return false;
  }
  return false;
}
bool Navigation::clamp_open() {
  if(monitor_) {
    monitor_->clamp_open(move_mode_);
    actionType_=eClampOpen;
    while(cancel_==false)
    {
      if(timed_clamp_.timeout())
      {
        printf("timed clamp is timeout\n");
        return false;
      }
      if(timed_clamp_.Get()==eOpened)
        return true;
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      monitor_->clamp_open(move_mode_);
      actionType_=eClampOpen;
    }
    return false;
  }
  return false;
}

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;
  }
  if (timedPose_.timeout()) {
    printf(" navigation Error:Pose is timeout \n");
    return false;
  }
  //生成轨迹
  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;
  }
  printf(" exec along autoDirect:%d\n",autoDirect);
  while (cancel_ == false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (pause_ == true) {
      //发送暂停消息
      continue;
    }
    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
      printf(" navigation Error:Pose/v/a is timeout \n");
      return false;
    }
    //判断是否到达终点
    if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,directY)) {
      if (Stop()) {
        printf(" exec along completed !!!\n");
        return true;
      }
    }
    /*
     * 判断周边是否有障碍物，半径r，当障碍物进入r内，停下等待，当障碍物远离 1.2*r，恢复
     * */
    if(move_mode_==Monitor_emqx::eSingle) {
      double r = 2.8;
      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;
    bool ret;
    TimerRecord::Execute([&, this]() {
        ret = mpc_once(traj, limit_v, out,directY);
    }, "mpc_once");
    if (ret == false) {
      Stop();
      return false;
    }
    if(directY==false)
      SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
    else
      SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
    actionType_=eMPC;
    /*
     * 判断车辆是否在终点附近徘徊，无法到达终点
     */
    if (mpc_possible_to_end(target, target_diff) == false) {
      printf(" --------------MPC  imposible to target -----------------------\n");
      Stop();
      //调整到目标点
      stLimit limitv, limitR;
      limitv.min = 0.05;
      limitv.max = 0.1;
      limitR.min = 2 * M_PI / 180.0;
      limitR.max = 10 * M_PI / 180.0;
      Pose2d accuracy(target_diff.x(),target_diff.y(),M_PI*2);
      if (execute_adjust_action(target, accuracy, limitv, limitv, limitR,false))
        return true;
      else
        return false;
    }

    //std::this_thread::sleep_for(std::chrono::milliseconds (400));
  }

  return false;
}

bool Navigation::mpc_possible_to_end(const Pose2d& target,const Pose2d& target_diff)
{
  if(timedPose_.timeout()==false) {
    Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
    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;
      return true;
    } else {  //xy不在精度内
      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 {
        return true;
      }
    }
  }
  return false;
}

void Navigation::navigatting()
{
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return ;
  }
  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");
        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");
        break;
      }
    }
    else if(act.type()==3)  //夹持
    {
      if(this->clamp_close()==false)
      {
        printf("夹持failed ...\n");
        break;
      }
    }
    else if(act.type()==4)
    {
      if(this->clamp_open()==false)
      {
        printf("打开夹持 failed...\n");
        break;
      }
    }
    else
    {
      printf(" unknow action type:%d\n",act.type());
      break;
    }
    unfinished_cations_.pop();
  }
  actionType_=eReady;
  Stop();
  if(cancel_==true) {
    printf(" navigation canceled\n");
    while(unfinished_cations_.empty()==false)
      unfinished_cations_.pop();
  }else {
    if (unfinished_cations_.empty())
      printf("navigation completed!!!\n");
    else{
      printf(" navigation Failed\n");
      while(unfinished_cations_.empty()==false)
        unfinished_cations_.pop();
    }
  }
  running_=false;
}