Agv_UI/dijkstraSample.cpp

//
// Created by zx on 22-11-29.
//

#include "dijkstra/dijkstra.h"
#include <pangolin/var/var.h>
#include <pangolin/var/varextra.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/display/display.h>
#include <pangolin/display/view.h>
#include <pangolin/display/widgets.h>
#include <pangolin/display/default_font.h>
#include <pangolin/handler/handler.h>

int start=1;
int end=5;


void DrawPath(PathMap& map,std::vector<int> path)
{
  float z=0;

}

void DrawMap(PathMap& map,std::vector<int> path,pangolin::GlFont* text_font)
{

  //绘制顶点
  std::map<int,PathMap::MapNode> nodes=map.GetNodes();
  std::map<int,PathMap::MapNode>::iterator it=nodes.begin();
  float z=0;

  glPointSize(50.0);
// 开始画点
  glBegin ( GL_POINTS );
  glColor3f(0, 1, 0);
  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, 1.0, 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);

  }

}

int main()
{

  pangolin::GlFont *text_font = new pangolin::GlFont("../config/tt0102m_.ttf", 30.0);
  pangolin::CreateWindowAndBind("Main", 1280, 960);

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


  // Define Camera Render Object (for view / scene browsing)
  pangolin::OpenGlRenderState s_cam(
          pangolin::ProjectionMatrix(1280, 960, 840, 840, 640, 640, 0.2, 100),
          pangolin::ModelViewLookAt(5, 3, 8, 5, 3, 0, pangolin::AxisY)
  );

  // Choose a sensible left UI Panel width based on the width of 20
  // charectors from the default font.
  const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();

  // Add named OpenGL viewport to window and provide 3D Handler
  pangolin::View &d_cam = pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 960.0f)
          .SetHandler(new pangolin::Handler3D(s_cam));

  // Add named Panel and bind to variables beginning 'ui'
  // A Panel is just a View with a default layout and input handling
  pangolin::CreatePanel("ui")
          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  // Safe and efficient binding of named variables.
  // Specialisations mean no conversions take place for exact types
  // and conversions between scalar types are cheap.
  pangolin::Var<bool> lvx_checkbox("ui.lvx_Checkbox", false, true);
  pangolin::Var<std::string> lvx_file("ui.lvx_file_String", "../map/lidarbag/start1.lvx");


  pangolin::Var<int> beg_Int("ui.beg", 1, 1, 18);
  pangolin::Var<int> end_Int("ui.end", 5, 1, 18);


  pangolin::Var<std::string> timequeue_string("ui.tq_String", "time string");

  pangolin::Var<std::string> path_string("ui.path", "cost");
  pangolin::Var<std::string> cost_string("ui.final_cost", "cost");



  // std::function objects can be used for Var's too. These work great with C++11 closures.
  pangolin::Var<std::function<void(void)>> save_window("ui.Save_Window", []() {
    pangolin::SaveWindowOnRender("window");
  });

  std::mutex mutex;
  std::vector<int> path;
  PathMap map;
  map.AddVertex(1, 0, 0);
  map.AddVertex(2, 3, 0);
  map.AddVertex(3, 6, 0);
  map.AddVertex(4, 9, 0);
  map.AddVertex(5, 9, 2);
  map.AddVertex(6, 6, 2);
  map.AddVertex(7, 3, 2);
  map.AddVertex(8, 0, 2);
  map.AddVertex(9, 0, 4);
  map.AddVertex(10, 3, 4);
  map.AddVertex(11, 6, 4);
  map.AddVertex(12, 9, 4);
  map.AddVertex(13, 9, 6);
  map.AddVertex(14, 6, 6);
  map.AddVertex(15, 3, 6);
  map.AddVertex(16, 0, 6);

  map.AddEdge(1, 2, false);
  map.AddEdge(1, 8, false);

  map.AddEdge(2, 3, false);
  map.AddEdge(2, 7, false);

  map.AddEdge(3, 4, false);
  map.AddEdge(3, 6, false);

  map.AddEdge(4, 5, false);

  map.AddEdge(5, 12, false);
  map.AddEdge(5, 6, false);

  map.AddEdge(6, 11, false);
  map.AddEdge(6, 7, false);

  map.AddEdge(7, 10, false);
  map.AddEdge(7, 8, false);

  map.AddEdge(8, 9, false);

  map.AddEdge(9, 10, false);
  map.AddEdge(9, 16, false);

  map.AddEdge(10, 11, false);
  map.AddEdge(10, 15, false);

  map.AddEdge(11, 12, false);
  map.AddEdge(11, 14, false);

  map.AddEdge(12, 13, false);
  map.AddEdge(13, 14, false);
  map.AddEdge(14, 15, false);
  map.AddEdge(15, 16, false);

  map.AddEdge(15, 17, false);
  map.AddEdge(14, 17, false);
  map.AddEdge(17, 18, false);

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

    auto start_tm = std::chrono::system_clock::now();

    start = beg_Int.Get();
    end = end_Int.Get();



    float distance = 0;
    mutex.lock();
    path.clear();
    map.GetShortestPath(start, end, path, distance);
    mutex.unlock();

    char path_str[64] = {0};
    sprintf(path_str, "%d-%d:", start, end);

    for (int i = path.size() - 1; i >= 0; --i)
    {
      sprintf(path_str + strlen(path_str), "%d-", path[i]);
    }
    sprintf(path_str + strlen(path_str), "%d", end);
    path_string = path_str;

    char cost[62] = {0};
    sprintf(cost, "d(%d-%d):%.3f", start, end, distance);
    cost_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;

  });

  // Demonstration of how we can register a keyboard hook to alter a Var
  pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'b', [&]() {
    //a_double = 3.5;

  });

  // Default hooks for exiting (Esc) and fullscreen (tab).

  double zoom = 1.0;
  while (!pangolin::ShouldQuit())
  {
    // Clear entire screen
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);


    if (d_cam.IsShown())
    {
      //s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(odom_pt[0],odom_pt[1],60, odom_pt[0],odom_pt[1],0, pangolin::AxisY));

      d_cam.Activate(s_cam);
      pangolin::glDrawAxis(1);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴

    }


    mutex.lock();
    std::vector<int> solve_path=path;
    mutex.unlock();
    std::reverse(solve_path.begin(),solve_path.end());
    solve_path.push_back(end);
    DrawMap(map, solve_path,text_font);


    // Swap frames and Process Events
    pangolin::FinishFrame();
    std::this_thread::sleep_for(std::chrono::microseconds(10));
  }

  delete text_font;


  return 0;
}