lio-cpp/MPC/navigation.cpp

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

#include "navigation.h"
#include "loaded_mpc.h"

Navigation* Navigation::navigation_=nullptr;
Navigation* Navigation::GetInstance()
{
  if(navigation_== nullptr)
  {
    navigation_=new Navigation();
  }
  return navigation_;
}
Navigation::~Navigation()
{

}

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

bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
{
  if(running_==true)
  {
    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_trajectorys_=global_trajectorys;
    thread_=new std::thread(&Navigation::navigatting,this);
    return true;

}
void Navigation::Cancel()
{
  cancel_=true;
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
    {
      thread_->join();
      delete thread_;
      thread_= nullptr;
    }
  }
}
void Navigation::Pause()
{
  pause_=true;
}


Navigation::Navigation()
{
}


// 待完成
bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
{
  Pose2d thresh=Pose2d::abs(distance);
  while(cancel_==false)
  {
    if (pause_ == true)
    {
      //发送暂停消息
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      continue;
    }
    if (timedPose_.timeout() == true)
    {
      printf(" navigation Error:Pose is timeout \n");
      break;
    }

    //计算目标pose在当前pose下的pose
    Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
    if (Pose2d::abs(diff) < thresh)
    {
      printf(" Move to pt complete\n");
      break;
    }
    //先旋转使得Y=0摆直，再移动x，再X最后旋转
    if(Pose2d::abs(diff).y()>thresh.y())
    {
      printf("旋转 diff.y:%.5f\n",diff.y());

    }else if(Pose2d::abs(diff).x()>thresh.x()){
      printf("y移动 diff.x():%.5f\n",diff.x());
    }
    else if(Pose2d::abs(diff).theta()>thresh.theta()){
      printf("旋转 diff.theta():%.5f\n",diff.theta());
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  if(cancel_==true)
    return false;
  return true;
}

//待完成
bool Navigation::mpc_once(const Trajectory& traj)
{
  if(timedPose_.timeout()==true)
  {
    printf(" MPC once Error:Pose is timeout \n");
    return false;
  }
  if(traj.size()==0)
  {
    return false;
  }

  Pose2d pose=timedPose_.Get();
  double velocity=0;    //从plc获取状态
  double angular=0;
  LoadedMPC MPC;
  Eigen::VectorXd statu;//agv状态
  statu<<pose.x(),pose.y(),pose.theta(),velocity,angular;

  std::vector<double> out;
  Trajectory optimize_trajectory;
  return MPC.solve(traj,traj[traj.size()-1],statu,out,optimize_trajectory);
}

bool Navigation::execute_child_trajectory(const Trajectory& child)
{
  if(child.size()==0)
    return true;
  Pose2d head,tail;

  Pose2d normal_diff(0.2,0.2,10.*M_PI/180.0);
  Pose2d mpc_diff(0.2,0.2,0.5*M_PI/180.0);

  Trajectory traj=child;
  while(cancel_==false)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if(pause_==true)
    {
      //发送暂停消息
      continue;
    }

    if(timedPose_.timeout()==true)
    {
      printf(" navigation Error:Pose is timeout \n");
      break;
    }

    if(Pose2d::abs(timedPose_.Get()-tail)<mpc_diff)
    {
      printf(" navigation completed!!!\n");
      return true;
    }

    head=child[0];
    tail=child[child.size()-1];
    //如果与起点差距过大先运动到起点
    if(false==Pose2d::abs(timedPose_.Get()-traj[0])<normal_diff)
    {
      if (false == moveToPoint(head, normal_diff))
        return false;
      else
        continue;
    }
    else{
      //一次变速
      if(mpc_once(traj)==false)
      {
        return false;
      }
    }

  }
  return false;
}

void Navigation::navigatting()
{
  pause_=false;
  cancel_=false;
  running_=true;
  printf(" navigation beg...\n");
  while(global_trajectorys_.empty()==false)
  {
    Trajectory traj=global_trajectorys_.front();
    if(execute_child_trajectory(traj))
    {
      global_trajectorys_.pop();
    }
    else
    {
      break;
    }
  }
  if(cancel_==true)
    printf(" navigation sia canceled\n");
  printf(" navigation end...\n");
  if(global_trajectorys_.size()==0)
    printf("navigation completed!!!\n");
  running_=false;
}