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 NavParameter::Navigation_parameter& parameter)
{
  parameter_=parameter;
  NavParameter::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);
  }

  NavParameter::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()) {
    NavParameter::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_);
    if (running_)
    {
      statu.set_key(global_navCmd_.key());
    }
    //发布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,1.0);
}

bool Navigation::Start(const NavMessage::NavCmd& cmd)
{
  if(newUnfinished_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;

    for (int i = 0; i < cmd.newactions_size(); ++i)
      newUnfinished_cations_.push(cmd.newactions(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;
    }
    //判断是否到达目标点
    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;
      }
      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=0.6;
      double acc_angular=10.0*M_PI/180.0;
      double dt=0.1;
      if (action == 1) {
        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];
            }
          }

        }
        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,minYawDiff,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);
//    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() };
    //目标，精度
    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::execute_vehicle_mode(NavMessage::NewAction action){
  if(action.type()==5 ) //最优动作导航 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;

      //原地调整起点
      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);
      return false;
    }
  }else{
    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;
  //入库,起点为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()*2,
                           action.spacenode().accuracy().y()*2,
                           action.spacenode().accuracy().theta()*2);
  double yaw = Pose2d::vector2yaw(target.x() - begin.x(), target.y() - begin.y());
  if (action.type() == 2) {  //出库起点终点对调
    Pose2d mid = target;
    Pose2d mid_accuracy = target_accuracy;
    target = begin;
    target_accuracy = begin_accuracy;
    begin = mid;
    begin_accuracy = mid_accuracy;
    yaw = Pose2d::vector2yaw(begin.x() - target.x(), begin.y() - target.y());
  }
  //给起点赋予朝向,保证agv以正对车位的方向行进

  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(1, 1, M_PI * 2);
  if (action.type() == 2) {  //出库要求起点距离当前点非常近
    distance = Pose2d(1, 0.1, 3 * M_PI);
    adjust_x = {0.03, 0.1};
    adjust_h = {0.03, 0.1};
    adjust_angular = {2 * M_PI / 180.0, 5 * M_PI / 180.0};
  }
  Pose2d relativeDiff=Pose2d::relativePose(begin,timedPose_.Get());
  if (!(Pose2d::abs(relativeDiff) < distance)) {
    printf(" Inout space failed type:%d: current pose too far from begin node\n", action.type());
    std::cout << " begin：" << begin << "   current:" << timedPose_.Get()<<"  relative:"<<relativeDiff << std::endl;
    return false;
  }

  if (action.type() == 1) //1,进库
    if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
      return false;

  //if (action.type() == 2) //2，出库
    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) {
    printf(" MPC once Error:Pose is timeout \n");
    return false;
  }
  if(timedV_.timeout() == true || timedA_.timeout() == true){
    printf(" MPC once Error: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;
  NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
  if (directY == true) {
    //将车身朝向旋转90°，车身朝向在(-pi,pi]
    statu[2] += M_PI / 2;
    if (statu[2] > M_PI)
      statu[2] -= 2 * M_PI;
    mpc_parameter=parameter_.y_mpc_parameter();
  }
  bool ret = false;
  LoadedMPC MPC(relative, limit_v.min, limit_v.max);
  LoadedMPC::MPC_parameter parameter={mpc_parameter.shortest_radius(),mpc_parameter.dt(),
                                     mpc_parameter.acc_velocity(),mpc_parameter.acc_angular()};
  //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
  //       mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
  ret = MPC.solve(traj, traj[traj.size() - 1], statu,parameter,
                  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,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);

  //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.06 && fabs(angular) < 5 * M_PI / 180.0)
        return true;
    }
  }else if(direct==2){    //横向MPC
    if(absDiff.x()<diff.x() && absDiff.y()<diff.y())
    {
      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.06 && 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;
}

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_) {
    printf("Clamp closing...\n");
    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_) {
    printf("Clamp openning...\n");
    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轴上
      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) {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (pause_ == true) {
      //发送暂停消息
      continue;
    }
    if (timedPose_.timeout()) {
      printf(" navigation Error:Pose is timeout \n");
      return false;
    }
    if(timedV_.timeout() || timedA_.timeout()){
      printf(" navigation Error:v/a is timeout \n");
      return false;
    }
    //判断是否到达终点
    /*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;
      }
    }
    /*
     * 判断周边是否有障碍物，半径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;
    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,directY) == false) {
      printf(" --------------MPC  imposible to target -----------------------\n");
      if(fabs(timedV_.Get())>0.1 || fabs(timedA_.Get())>5*M_PI/180.0)
        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,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 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;*/
      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 {
        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");

  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){  //出库
      if(!execute_InOut_space(act)){
        printf(" out space failed\n");
        break;
      }
    }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() == 7) {
      if (this->clamp_open() == false) {
        printf("打开夹持 failed...\n");
        break;
      }
    } else if(act.type()==8){ //切换模式
      SwitchMode(Monitor_emqx::ActionMode(act.changedmode()),act.wheelbase());
    }else{
      printf(" action type invalid not handled !!\n");
      break;
    }
    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;
}

void Navigation::SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
    printf("Child AGV can not Switch Mode\n");
}