Agv_UI/main.cpp

#include <csignal>

#include "pangolinViewer.h"

#include "dijkstra/dijkstra.h"
#include "define/timedlockdata.hpp"
#include "define/TimerRecord.h"
#include <glog/logging.h>
#include "dijkstra/trajectory.h"
#include "emqx/terminator_emqx.h"
#include "tool/pathcreator.h"
#include <queue>


Terminator_emqx* g_terminator= nullptr;
//路径相关
int node_beg=1,node_end=6;
std::mutex pathMutex;
std::vector<int> shortest_path;
PathMap DijkstraMap;

//控制相关
TimedLockData<NavMessage::NavStatu> timed_navStatu_;


bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs);

bool InitMap(std::string config_file);
bool InitDijkstraMap();

void pangolinSpinOnce(PangolinViewer* viewer) {

  if (timed_navStatu_.timeout() == false) {
    NavMessage::NavStatu statu = timed_navStatu_.Get();
    if (statu.pose_timeout() == false && statu.twist_timeout() == false) {
      Pose2d pose2d(statu.current_pose().x(), statu.current_pose().y(), statu.current_pose().theta());
      Eigen::Matrix3d rotation_matrix3;
      rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
                         Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                         Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

      Eigen::Matrix4d pose;
      pose.topLeftCorner(3, 3) = rotation_matrix3;
      pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
      viewer->ShowRealtimeStatu(pose, statu.current_pose().v(), statu.current_pose().vth());
    }

    Trajectory select_traj;
    Trajectory predict_traj;
    for (int i = 0; i < statu.selected_traj().poses_size(); ++i) {
      select_traj.push_point(Pose2d(statu.selected_traj().poses(i).x(),
                                    statu.selected_traj().poses(i).y(),
                                    statu.selected_traj().poses(i).theta()));
    }
    for (int i = 0; i < statu.predict_traj().poses_size(); ++i) {
      predict_traj.push_point(Pose2d(statu.predict_traj().poses(i).x(),
                                     statu.predict_traj().poses(i).y(),
                                     statu.predict_traj().poses(i).theta()));
    }
    viewer->DrawTrajectory(select_traj, 1, 0, 0, 12);
    viewer->DrawTrajectory(predict_traj, 0, 1, 1, 15);
  }


  //绘制dijkstra地图及最短路径
  pathMutex.lock();
  std::vector<int> solve_path = shortest_path;
  pathMutex.unlock();
  std::reverse(solve_path.begin(), solve_path.end());
  solve_path.push_back(node_end);
  viewer->DrawDijkastraMap(DijkstraMap, solve_path);

}

bool OnDijkstraBtn(int beg,int end,float& distance,
        std::vector<int>& shortestPath)
{
  bool ret= DijkstraMap.GetShortestPath(beg, end, shortestPath, distance);
  if(ret)
  {
    node_beg=beg;
    node_end=end;
    pathMutex.lock();
    shortest_path = shortestPath;
    pathMutex.unlock();
  }
  return ret;
}

bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);

void navigation_start()
{
  pathMutex.lock();
  std::vector<int> solve_path=shortest_path;
  pathMutex.unlock();
  std::reverse(solve_path.begin(),solve_path.end());
  solve_path.push_back(node_end);

  //std::queue<Trajectory> global_trajectorys;
  //Create_trajectoris(DijkstraMap,solve_path,global_trajectorys);

  //发布指令
  NavMessage::NavCmd cmd;
  if(false==CreateNavCmd(solve_path,cmd))
    return;
  std::cout<<" ------------------------------------new NavCmd ---------------------------"<<std::endl;
  std::cout<<cmd.DebugString()<<std::endl<<std::endl;
  if(g_terminator)
    g_terminator->SendNavCmd(cmd);

}

void navigation_pause()
{
  NavMessage::NavCmd cmd;
  cmd.set_action(1);  //暂停
  if(g_terminator)
    g_terminator->SendNavCmd(cmd);
}
void navigation_stop()
{
  NavMessage::NavCmd cmd;
  cmd.set_action(3);  //取消导航
  if(g_terminator)
    g_terminator->SendNavCmd(cmd);
}


void NavStatuArrived(const NavMessage::NavStatu& statu,void* context)
{
  timed_navStatu_.reset(statu,0.5);

}

int main(int argc, char** argv)
{
  //reg参数
  std::string config_file="../config/horizon_config.yaml";
  if(InitMap(config_file)==false)
    return -1;

  if(!InitDijkstraMap())
  {
    printf("Dijkstra Map load failed\n");
    return -2;
  }

  g_terminator=new Terminator_emqx("UI_nav","agv1/nav_cmd","agv1/nav_statu");
  g_terminator->set_statu_arrived(NavStatuArrived, nullptr);
  if(g_terminator->Connect("192.168.2.100",1883)==false)
  {
    printf(" AGV emqx connect failed\n");
    return -1;
  }


  PangolinViewer* viewer=PangolinViewer::CreateViewer();
  viewer->SetCallbacks(pangolinSpinOnce,OnDijkstraBtn);
  viewer->SetNavigationCallbacks(navigation_start,navigation_pause,navigation_stop);
  viewer->Join();

  TimerRecord::PrintAll();

  delete viewer;
  delete g_terminator;
  return 0;
}

bool InitMap(std::string config_file)
{
  return true;
}

bool InitDijkstraMap()
{
  DijkstraMap.AddVertex(1, 0, 0);
  DijkstraMap.AddVertex(2, 20, 0);
  DijkstraMap.AddVertex(3, 60, 0);
  DijkstraMap.AddVertex(4, 100, 0);
  DijkstraMap.AddVertex(5, 100, -16);
  DijkstraMap.AddVertex(6, 60, -16);
  DijkstraMap.AddVertex(7, 20, 20);

  bool ret=true;
  ret=ret&&DijkstraMap.AddEdge(1, 2, false);
  ret=ret&&DijkstraMap.AddEdge(1, 3, false);
  ret=ret&&DijkstraMap.AddEdge(1, 4, false);
  ret=ret&&DijkstraMap.AddEdge(2, 3, false);
  ret=ret&&DijkstraMap.AddEdge(2, 4, false);
  ret=ret&&DijkstraMap.AddEdge(2, 7, false);
  ret=ret&&DijkstraMap.AddEdge(3, 4, false);
  ret=ret&&DijkstraMap.AddEdge(3, 6, false);
  ret=ret&&DijkstraMap.AddEdge(4, 5, false);
  ret=ret&&DijkstraMap.AddEdge(5, 6, false);
  return ret;
}

bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
{
  if(nodes_id.size()<2)
    return false;
  for(int i=0;i<nodes_id.size()-1;++i)
  {

    float x1,y1,x2,y2;
    map.GetVertexPos(nodes_id[i],x1,y1);
    map.GetVertexPos(nodes_id[i+1],x2,y2);

    Pose2d start(x1,y1,0);
    Pose2d end(x2,y2,0);
    Trajectory traj=Trajectory::create_line_trajectory(start,end,0.05);
    if(traj.size()>1)
      trajs.push(traj);
    else
      return false;

  }
  return true;
}

bool CreateNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
{

  if(nodes.size()==0)
    return false;

  cmd.set_action(0);  //开始新导航
  char key[255]={0};
  static int N=1111;
  sprintf(key,"abcd-efg-%d",N++);
  cmd.set_key(key);

  NavMessage::Pose2d headdiff,enddiff;
  headdiff.set_x(0.1);
  headdiff.set_y(0.1);
  headdiff.set_theta(2*M_PI/180.0);

  enddiff.set_x(0.05);
  enddiff.set_y(0.03);
  enddiff.set_theta(0.5*M_PI/180.0);

  //调整到起点，朝向与当前朝向一致
  if(timed_navStatu_.timeout()|| timed_navStatu_.Get().pose_timeout())
  {
    printf(" agv statu is timeout can not get current direction\n");
    return false;
  }
  float current_theta=timed_navStatu_.Get().current_pose().theta();
  float x,y;
  DijkstraMap.GetVertexPos(nodes[0],x,y);
  NavMessage::Action act_head;
  act_head.set_type(1);
  act_head.mutable_target()->set_x(x);
  act_head.mutable_target()->set_y(y);
  act_head.mutable_target()->set_theta(current_theta);
  act_head.mutable_target_diff()->CopyFrom(headdiff);
  cmd.add_actions()->CopyFrom(act_head);

  for(int i=1;i<nodes.size();++i)
  {
    //调整到上一点位姿，朝向当前点
    float lx,ly;
    DijkstraMap.GetVertexPos(nodes[i-1],lx,ly);
    DijkstraMap.GetVertexPos(nodes[i],x,y);
    float theta=Trajectory::gen_axis_angle(Pose2d(lx,ly,0),Pose2d(x,y,0));
    NavMessage::Action act_adjust;
    act_adjust.set_type(1);
    act_adjust.mutable_target()->set_x(lx);
    act_adjust.mutable_target()->set_y(ly);
    act_adjust.mutable_target()->set_theta(theta);
    act_adjust.mutable_target_diff()->CopyFrom(headdiff);
    cmd.add_actions()->CopyFrom(act_adjust);

    //MPC 控制
    NavMessage::Action act_along;
    act_along.set_type(2);
    act_along.mutable_target()->set_x(x);
    act_along.mutable_target()->set_y(y);
    act_along.mutable_target()->set_theta(theta);
    if(i==nodes.size()-1) //最后一个节点
      act_along.mutable_target_diff()->CopyFrom(enddiff);
    else
      act_along.mutable_target_diff()->CopyFrom(headdiff);
    cmd.add_actions()->CopyFrom(act_along);
  }
  return true;

}