Agv_UI/pangolinViewer.cpp

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

#include "pangolinViewer.h"
PangolinViewer* PangolinViewer::viewer_= nullptr;

PangolinViewer* PangolinViewer::CreateViewer()
{
  if (viewer_== nullptr)
  {
    viewer_=new PangolinViewer();
  }
  return viewer_;
}
PangolinViewer::~PangolinViewer()
{
  if(thread_!= nullptr)
  {
    if (thread_->joinable())
      thread_->join();
    delete thread_;
    thread_= nullptr;
  }
}


PangolinViewer::PangolinViewer()
{
    thread_=new std::thread(&PangolinViewer::loop,this);
}

void PangolinViewer::Join()
{
  if(thread_!= nullptr)
  {
    if (thread_->joinable())
      thread_->join();
  }
}

void PangolinViewer::SetAutoTestNavCallback(AutoTestBtnCallback callback)
{
  navigation_AutoBtn_callback_=callback;
}

void PangolinViewer::SetNavigationCallbacks(StartBtnCallback startBtn,
                                            PauseBtnCallback pauseBtn,
                            StopBtnCallback stopBtn)
{
  naviagtion_startBtn_callback_=startBtn;
  naviagtion_pauseBtn_callback_=pauseBtn;
  navigation_stopBtn_callback_=stopBtn;
}

void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,DijkstraBtnCallback dijkstra)
{
  spinOnce_callback_=spinOnce;
  dijkstraBtn_callback_=dijkstra;
}

void PangolinViewer::ResetPathInfo(int beg,int end,float distance)
{
  *beg_Int_=beg;
  *end_Int_=end;
  char cost[64]={0};
  sprintf(cost, "d(%d-%d):%f", beg, end, distance);
  *distance_string_=cost;
}

void PangolinViewer::Init()
{
  pangolin::CreateWindowAndBind("Main", 1280, 760);
  // 3D Mouse handler requires depth testing to be enabled
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);   // 启用混合
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  text_font_ = new pangolin::GlFont("../config/tt0102m_.ttf", 30.0);
   s_cam_=new pangolin::OpenGlRenderState(
          pangolin::ProjectionMatrix(1480, 760, 840, 840, 480, 380, 0.1, 1000),
          pangolin::ModelViewLookAt(25, 0, 90, 25, 0, 20, pangolin::AxisY)
  );
  const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();
  d_cam_ = &pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 760.0f)
          .SetHandler(new pangolin::Handler3D(*s_cam_));
  pangolin::CreatePanel("ui")
          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));


  timequeue_string_=new pangolin::Var<std::string>("ui.tq_String", "timequeue_string");
  xyz_string_=new pangolin::Var<std::string>("ui.xyz_String", "xyz");
  rpy_string_=new pangolin::Var<std::string>("ui.rpy_String", "rpy");
  velocity_string_=new pangolin::Var<std::string>("ui.velocity", "");

  beg_Int_=new pangolin::Var<int>("ui.beg", 1, 0, 11);
  end_Int_=new pangolin::Var<int>("ui.end", 10, 0, 11);
  v_=new pangolin::Var<double>("ui.v", 1.0, -2.0,2.0 );
  a_=new pangolin::Var<double>("ui.a", 1.0, -10.0, 10.0);
  path_string_=new pangolin::Var<std::string>("ui.path", "cost");
  distance_string_=new pangolin::Var<std::string>("ui.distance", "distance");

  pangolin::Var<std::function<void(void)>> solve_view("ui.Solve", [&]()
  {
    if(dijkstraBtn_callback_!= nullptr)
    {
      auto start_tm = std::chrono::system_clock::now();
      int node_beg = beg_Int_->Get();
      int node_end = end_Int_->Get();
      float distance=std::numeric_limits<float>().max();
      std::vector<int> shortestPath;
      if(dijkstraBtn_callback_(node_beg,node_end,distance,shortestPath))
      {
        shortest_path_=shortestPath;

        char path_str[64] = {0};
        sprintf(path_str, "%d-%d:", node_beg, node_end);
        for (int i = shortestPath.size() - 1; i >= 0; --i)
        {
          sprintf(path_str + strlen(path_str), "%d-", shortestPath[i]);
        }
        sprintf(path_str + strlen(path_str), "%d", node_end);
        *path_string_=path_str;

        char cost[62] = {0};
        sprintf(cost, "d(%d-%d):%f", node_beg, node_end, distance);
        *distance_string_ = cost;

        auto end_tm = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_tm - start_tm);
        double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
        char tm[32] = {0};
        sprintf(tm, "time:%.5fs", time);
        *timequeue_string_ = tm;
      }

    }

  });


  pangolin::Var<std::function<void(void)>> navigation_beg("ui.nav_Beg", [&]()
  {

    if(naviagtion_startBtn_callback_!= nullptr)
      naviagtion_startBtn_callback_();
  });

  pangolin::Var<std::function<void(void)>> navigation_pause("ui.nav_PAUSE", [&]()
  {

      if(naviagtion_pauseBtn_callback_!= nullptr)
        naviagtion_pauseBtn_callback_();
  });

  pangolin::Var<std::function<void(void)>> navigation_end("ui.nav_End", [&]()
  {

    if(navigation_stopBtn_callback_!= nullptr)
      navigation_stopBtn_callback_();
  });

  pangolin::Var<std::function<void(void)>> navigation_auto_test("ui.nav_auto_test", [&]()
  {

      if(navigation_AutoBtn_callback_!= nullptr)
        navigation_AutoBtn_callback_();
  });


}

void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
               double alpha,double psize)
{
  glPointSize(psize);
  glBegin(GL_POINTS);
  glColor4f(r, g, b, alpha);
  for (int i = 0; i < cloud->size(); ++i)
  {
    PointType pt = cloud->points[i];
    glVertex3d(pt.x , pt.y , pt.z );

  }
  glEnd();
}

void PangolinViewer::DrawTrajectory(const Trajectory& traj,
        double r,double g,double b,double ptsize)
{
  glPointSize(ptsize);
  glBegin(GL_POINTS);
  glColor4f(r, g, b, 1);
  for (int i = 0; i < traj.size(); ++i)
  {
    Pose2d pt =traj[i];
    glVertex3d(pt.x() , pt.y() , 0);

  }
  glEnd();
}

void PangolinViewer::DrawDijkastraMap(PathMap& map,std::vector<int> path)
{
  //绘制顶点
  std::map<int,PathMap::MapNode> nodes=map.GetNodes();
  std::map<int,PathMap::MapNode>::iterator it=nodes.begin();
  float z=0;

  glPointSize(10.0);
// 开始画点
  glBegin ( GL_POINTS );
  glColor3f(1, 0, 1);
  for(it;it!=nodes.end();it++)
  {
    PathMap::MapNode node=it->second;
    // 设置点颜色，rgb值，float类型
    glVertex3f(node.x, node.y, z);
  }
  glEnd();

  glLineWidth(5);
  glBegin(GL_LINES);
  //绘制边
  glColor3f(1.0, 0.9, 0.0);
  std::vector<PathMap::MapEdge> edges=map.GetEdges();
  for(int i=0;i<edges.size();++i)
  {
    PathMap::MapEdge edge=edges[i];
    float x1,y1,x2,y2;
    map.GetVertexPos(edge.id1,x1,y1);
    map.GetVertexPos(edge.id2,x2,y2);

    glVertex3d(x1,y1, z);
    glVertex3d(x2,y2,z);
  }

  glEnd();

  if(path.size()>0)
  {
    glLineWidth(10);
    glBegin(GL_LINE_STRIP);
    glColor3f(0, 0, 1);
    for (int i = 0; i < path.size() ; ++i)
    {
      float x1, y1;
      if (map.GetVertexPos(path[i], x1, y1))
      {
        glVertex3d(x1, y1, z);
      }

    }
    glEnd();
  }

  glColor3f(1,1,1);
  it=nodes.begin();
  for(it;it!=nodes.end();it++)
  {
    PathMap::MapNode node=it->second;
    char idx[16]={0};
    sprintf(idx,"%d",it->first);
    text_font_->Text(idx).Draw(node.x,node.y,z);
  }

}

void PangolinViewer::ShowBrotherStatu(Eigen::Matrix4d pose,double velocity,double angular)
{
  double l=2;
  Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
  Eigen::Vector3d Xw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(1,0,0))+Ow;
  Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
  Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;

  glLineWidth(5);
  glBegin(GL_LINES);
  glColor3f(1.0, 0, 0.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Xw[0], Xw[1], Xw[2]);
  glColor3f(1.0, 1.0, 1.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Yw[0], Yw[1], Yw[2]);
  glColor3f(0, 1.0, 1.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Zw[0], Zw[1], Zw[2]);
  glEnd();

}

void PangolinViewer::ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,double angular)
{
  double l=10;
  Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
  Eigen::Vector3d Xw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(1,0,0))+Ow;
  Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
  Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;

  glLineWidth(5);
  glBegin(GL_LINES);
  glColor3f(1.0, 0.0, 0.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Xw[0], Xw[1], Xw[2]);
  glColor3f(0.0, 1.0, 0.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Yw[0], Yw[1], Yw[2]);
  glColor3f(0.0, 0.0, 1.0);
  glVertex3d(Ow[0], Ow[1], Ow[2]);
  glVertex3d(Zw[0], Zw[1], Zw[2]);
  glEnd();


  Eigen::Matrix3d rotation = pose.topLeftCorner(3, 3);
  Eigen::Vector3d eulerAngle = rotation.eulerAngles(0, 1, 2);
  Eigen::Vector3d t = pose.topRightCorner(3, 1);
  char rpy[64] = {0};
  sprintf(rpy,
            "%.3f,%.3f,%.3f",
            eulerAngle[0] * 180.0 / M_PI,
            eulerAngle[1] * 180.0 / M_PI,
            eulerAngle[2] * 180.0 / M_PI);
  char translation[64] = {0};
  sprintf(translation, "%.3f,%.3f,%.3f", t[0], t[1], t[2]);
  *xyz_string_ = translation;
  *rpy_string_ = rpy;
  char twist[64]={0};
  sprintf(twist,"vel:%.5f, \tagl:%.5f\n",velocity,angular);
  *velocity_string_=twist;

}


void PangolinViewer::loop(PangolinViewer* p)
{
  if(p== nullptr)
    return ;
  p->Init();
  while (!pangolin::ShouldQuit())
  {
    // Clear entire screen
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    /*if (pangolin::Pushed(*p->start_button_))
    {
      if (*p->lvx_checkbox_)
      {
        std::string file = p->lvx_file_->Get();

      }
      else
      {

      }
    }
    if (pangolin::Pushed(*p->stop_button_))
    {

    }


    if (p->freq_double_->GuiChanged())
    {
      double publish_freq=p->freq_double_->Get();

    }*/

    if (p->d_cam_->IsShown())
    {
      p->d_cam_->Activate(*p->s_cam_);
      pangolin::glDrawAxis(3);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴
      if(p->spinOnce_callback_!= nullptr)
        p->spinOnce_callback_(p);

    }
    pangolin::FinishFrame();
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
}