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(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(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 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);
    terminator_->AddCallback(terminal_p.subbrotherstatutopic(),Navigation::BrotherAgvStatuCallback,this);

  }
  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(eReady); //默认ready
    statu.set_move_mode(move_mode_);
    NavMessage::Action *current_action = nullptr;
    if (running_)
    {
      statu.set_statu(eRunning);
      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信息
    if (current_action != nullptr) {
      delete current_action;
      //发布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());
        }
      }
    }
    MqttMsg msg;
    msg.fromProtoMessage(statu);
    if(terminator_)
      terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);

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

NavMessage::RobotStatu 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 robotStatu;
}

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;
    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) {
    printf(" msg transform to AGVStatu failed ..!!\n");
    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)
  {
    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)
{
  std::cout<<" adjust tarhet:"<<target<<"  diff:"<<target_diff<<std::endl;
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return false;
  }

  Pose2d thresh=Pose2d::abs(target_diff);

  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, target_diff)) {
      if(Stop()) {
        printf("adjust completed\n");
        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()) {
          double theta = limit(diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
          double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
          SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, angular);
          std::cout<<"current:"<<timedPose_.Get()<<"  target:"<<target<<" diff:"<<diff<<std::endl;
          printf(" Ratate | input anguar:%f,next angular:%f,target:%f\n",timedA_.Get(),angular,theta);
          continue;
        }
      }
      if (action == 2) {
        if (Pose2d::abs(diff).y() > thresh.y()) {
          double hv=limit(diff.y(),limit_h.min,limit_h.max);
          double velocity=next_speed(timedV_.Get(),hv,acc_v,dt);
          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal,velocity , 0);
          printf(" Horizontal input:%f out:%f\n",timedV_.Get(),velocity);
          continue;
        }
      }
      if (action == 3) {
        if (Pose2d::abs(diff).x() > thresh.x()) {
          double v=limit(diff.x(),limit_v.min,limit_v.max);
          double velocity=next_speed(timedV_.Get(),v,acc_v,dt);
          SendMoveCmd(move_mode_,Monitor_emqx::eMPC, velocity, 0);
          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::mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out)
{
  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);//计算另一节在当前小车坐标系下的坐标
  LoadedMPC MPC(relative,limit_v.min, limit_v.max);
  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;
  bool ret= MPC.solve(traj,traj[traj.size()-1],statu,out,selected_trajectory,optimize_trajectory);
  predict_traj_.reset(optimize_trajectory,1);
  selected_traj_.reset(selected_trajectory,1);
  return ret;
}

bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
                           const Pose2d& target,const Pose2d& diff)
{
  Pose2d distance=Pose2d::relativePose(target,cur);
    if(Pose2d::abs(distance)<diff)
    {
        if(fabs(velocity)<0.1 && 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_);
    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_);
    }
    return false;
  }
  return false;
}
bool Navigation::clamp_open() {
  if(monitor_) {
    monitor_->clamp_open(move_mode_);
    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_);
    }
    return false;
  }
  return false;
}

bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v)
{
  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;

  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))
    {
        if(Stop()) {
          printf(" exec along completed !!!\n");
          return true;
        }
    }
      //一次变速
      std::vector<double> out;
      bool ret;
      TimerRecord::Execute([&,this](){
        ret=mpc_once(traj,limit_v,out);
      },"mpc_once");
      if(ret==false)
      {
        Stop();
        return false;
      }
      /*
       * 判断车辆是否在终点附近徘徊，无法到达终点
       */
      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;
        if(execute_adjust_action(target,target_diff,limitv,limitv,limitR))
          return true;
        else
          return false;
      }

      SendMoveCmd(move_mode_,Monitor_emqx::eMPC,out[0],out[1]);
      //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;
  printf(" navigation beg...\n");

  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)
      {
        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) {
        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();
  }

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