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(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)
{
  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::PoseTimeout(){
  if(timedPose_.timeout()== false && timedBrotherPose_.timeout()==false){
    return false;
  }else{
    if(timedPose_.timeout()){
      printf(" current pose is timeout \n");
    }
    if(timedBrotherPose_.timeout()){
      printf(" brother pose is timeout \n");
    }
    return true;
  }
}

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

  }

  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) {
  if(timedPose_.timeout()==false){
    NavMessage::LidarOdomStatu odom;
    odom.set_x(timedPose_.Get().x());
    odom.set_y(timedPose_.Get().y());
    odom.set_theta(timedPose_.Get().theta());
    if(timedV_.timeout()==false)
      odom.set_v(timedV_.Get());
    if(timedA_.timeout()==false)
      odom.set_vth(timedA_.Get());
    statu.mutable_odom()->CopyFrom(odom);
  }
  statu.set_in_space(isInSpace_);
  if(isInSpace_) statu.set_space_id(space_id_);
    //发布nav状态
    statu.set_statu(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);
}

void Navigation::Cancel(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response)
{
  cancel_=true;
  response.set_ret(0);
}

bool Navigation::Stop() {
  if(monitor_)
  {
    monitor_->stop();
    while(cancel_==false)
    {
      if(timedV_.timeout()==false && timedA_.timeout()==false)
      {
        if(fabs(timedV_.Get())<1e-2 && fabs(timedA_.Get())<1e-3)
          return true;
        else
          monitor_->stop();
        printf("Stoped wait V/A ==0(1e-4,1e-4),V:%f,A:%f\n",fabs(timedV_.Get()),fabs(timedA_.Get()));
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
      }else{
        printf("Stop failed V/A timeout\n");
        return false;
      }
    }
  }
  printf("stoped ret false.\n");
  return false;
}

bool Navigation::SlowlyStop() {

  double acc = 3.0;
  double dt = 0.1;
  while (cancel_ == false) {
    if (timedV_.timeout() == false) {

      double v = timedV_.Get();
      if(fabs(v<1e-2 && fabs(timedA_.Get())<1e-3))
        return true;

      if (v > 0) {
        double new_v = v - acc * dt;
        new_v = new_v > 0 ? new_v : 0;
        SendMoveCmd(move_mode_, Monitor_emqx::ActionType(actionType_), new_v,0);
      }else{
        double new_v = v + acc * dt;
        new_v = new_v < 0 ? new_v : 0;
        SendMoveCmd(move_mode_, Monitor_emqx::ActionType(actionType_), new_v,0);
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } else {
      printf("Stop failed V/A timeout\n");
      return false;
    }
  }
  printf("stoped ret false.\n");
  return false;
}


Navigation::Navigation()
{
  isInSpace_=false; //是否在车位或者正在进入车位
  RWheel_position_=eUnknow;
  move_mode_=Monitor_emqx::eSingle;
  monitor_= nullptr;
  terminator_= nullptr;

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

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

  NavMessage::NavStatu 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;
  if(brother_statu.has_odom()) {
    NavMessage::LidarOdomStatu odom = brother_statu.odom();
    Pose2d pose(odom.x(), odom.y(), odom.theta());
    navigator->timedBrotherPose_.reset(pose, 1);
    navigator->timedBrotherV_.reset(odom.v(), 1);
    navigator->timedBrotherA_.reset(odom.vth(), 1);
    navigator->timedBrotherNavStatu_.reset(brother_statu,0.1);
  }
}


bool Navigation::RotateReferToTarget(const Pose2d& target,stLimit limit_rotate,bool anyDirect) {

  while(cancel_==false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if (timedPose_.timeout()) {
      printf(" RotateBytarget failed  pose timeout\n");
      return false;
    }
    if (timedA_.timeout()) {
      printf(" RotateBytarget failed  wmg timeout\n");
      return false;
    }
    Pose2d targetInPose = Pose2d::relativePose(target, timedPose_.Get());
    float yawDiff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y());
    float minYawdiff = yawDiff;

    if (anyDirect) {
      Pose2d p0 = timedPose_.Get();
      float yawdiff[4] = {0};
      yawdiff[0] = yawDiff;

      Pose2d relativePose;
      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI / 2));
      yawdiff[1] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());  ////使用减法，保证角度在【-pi pi】
      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI));
      yawdiff[2] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
      relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, 3 * M_PI / 2));
      yawdiff[3] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
      for (int i = 1; i < 4; ++i) {
        if (fabs(yawdiff[i]) < fabs(minYawdiff)) {
          minYawdiff = yawdiff[i];
          targetInPose=Pose2d::relativePose(target,p0-Pose2d(0,0,i*M_PI/2.0));
        }
      }
    }
    /*std::cout<<" Rotate refer to target:"<<target<<",targetInPose:"
        <<targetInPose<<",anyDirect:"<<anyDirect<<std::endl;*/
    //以当前点为原点，想方向经过目标点的二次曲线曲率>0.25,则需要调整 y=a x*x

    float cuv=compute_cuv(targetInPose);
    float dt=0.1;
    float acc_angular=15*M_PI/180.0;
    if (cuv>2*M_PI/180.0) {
      double theta = limit_gause(minYawdiff, limit_rotate.min, limit_rotate.max);
      double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
      double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);

      SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
      actionType_ = eRotation;
      printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f anyDitect:%d\n",
             timedA_.Get(), angular, limit_angular, minYawdiff, anyDirect);

      continue;
    }else{
      if(fabs(timedA_.Get())<5*M_PI/180.0) {
        printf(" Rotate refer target completed,cuv:%f\n", cuv);
        return true;
      }
      continue;
    }
  }
  return false;
}


bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
                              bool anyDirect,bool enable_rotate) {
  if (IsArrived(node)) {
    return true;
  }
  bool already_in_mpc=false;
  while (cancel_ == false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if (PoseTimeout()) {
      printf(" navigation Error:Pose is timeout \n");
      break;
    }
    Pose2d current=timedPose_.Get();

    if (IsArrived(node)) {
      break;
    }
    //两个方向都无法到达目标点,旋转 朝向目标点
    bool x_enable=PossibleToTarget(node,false);
    bool y_enable=PossibleToTarget(node,true);
    bool enableTotarget=x_enable||y_enable;
    if(anyDirect==false){
      enableTotarget=x_enable;
    }
    //巡线无法到达目标点
    if(enableTotarget==false) {
      if(already_in_mpc && enable_rotate==false) {
        printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
        return false;
      }
      Pose2d target(node.x(), node.y(), 0);
      if (RotateReferToTarget(target, limit_w, anyDirect) == false) {
        std::cout << " Rotate refer to target failed,target:" << target <<
                  "  anyDirect:" << anyDirect << std::endl;
        return false;
      }
      continue;
    }
    already_in_mpc=true;
    MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
    if (ret == eMpcSuccess) {
      printf(" MPC to target:%f %f l:%f,w:%f theta:%f  completed\n",
             node.x(),node.y(),node.l(),node.w(),node.theta());
      break;
    } else if (ret == eImpossibleToTarget) {
      printf(" MPC to target:%f %f l:%f,w:%f theta:%f  impossible ,retry ..............................\n",
             node.x(),node.y(),node.l(),node.w(),node.theta());
    } else {
      return false;
    }
  }
  if (cancel_) {
    return false;
  }
  return true;
}

bool Navigation::execute_nodes(NavMessage::NewAction action)
{
  if(action.type()==3 || action.type()==4) //最优动作导航 or 导航中保证朝向严格一致
  {
    if(!parameter_.has_nodeangularlimit()|| !parameter_.has_nodevelocitylimit()||action.pathnodes_size()==0) {
      std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
      return false;
    }
    bool anyDirect=(action.type()==3);

    //速度限制
    stLimit adjust_angular={ parameter_.nodeangularlimit().min(),parameter_.nodeangularlimit().max() };
    stLimit mpc_velocity={ parameter_.nodevelocitylimit().min(),parameter_.nodevelocitylimit().max() };

    for(int i=0;i< action.pathnodes_size();++i) {
      NavMessage::PathNode node = action.pathnodes(i);
      if(i+1<action.pathnodes_size()){
        NavMessage::PathNode next = action.pathnodes(i+1);
        Pose2d vec(next.x()-node.x(),next.y()-node.y(),0);
        node.set_theta(Pose2d::vector2yaw(vec.x(),vec.y()));
        node.set_l(0.3);
        node.set_w(0.1);

      }else{
        node.set_theta(0);
        node.set_w(0.2);
        node.set_l(0.2);
      }
      if(MoveToTarget(node,mpc_velocity,adjust_angular,anyDirect,true)==false)
        return false;
      else
        isInSpace_=false;
    }
    return true;
  }
  printf("execute PathNode failed ,action type is not 3/4  :%d\n",action.type());
  return false;
}

bool Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target) {
  if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
    printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
    return false;
  }
  NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();

  stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
  double acc_angular = 20 * M_PI / 180.0;
  double dt = 0.1;
  Pose2d rotated = timedPose_.Get();

  //前车先到,当前车进入2点,保持与前车一致的朝向
  if (brother.in_space() && brother.space_id() == space.id()) {
    rotated.mutable_theta() = brother.odom().theta();
  } else { //当前车先到（后车）,倒车入库
    rotated.mutable_theta() = space.theta();
    rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
  }
  printf("brother spaceid:%s  current space id:%s ,space theta :%f\n",brother.space_id().c_str(),space.id().c_str(),space.theta());
  double x = space.x() - wheelbase/2 * cos(rotated.theta());
  double y = space.y() - wheelbase/2 * sin(rotated.theta());
  target.set_x(x);
  target.set_y(y);
  target.set_theta(rotated.theta());
  target.set_l(0.05);
  target.set_w(0.03);
  target.set_id(space.id());
  printf(" new target :%f,%f, theta:%f,wheelbase:%f\n",target.x(),target.y(),rotated.theta(),wheelbase);
  //整车协调的时候，保持前后与车位一致即可
  if(move_mode_==Monitor_emqx::eMain){
    double yawDiff1 = (rotated - timedPose_.Get()).theta();
    double yawDiff2 = (rotated+Pose2d(0,0,M_PI) - timedPose_.Get()).theta();
    if(fabs(yawDiff1)<fabs(yawDiff2)){
      rotated=rotated+Pose2d(0,0,M_PI);
    }
  }

  while (cancel_ == false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    Pose2d current = timedPose_.Get();
    double yawDiff = (rotated - current).theta();
    if (fabs(yawDiff) > 1 * M_PI / 180.0) {
      double theta = limit_gause(yawDiff, 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(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
             timedA_.Get(), angular, limit_angular, yawDiff);*/

      continue;
    } else {
      if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
        printf(" RotateBeforeEnterSpace refer target completed\n");
        printf("---------------- update new target :%f %f %f \n",target.x(),target.y(),target.theta());
        return true;
      }
    }
  }
  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 (!parameter_.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 (PoseTimeout() == true) {
    printf(" inout_space failed type:%d : current pose is timeout\n", action.type());
    return false;
  }

  stLimit limit_v={parameter_.inoutvlimit().min(),parameter_.inoutvlimit().max()};
  stLimit limit_w={2*M_PI/180.0,20*M_PI/180.0};
  //入库,起点为streetNode
  if(action.type()==1){
    Pose2d vec(action.spacenode().x()-action.streetnode().x(),action.spacenode().y()-action.streetnode().y(),0);
    action.mutable_streetnode()->set_l(0.2);
    action.mutable_streetnode()->set_w(0.05);
    action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(),vec.y()));
    if(MoveToTarget(action.streetnode(),limit_v,limit_w,true,true)==false)
    {
      printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
      return false;
    }
    //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
    printf("inout ----------------------------------------\n");
    //计算车位朝向
    action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(),vec.y()));
    //计算当前车是进入车位1点还是2点
    NavMessage::PathNode new_atrget;
    if(RotateBeforeEnterSpace(action.spacenode(),action.wheelbase(),new_atrget)==false){
      return false;
    }
    if(MoveToTarget(new_atrget,limit_v,limit_w,true,false)==false)
    {
      printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
      return false;
    }
  }else if(action.type()==2){
    Pose2d space(action.spacenode().x(),action.spacenode().y(),0);
    Pose2d diff=Pose2d::abs(Pose2d::relativePose(space,timedPose_.Get()));
    if(diff.y()<0.05){
      //出库，移动到马路点，允许自由方向，不允许中途旋转
      Pose2d vec(action.streetnode().x()-action.spacenode().x(),action.streetnode().y()-action.spacenode().y(),0);
      action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(),vec.y()));
      action.mutable_streetnode()->set_l(0.2);
      action.mutable_streetnode()->set_w(0.1);
      if(MoveToTarget(action.streetnode(),limit_v,limit_w,true,false)==false){
        printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type());
        return false;
      }
    }else{
      std::cout<<" Out space failed: current pose too far from space node:"<<space<<",diff:"<<diff<<std::endl;
      return false;
    }

  }
  return true;
}


bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY) {
  if (PoseTimeout() == 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();

  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();
  double obs_w=0.85;
  double obs_h=1.45;

  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();
    obs_w=1.45;
    obs_h=0.85;
  }

  Pose2d brother = timedBrotherPose_.Get();
  Pose2d relative = Pose2d::relativePose(brother, pose);
  if(directY)
    relative = Pose2d::relativePose(brother.rotate(brother.x(),brother.y(),M_PI/2), pose.rotate(pose.x(),pose.y(),M_PI/2));//计算另一节在当前小车坐标系下的坐标
  //std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
  if (move_mode_ == Monitor_emqx::ActionMode::eMain)
    relative = Pose2d(-1e8, -1e8, 0);

  MpcError ret = failed;
  LoadedMPC MPC(relative, obs_w,obs_h,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;
  if(ret==no_solution) {
    printf(" mpc solve no solution set v/w = 0\n");
    out.clear();
    out.push_back(0);
    out.push_back(0);
  }
  predict_traj_.reset(optimize_trajectory, 1);
  selected_traj_.reset(selected_trajectory, 1);
  return ret==success || ret==no_solution;
}


bool Navigation::IsArrived(NavMessage::PathNode targetNode)
{
  if(timedPose_.timeout()|| timedV_.timeout()||timedA_.timeout()){
    printf(" pose / v / w    timeout,IsArrived return false\n");
    return false;
  }
  Pose2d current=timedPose_.Get();
  double x=current.x();
  double y=current.y();
  double tx=targetNode.x();
  double ty=targetNode.y();
  double l=fabs(targetNode.l());
  double w=fabs(targetNode.w());
  double theta=-targetNode.theta();

  if(l<1e-10 || w<1e-10){
    printf("IsArrived: Error target l or w == 0\n");
    return false;
  }

  double a=(x-tx)*cos(theta)-(y-ty)*sin(theta);
  double b=(x-tx)*sin(theta)+(y-ty)*cos(theta);

  if( pow(a/l,8)+pow(b/w,8) <=1){
    if(fabs(timedV_.Get())<0.06 && fabs(timedA_.Get())<5*M_PI/180.0) {
      printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n",tx,ty,l,w,theta);
      return true;
    }
  }
  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);
    if(mode==ActionType::eRotation)
      RWheel_position_=eR;
    if(mode==ActionType::eVertical)
      RWheel_position_=eX;
    if(mode==ActionType::eHorizon)
      RWheel_position_=eY;
  }
}

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


Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node,stLimit limit_v,bool autoDirect) {
  if (IsArrived(node)) {
      printf(" current already in target completed !!!\n");
      return eMpcSuccess;
  }
  if (inited_ == false) {
    printf(" navigation has not inited\n");
    return eMpcFailed;
  }
  if (PoseTimeout()) {
    printf(" navigation Error:Pose is timeout \n");
    return eMpcFailed;
  }
  Pose2d current=timedPose_.Get();
  Pose2d target(node.x(),node.y(),0);
  //生成轨迹
  Trajectory traj = Trajectory::create_line_trajectory(current, target);
  if (traj.size() == 0)
    return eMpcSuccess;
  //判断 巡线方向
  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 (PoseTimeout()) {
      printf(" navigation Error:Pose is timeout \n");
      return eMpcFailed;
    }
    if(timedV_.timeout() || timedA_.timeout()){
      printf(" navigation Error:v/a is timeout \n");
      return eMpcFailed;
    }
    //判断是否到达终点
    if (IsArrived(node)) {
      if (Stop()) {
        printf(" exec along completed !!!\n");
        return eMpcSuccess;
      }
    }

    //一次变速
    std::vector<double> out;
    bool ret;
    TimerRecord::Execute([&, this]() {
        ret = mpc_once(traj, limit_v, out,directY);
    }, "mpc_once");
    if (ret == false) {
      Stop();
      return eMpcFailed;
    }
    /*
     * AGV 在终点附近低速巡航时，设置最小速度0.05
     */
    Pose2d target_in_agv=Pose2d::relativePose(target,timedPose_.Get());
    if (directY) {
      target_in_agv=target_in_agv.rotate(M_PI / 2.0);
    }
    Pose2d absPose=Pose2d::abs(target_in_agv);
    if(absPose.x()<0.5 && absPose.y()<0.5) {
      if (out[0] >= 0 && out[0] < limit_v.min)
        out[0] = limit_v.min;
      if (out[0] < 0 && out[0] > -limit_v.min)
        out[0] = -limit_v.min;
    }
    //下发速度
    if(directY==false)
      SendMoveCmd(move_mode_, Monitor_emqx::eVertical, out[0], out[1]);
    else
      SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
    actionType_=directY?eHorizon:eVertical;
    /*
     * 判断车辆是否在终点附近徘徊，无法到达终点
     */
    if (PossibleToTarget(node, directY) == false) {
      printf(" --------------MPC  imposible to target -------------------------------------------\n");
      if(fabs(timedV_.Get())>0.1 || fabs(timedA_.Get())>10*M_PI/180.0)
        SlowlyStop();
      else
        Stop();
      return eImpossibleToTarget;

    }
  }

  return eMpcFailed;
}

bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode,bool directY) {
  if (timedPose_.timeout()) {
    return false;
  }
  Pose2d current = timedPose_.Get();

  double tx = targetNode.x();
  double ty = targetNode.y();
  double l = fabs(targetNode.l());
  double w = fabs(targetNode.w());
  double theta = -targetNode.theta();

  double W = 2.45;
  double L = 1.3;
  double minYaw=3*M_PI/180.0;
  if(move_mode_==Monitor_emqx::eMain){
    L=1.3+2.7;

  }
  if(directY){
    current=current.rotate(current.x(),current.y(),M_PI/2);
    double t=W;
    W=L;
    L=t;
  }
  double minR = W / 2 + L / tan(minYaw);
  //printf("l:%f,w:%f\n",l,w);
  std::vector<Pose2d> poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l*2, w*2);

  bool nagY=false;
  bool posiY=false;
  for (int i = 0; i < poses.size(); ++i) {
    Pose2d rpos = poses[i].rotate(tx, ty, theta);
    Pose2d relative=Pose2d::relativePose(rpos,current);
    double ax =fabs(relative.x());
    double ay = fabs(relative.y());
    if(relative.y()>0) posiY=true;
    if(relative.y()<0) nagY=true;
    if (ax * ax + pow(ay - minR, 2) > minR*minR) {
      /*printf(" possible to target:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n",
             targetNode.x(),targetNode.y(),targetNode.l(),targetNode.w(),targetNode.theta(),
             minR,ax,ay);*/
      return true;
    }else if(nagY&&posiY){
      return true;
    }
    printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n",directY,
           relative.x(),relative.y(),targetNode.l(),targetNode.w(),targetNode.theta(),
           minR,ax,ay);
  }


  printf(" impossible to target:%f %f l:%f,w:%f theta:%f\n",
         targetNode.x(),targetNode.y(),targetNode.l(),targetNode.w(),targetNode.theta());
  return false;
}

void Navigation::Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response) {
  if (inited_ == false) {
    response.set_ret(-1);
    response.set_info("navigation has not inited");
    printf(" navigation has not inited\n");
    return ;
  }
  if(newUnfinished_cations_.empty()==false)  //正在运行中，上一次指令未完成
  {
    response.set_ret(-2);
    response.set_info("navigation is running pls cancel before");
    printf(" navigation is running pls cancel before\n");
    return ;
  }
  //add 先检查当前点与起点的距离
  global_navCmd_=cmd;

  for (int i = 0; i < cmd.newactions_size(); ++i)
    newUnfinished_cations_.push(cmd.newactions(i));

  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){  //入库
      space_id_=act.spacenode().id();
      isInSpace_=true;
      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{
        isInSpace_=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() == 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) {
    response.set_ret(-3);
    response.set_info("navigation canceled");
    printf(" navigation canceled\n");
    while (newUnfinished_cations_.empty() == false)
      newUnfinished_cations_.pop();
  } else {
    if (newUnfinished_cations_.empty()) {
      response.set_ret(0);
      response.set_info("navigation completed!!!");
      printf("navigation completed!!!\n");
    }
    else {
      response.set_ret(-4);
      response.set_info("navigation Failed!!!!");
      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");
}

float Navigation::compute_cuv(const Pose2d& target)
{
  float x=fabs(target.x());
  float y=fabs(target.y());
  float cuv= atan(y/(x+1e-8));
  //printf(" ---------------------- cuv：%f\n",cuv);
  return cuv;
}