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 "tool/proto_tool.h"
#include <queue>
#include "map.pb.h"

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_;
TimedLockData<NavMessage::AGVStatu> timed_AgvStatu_;
TimedLockData<NavMessage::AGVStatu> timed_AgvBrotherStatu_;


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

bool InitMap(std::string config_file);


void pangolinSpinOnce(PangolinViewer* viewer) {

  if (timed_AgvStatu_.timeout() == false) {
    NavMessage::AGVStatu statu = timed_AgvStatu_.Get();

    Pose2d pose2d(statu.x(), statu.y(), statu.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.v(), statu.vth());
  }
  //显示另一节agv状态
  if (timed_AgvBrotherStatu_.timeout() == false) {
    NavMessage::AGVStatu statu = timed_AgvBrotherStatu_.Get();
    Pose2d pose2d(statu.x(), statu.y(), statu.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->ShowBrotherStatu(pose, statu.v(), statu.vth());
  }
  //绘制导航实时状态
  if (timed_navStatu_.timeout() == false) {
    NavMessage::NavStatu statu = timed_navStatu_.Get();
    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) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }

}

void navigation_pause()
{
  NavMessage::NavCmd cmd;
  cmd.set_action(1);  //暂停
  if(g_terminator) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }
}
void navigation_stop()
{
  NavMessage::NavCmd cmd;
  cmd.set_action(3);  //取消导航
  if(g_terminator) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }
}

void AgvStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::AGVStatu statu;
  if(msg.toProtoMessage(statu))
    timed_AgvStatu_.reset(statu,0.5);

}
void AgvBrotherStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::AGVStatu statu;
  if(msg.toProtoMessage(statu))
    timed_AgvBrotherStatu_.reset(statu,0.5);

}
void NavStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::NavStatu statu;
  if(msg.toProtoMessage(statu))
    timed_navStatu_.reset(statu,0.5);

}


int main(int argc, char** argv)
{
  if(argc<2)
  {
    printf(" argc must >2  exe mapfile\n");
    return -1;
  }
  std::string config_file=argv[1];
  if(InitMap(config_file)==false)
    return -1;


  //"agv1/nav_statu"
  g_terminator=new Terminator_emqx("UI_nav-child");

  if(g_terminator->Connect("192.168.0.70",1883)==false)
  {
    printf(" AGV emqx connect failed\n");
    return -1;
  }
  g_terminator->AddCallback("agv1_child/nav_statu",NavStatuArrived, nullptr);
  g_terminator->AddCallback("agv1_child/agv_statu",AgvStatuArrived, nullptr);
  g_terminator->AddCallback("agv1/agv_statu",AgvBrotherStatuArrived, nullptr);


  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)
{
  MapMessage::Map proto_map;
  if(proto_tool::read_proto_param(config_file,proto_map)==false)
  {
    printf(" read map proto failed\n");
    return false;
  }

  std::map<int,int> exist_id;
  for(int i=0;i<proto_map.nodes_size();++i)
  {
    MapMessage::Node node=proto_map.nodes(i);
    if(exist_id.find(node.id())!=exist_id.end())
    {
      printf("Can not add vertex, Node Id :%d exist\n",node.id());
      return false;
    }
    DijkstraMap.AddVertex(node.id(),node.x(),node.y());
    exist_id[node.id()]=0;
  }

  for(int i=0;i<proto_map.edges_size();++i)
  {
    MapMessage::Edge edge=proto_map.edges(i);
    if(exist_id.find(edge.id1())!=exist_id.end() && exist_id.find(edge.id2())!=exist_id.end())
    {
      if(false==DijkstraMap.AddEdge(edge.id1(),edge.id2(),edge.directed()))
        return false;
    }
    else
    {
      printf(" Cant add edge id1 or id2 (%d %d) is not exist\n",edge.id1(),edge.id2());
      return false;
    }
  }
  return true;
}


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.03);
  enddiff.set_y(0.05);
  enddiff.set_theta(0.5*M_PI/180.0);


  //调整到起点，朝向与当前朝向一致
  if(timed_AgvStatu_.timeout())
  {
    printf(" agv statu is timeout can not get current direction\n");
    return false;
  }
  float current_theta=timed_AgvStatu_.Get().theta();
  float x,y;
  DijkstraMap.GetVertexPos(nodes[0],x,y);

  NavMessage::SpeedLimit v_limit,angular_limit,horize_limit;
  v_limit.set_min(0.1);
  v_limit.set_max(0.2);
  horize_limit.set_min(0.05);
  horize_limit.set_max(0.2);
  angular_limit.set_min(1);
  angular_limit.set_max(10);

  NavMessage::SpeedLimit mpc_x_limit,mpc_angular_limit;
  mpc_x_limit.set_min(0.05);
  mpc_x_limit.set_max(1.0);

  mpc_angular_limit.set_min(0*M_PI/180.0);
  mpc_angular_limit.set_max(3*M_PI/180.0);

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

  act_head.mutable_velocity_limit()->CopyFrom(v_limit);
  act_head.mutable_horize_limit()->CopyFrom(horize_limit);
  act_head.mutable_angular_limit()->CopyFrom(angular_limit);
  cmd.add_actions()->CopyFrom(act_head);

  Pose2d begin;
  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));
    begin=Pose2d(lx,ly,theta);
    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);
    act_adjust.mutable_velocity_limit()->CopyFrom(v_limit);
    act_adjust.mutable_horize_limit()->CopyFrom(horize_limit);
    act_adjust.mutable_angular_limit()->CopyFrom(angular_limit);
    cmd.add_actions()->CopyFrom(act_adjust);

    //MPC 控制
    NavMessage::Action act_along;
    act_along.set_type(2);
    act_along.mutable_begin()->set_x(begin.x());
    act_along.mutable_begin()->set_y(begin.y());
    act_along.mutable_begin()->set_theta(begin.theta());
    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);
    act_along.mutable_velocity_limit()->CopyFrom(mpc_x_limit);
    act_along.mutable_angular_limit()->CopyFrom(mpc_angular_limit);
    cmd.add_actions()->CopyFrom(act_along);
  }
  return true;

}