navigation ...

CMakeLists.txt

@@ -80,7 +80,7 @@ add_definitions(${PCL_DEFINITIONS})
 #---------------------------------------------------------------------------------------
 add_executable(${PROJECT_NAME}_node
 
-		lio/src/PoseEstimation.cpp
+		main.cpp
 		lio/src/segment/LidarFeatureExtractor.cpp
 		lio/src/segment/segment.cpp
 		lio/src/segment/pointsCorrect.cpp
@@ -91,11 +91,12 @@ add_executable(${PROJECT_NAME}_node
 		lio/src/lio/ceresfunc.cpp
 		lio/src/lio/ndtMap.cpp
 
-		define/time_data.cpp
+		pangolinViewer.cpp
 		dijkstra/dijkstra.cpp
 		MPC/loaded_mpc.cpp
 		MPC/trajectory.cpp
 		MPC/pose2d.cpp
+		MPC/navigation.cpp
 
 		lio/src/utils/TimerRecord.cpp
 

MPC/navigation.cpp

@@ -0,0 +1,108 @@
+//
+// Created by zx on 22-12-2.
+//
+
+#include "navigation.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)
+  {
+    printf(" navigation is running pls cancel before\n");
+    return false;
+  }
+  if(thread_==nullptr)
+  {
+    //add 先检查当前点与起点的距离
+    global_trajectorys_=global_trajectorys;
+    thread_=new std::thread(&Navigation::navigatting,this);
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+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::execute_child_trajectory(const Trajectory& child)
+{
+  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;
+    }
+
+
+
+  }
+
+  return false;
+}
+
+void Navigation::navigatting(Navigation* p)
+{
+  p->pause_=false;
+  p->cancel_=false;
+  p->running_=true;
+  while(p->global_trajectorys_.empty()==false)
+  {
+    Trajectory traj=p->global_trajectorys_.front();
+    if(p->execute_child_trajectory(traj))
+    {
+      p->global_trajectorys_.pop();
+    }
+    else
+    {
+      break;
+    }
+  }
+  p->running_=false;
+}

MPC/navigation.h

@@ -0,0 +1,40 @@
+//
+// Created by zx on 22-12-2.
+//
+
+#ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
+#define LIO_LIVOX_CPP_MPC_NAVIGATION_H_
+#include "../define/typedef.h"
+#include "../define/timedlockdata.hpp"
+#include "pose2d.h"
+#include "trajectory.h"
+#include <queue>
+#include <thread>
+class Navigation
+{
+ public:
+    static Navigation* GetInstance();
+    ~Navigation();
+
+    void ResetPose(const Pose2d& pose);
+    bool Start(std::queue<Trajectory> global_trajectorys);
+    void Cancel();
+    void Pause();
+
+ protected:
+    Navigation();
+    bool execute_child_trajectory(const Trajectory& child);
+    static void navigatting(Navigation* p);
+ protected:
+    static Navigation* navigation_;
+    std::mutex  mtx_;
+    TimedLockData<Pose2d> timedPose_;           //当前位姿
+    std::queue<Trajectory> global_trajectorys_; //多段轨迹
+    std::thread* thread_= nullptr;
+    bool running_=false;
+    bool pause_=false;
+    bool cancel_=false;
+};
+
+
+#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_

MPC/trajectory.h

@@ -29,7 +29,6 @@ class Trajectory
     std::vector<Pose2d>             m_pose_vector;
 };
 
-
 /*
  * 根据梯度和方向计算角度,x轴正，逆时针为正
  */

define/time_data.cpp

@@ -1,36 +0,0 @@
-//
-// Created by zx on 22-12-1.
-//
-
-#include "time_data.h"
-
-template <class T>
-TimedLockData<T>::TimedLockData()
-{
-  timeout_=0.1;
-}
-
-template <class T>
-void TimedLockData<T>::reset(const T& tdata,double timeout)
-{
-  std::unique_lock lock(mutex_);
-  data_=tdata;
-  timeout_=timeout;
-  tp_=std::chrono::steady_clock::now();
-}
-
-template <class T>
-bool TimedLockData<T>::timeout()
-{
-  auto now=std::chrono::steady_clock::now();
-  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - tp_);
-  double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
-  return time>timeout_;
-}
-
-template <class T>
-T& TimedLockData<T>::Data()
-{
-  std::unique_lock lock(mutex_);
-  return data_;
-}

define/time_data.h

@@ -1,27 +0,0 @@
-//
-// Created by zx on 22-12-1.
-//
-
-#ifndef LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
-#define LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
-#include <chrono>
-#include <mutex>
-
-template <class T>
-class TimedLockData
-{
- public:
-    TimedLockData();
-    void reset(const T& tdata,double timeout=0.1);
-    bool timeout();
-    T& Data();
- protected:
-    T data_;
-    std::chrono::steady_clock::time_point tp_;
-    std::mutex mutex_;
-    double timeout_;
-
-};
-
-
-#endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

define/timedlockdata.hpp

@@ -0,0 +1,59 @@
+//
+// Created by zx on 22-12-1.
+//
+
+#ifndef LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#define LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_
+#include <chrono>
+#include <mutex>
+
+template <typename T>
+class TimedLockData
+{
+ public:
+    TimedLockData();
+    void reset(const T& tdata,double timeout=0.1);
+    bool timeout();
+    T Get();
+
+ protected:
+    T data_;
+    std::chrono::steady_clock::time_point tp_;
+    std::mutex mutex_;
+    double timeout_;
+
+};
+
+template <typename T>
+TimedLockData<T>::TimedLockData()
+{
+  timeout_=0.1;
+}
+
+template <typename T>
+void TimedLockData<T>::reset(const T& tdata,double timeout)
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  data_=tdata;
+  timeout_=timeout;
+  tp_=std::chrono::steady_clock::now();
+}
+
+template <typename T>
+bool TimedLockData<T>::timeout()
+{
+  auto now=std::chrono::steady_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - tp_);
+  double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+  return time>timeout_;
+}
+
+template <typename T>
+T TimedLockData<T>::Get()
+{
+  std::lock_guard<std::mutex> lck (mutex_);
+  return data_;
+}
+
+
+#endif //LIO_LIVOX_CPP_DEFINE_TIME_DATA_H_

dijkstra/dijkstra.cpp

@@ -137,6 +137,7 @@ bool PathMap::AddEdge(int id1,int id2,bool directed)
     float distance=sqrt(pow(x2-x1,2)+pow(y2-y1,2));
     grapher_->insert_edge(id1, id2, distance, directed);
     edge_.push_back({id1,id2,directed});
+    printf(" Add edge %d-%d \n",id1,id2);
     return true;
   }
   return false;

lio/src/PoseEstimation.cpp

@@ -1,598 +0,0 @@
-
-
-#include <csignal>
-
-
-#include "lio/Estimator.h"
-#include "lio/mapper.h"
-
-#include "lddc.h"
-#include "lds_hub.h"
-#include "lds_lidar.h"
-#include "lds_lvx.h"
-#include "livox_sdk.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>
-#include "segment/LidarFeatureExtractor.h"
-
-#include "../dijkstra/dijkstra.h"
-#include "time_data.h"
-
-//雷达相关
-livox_ros::Lddc *lddc= nullptr;
-//特征提取相关
-LidarFeatureExtractor* lidarFeatureExtractor;
-pcl::PointCloud<PointType>::Ptr laserCloud;
-int Lidar_Type=0;
-int N_SCANS = 6;
-bool Feature_Mode = false;
-bool Use_seg = false;
-
-//slam 地图相关
-Mapper* pMaper= nullptr;
-
-//路径相关
-std::mutex pathMutex;
-std::vector<int> shortestPath;
-PathMap DijkstraMap;
-int node_beg=1;
-int node_end=5;
-
-
-void SigHandle(int sig) {
-  printf("catch sig %d\n", sig);
-  TimerRecord::PrintAll();
-}
-
-class LockCloud
-{
- public:
-    LockCloud()
-    {
-        cloud_.reset(new pcl::PointCloud<PointType>);
-    }
-    void Lock(){
-        lock_.lock();
-    }
-    void unlock(){
-        lock_.unlock();
-    }
-    pcl::PointCloud<PointType>::Ptr Get()
-    {
-        return cloud_;
-    }
-    void Set(pcl::PointCloud<PointType>::Ptr cloud)
-    {
-        lock_.lock();
-        cloud_=cloud;
-        lock_.unlock();
-    }
-
- protected:
-    std::mutex lock_;
-    pcl::PointCloud<PointType>::Ptr cloud_;
-};
-
-LockCloud scan_cloud;
-LockCloud localmap_cloud;
-Eigen::Matrix4d cur_pose;
-double pose_line_length=2.0;
-double final_cost=0;
-
-void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
-{
-    scan_cloud.Set(cloud);
-}
-
-void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
-{
-    localmap_cloud.Set(cloud);
-}
-
-
-void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
-{
-    cur_pose=tranform;
-}
-
-void FinalCostCallback(double cost)
-{
-  final_cost=cost;
-}
-
-void CloudCallback(TimeCloud tc,int handle)
-{
-    double timeSpan =tc.cloud->points.back().normal_x;
-    laserCloud.reset(new pcl::PointCloud<PointType>);
-    laserCloud->reserve(15000*N_SCANS);
-
-    PointType point;
-    for(const auto& p : tc.cloud->points)
-    {
-
-        //std::cout<<point.normal_y<<"   "<<point.normal_x<<"   "<<timeSpan<<std::endl;
-        if (p.normal_y > N_SCANS - 1) continue;
-        if (p.x < 0.01) continue;
-        if (!pcl_isfinite(p.x) ||
-                !pcl_isfinite(p.y) ||
-                !pcl_isfinite(p.z))
-        {
-            continue;
-        }
-        point.x=p.x;
-        point.y=p.y;
-        point.z=p.z;
-        point.normal_x = p.normal_x / timeSpan; //normao_x  time
-        point.intensity=p.intensity;
-        int line=int(p.normal_y);
-        point.normal_y=LidarFeatureExtractor::_int_as_float(line);
-        laserCloud->push_back(point);
-      /*std::cout<<" x:"<<point.x<<" y:"<<point.y<<" z:"<<point.z
-               <<" intnesity:"<<point.intensity<<" normal_x:"<<point.normal_x
-               <<" normal_y:"<<point.normal_y<<std::endl;*/
-    }
-
-    tc.cloud=laserCloud;
-    pMaper->AddCloud(tc);
-
-}
-
-
-void ImuDataCallback(ImuData imu,int handle){
-  pMaper->AddImu(imu);
-}
-
-
-
-bool InitLidar(double freq);
-bool InitMap(std::string config_file);
-bool InitDijkstraMap();
-void DrawPose(Eigen::Matrix4d pose);
-void DrawMap(PathMap& map,std::vector<int> path,pangolin::GlFont* text_font);
-void DrawCloud(pcl::PointCloud<PointType>::Ptr cloud,
-        double r,double g,double b,double psize,double alpha=1.0);
-
-int main(int argc, char** argv)
-{
-
-  //reg参数
-  std::string config_file="../config/horizon_config.yaml";
-  if(InitMap(config_file)==false)
-    return -1;
-
-
-  signal(SIGINT, SigHandle);
-
-  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);
-
-  pangolin::OpenGlRenderState s_cam(
-          pangolin::ProjectionMatrix(1280, 960, 840, 840, 640, 480, 0.1, 1000),
-          pangolin::ModelViewLookAt(25, 25, 70, 25, 25, 5, pangolin::AxisY)
-  );
-
-  const int UI_WIDTH = 30 * pangolin::default_font().MaxWidth();
-  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));
-
-  pangolin::CreatePanel("ui")
-          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
-
-  pangolin::Var<bool> lvx_checkbox("ui.lvx_Checkbox", true, true);
-  pangolin::Var<std::string> lvx_file("ui.lvx_file_String", "../map/lidarbag/start1.lvx");
-
-  pangolin::Var<bool> start_button("ui.start_Button", false, false);
-  pangolin::Var<bool> stop_button("ui.stop_Button", false, false);
-  pangolin::Var<double> freq_double("ui.Freq_Double", 10, 1, 15);
-  /*pangolin::Var<int> an_int("ui.An_Int", 2, 0, 5);
-  pangolin::Var<double> a_double_log("ui.Log_scale", 3, 1, 1E4, true);*/
-  pangolin::Var<std::string> timequeue_string("ui.tq_String", "timequeue_string");
-  pangolin::Var<std::string> xyz_string("ui.xyz_String", "xyz");
-  pangolin::Var<std::string> rpy_string("ui.rpy_String", "rpy");
-  pangolin::Var<std::string> cost_string("ui.final_cost", "cost");
-
-  pangolin::Var<int> beg_Int("ui.beg", 1, 1, 7);
-  pangolin::Var<int> end_Int("ui.end", 5, 1, 7);
-  pangolin::Var<std::string> path_string("ui.path", "cost");
-  pangolin::Var<std::string> distance_string("ui.distance", "distance");
-
-  if(!InitDijkstraMap())
-    return -2;
-
-  pangolin::Var<std::function<void(void)>> solve_view("ui.Solve", [&]() {
-
-    auto start_tm = std::chrono::system_clock::now();
-
-    node_beg = beg_Int.Get();
-    node_end = end_Int.Get();
-
-
-    float distance = 0;
-    pathMutex.lock();
-    shortestPath.clear();
-    DijkstraMap.GetShortestPath(node_beg, node_end, shortestPath, distance);
-    pathMutex.unlock();
-
-    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):%.3f", 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;
-
-  });
-
-  // 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 publish_freq  = freq_double.Get(); /* Hz */
-  if(!InitLidar(publish_freq))
-    return -3;
-
-
-  double zoom = 1.0;
-  while (!pangolin::ShouldQuit())
-  {
-    // Clear entire screen
-    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-    if (pangolin::Pushed(start_button))
-    {
-      if (lvx_checkbox)
-      {
-        std::string file = lvx_file.Get();
-        lddc->Start(file);
-      }
-      else
-      {
-        lddc->Start();
-      }
-    }
-    if (pangolin::Pushed(stop_button))
-    {
-      lddc->Stop();
-    }
-
-    Eigen::Matrix3d rotation=cur_pose.topLeftCorner(3,3);
-    Eigen::Vector3d eulerAngle=rotation.eulerAngles(0,1,2);
-    Eigen::Vector3d t=cur_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 cost[64]={0};
-    sprintf(cost,"cost:%f",final_cost);
-    cost_string=cost;
-
-
-    if (freq_double.GuiChanged())
-    {
-      publish_freq=freq_double.Get();
-      lddc->SetPublishFrq(publish_freq);
-    }
-
-    char info_str[64]={0};
-    sprintf(info_str,"time:%.5fs queue:%d",pMaper->frame_tm_,pMaper->_lidarMsgQueue.size());
-    timequeue_string=info_str;
-
-    if (d_cam.IsShown())
-    {
-      d_cam.Activate(s_cam);
-      pangolin::glDrawAxis(3);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴
-
-      DrawPose(cur_pose);
-
-      //绘制变换后的扫描点
-      double ptSize=1;
-      double alpha=1;
-      scan_cloud.Lock();
-      DrawCloud(scan_cloud.Get(),1,1,1,ptSize,alpha);
-      scan_cloud.unlock();
-
-      //地图点
-      ptSize=0.1;
-      alpha=0.2;
-      DrawCloud(pMaper->GetMapCorner(),1,0,0,ptSize,alpha);
-      DrawCloud(pMaper->GetMapSurf(),0.1,1,0,ptSize,alpha);
-      DrawCloud(pMaper->GetMapNonf(),0.1,0,1,ptSize,alpha);
-
-      //绘制dijkstra地图及最短路径
-      pathMutex.lock();
-      std::vector<int> solve_path=shortestPath;
-      pathMutex.unlock();
-      std::reverse(solve_path.begin(),solve_path.end());
-      solve_path.push_back(node_end);
-      DrawMap(DijkstraMap, solve_path,text_font);
-
-    }
-    pangolin::FinishFrame();
-    std::this_thread::sleep_for(std::chrono::microseconds(10));
-  }
-
-  lddc->Stop();
-  pMaper->completed();
-  delete pMaper;
-  delete lddc;
-  delete text_font;
-
-  return 0;
-}
-
-bool InitMap(std::string config_file)
-{
-  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
-  if (!fsSettings.isOpened())
-  {
-    std::cout << "config_file error: cannot open " << config_file << std::endl;
-    return false;
-  }
-  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
-  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
-  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
-  Use_seg = static_cast<int>(fsSettings["Use_seg"]);
-
-  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
-  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
-  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
-  int PartNum = static_cast<int>(fsSettings["PartNum"]);
-  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
-  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
-  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
-  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);
-
-  int ivox_nearby_type = static_cast<int>(fsSettings["ivox_nearby_type"]);
-  int max_ivox_node = static_cast<int>(fsSettings["max_ivox_node"]);
-  float ivox_resolution = static_cast<float>(fsSettings["ivox_node_solution"]);
-
-  laserCloud.reset(new pcl::PointCloud<PointType>);
-
-  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,
-                                                    NumCurvSize,
-                                                    DistanceFaraway,
-                                                    NumFlat,
-                                                    PartNum,
-                                                    FlatThreshold,
-                                                    BreakCornerDis,
-                                                    LidarNearestDis,
-                                                    KdTreeCornerOutlierDis);
-
-  //map 参数
-  std::string map_dir="../map/";
-  float filter_parameter_corner = 0.2;
-  float filter_parameter_surf = 0.4;
-  int IMU_Mode = 2;
-
-  double vecTlb[]={1.0, 0.0, 0.0, -0.05512,
-          0.0, 1.0, 0.0, -0.02226,
-          0.0, 0.0, 1.0,  0.0297,
-          0.0, 0.0, 0.0,  1.0};
-
-  // set extrinsic matrix between lidar & IMU
-  Eigen::Matrix3d R;
-  Eigen::Vector3d t;
-  R << vecTlb[0], vecTlb[1], vecTlb[2],
-          vecTlb[4], vecTlb[5], vecTlb[6],
-          vecTlb[8], vecTlb[9], vecTlb[10];
-  t << vecTlb[3], vecTlb[7], vecTlb[11];
-  Eigen::Quaterniond qr(R);
-  R = qr.normalized().toRotationMatrix();
-
-
-  int WINDOWSIZE = 0;
-  if (IMU_Mode < 2)
-    WINDOWSIZE = 1;
-  else
-    WINDOWSIZE = 20;
-
-  pMaper = new Mapper(WINDOWSIZE, lidarFeatureExtractor,Use_seg,filter_parameter_corner, filter_parameter_surf,
-                      ivox_nearby_type, max_ivox_node, ivox_resolution);
-  pMaper->InitRT(R, t);
-  pMaper->SetOdomCallback(OdometryCallback);
-  pMaper->SetCloudMappedCallback(CloudFullCallback);
-  pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
-  pMaper->SetFinalCostCallback(FinalCostCallback);
-  pMaper->LoadMap(map_dir + "/featurebbt_transformed.txt");
-
-  double RPI = M_PI / 180.0;
-  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 11.5 * RPI);
-  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
-  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
-  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
-
-  Eigen::Matrix3d rotation_matrix;
-  rotation_matrix = yawAngle * pitchAngle * rollAngle;
-  Eigen::Matrix4d transform;
-  transform.topLeftCorner(3, 3) = rotation_matrix;
-  transform.topRightCorner(3, 1) = Eigen::Vector3d(0.46, 0.48, 0.73);
-  pMaper->SetInitPose(transform);
-
-  printf("  window size :%d\n--------------", WINDOWSIZE);
-  return true;
-}
-
-bool InitDijkstraMap()
-{
-  DijkstraMap.AddVertex(1, 2.3, 3.1);
-  DijkstraMap.AddVertex(2, 19.2, 3.1);
-  DijkstraMap.AddVertex(3, 59.7, 3.3);
-  DijkstraMap.AddVertex(4, 98.4, 3.3);
-  DijkstraMap.AddVertex(5, 98.5, -13);
-  DijkstraMap.AddVertex(6, 59.5, -13);
-  DijkstraMap.AddVertex(7, 20, 18);
-
-  bool ret=true;
-  ret=ret&&DijkstraMap.AddEdge(1, 2, false);
-  ret=ret&&DijkstraMap.AddEdge(2, 3, 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 InitLidar(double publish_freq)
-{
-  LivoxSdkVersion _sdkversion;
-  GetLivoxSdkVersion(&_sdkversion);
-  const int32_t kSdkVersionMajorLimit = 2;
-  if (_sdkversion.major < kSdkVersionMajorLimit) {
-    printf("The SDK version[%d.%d.%d] is too low\n", _sdkversion.major,
-           _sdkversion.minor, _sdkversion.patch);
-    return false;
-  }
-  if (publish_freq > 100.0) {
-    publish_freq = 100.0;
-  } else if (publish_freq < 0.1) {
-    publish_freq = 0.1;
-  } else {
-    publish_freq = publish_freq;
-  }
-  lddc= new livox_ros::Lddc(publish_freq);
-
-  lddc->SetCloudCallback(CloudCallback);
-  lddc->SetImuCallback(ImuDataCallback);
-  return true;
-}
-void DrawPose(Eigen::Matrix4d pose)
-{
-  double l=pose_line_length<0.1?0.1:pose_line_length;
-  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();
-
-}
-
-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=3.6;
-
-  glPointSize(20.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 DrawCloud(pcl::PointCloud<PointType>::Ptr cloud,
-        double r,double g,double b,double psize,double alpha)
-{
-  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();
-}

lio/src/lio/mapper.cpp

@@ -28,7 +28,8 @@ Mapper::Mapper(int wndsize,LidarFeatureExtractor* feature_extractor,int Use_seg,
 Mapper::~Mapper()
 {
     exit_=true;
-    thread_.join();
+    if (thread_.joinable())
+      thread_.join();
     if(estimator){
         delete estimator;
         estimator= nullptr;
@@ -39,7 +40,8 @@ Mapper::~Mapper()
 void Mapper::completed()
 {
     exit_=true;
-    thread_.join();
+    if (thread_.joinable())
+      thread_.join();
 }
 
 void Mapper::InitRT(const Eigen::Matrix3d& R,const Eigen::Vector3d& t)

main.cpp

@@ -0,0 +1,349 @@
+
+
+#include <csignal>
+#include "lio/Estimator.h"
+#include "lio/mapper.h"
+#include "lddc.h"
+#include "lds_hub.h"
+#include "lds_lidar.h"
+#include "lds_lvx.h"
+#include "livox_sdk.h"
+
+#include "pangolinViewer.h"
+#include "segment/LidarFeatureExtractor.h"
+
+#include "dijkstra/dijkstra.h"
+#include "define/timedlockdata.hpp"
+#include "MPC/navigation.h"
+
+//雷达相关
+livox_ros::Lddc *lddc= nullptr;
+//特征提取相关
+LidarFeatureExtractor* lidarFeatureExtractor;
+pcl::PointCloud<PointType>::Ptr laserCloud;
+int Lidar_Type=0;
+int N_SCANS = 6;
+bool Feature_Mode = false;
+bool Use_seg = false;
+
+//slam 地图相关
+Mapper* pMaper= nullptr;
+
+//路径相关
+int node_beg=1,node_end=6;
+std::mutex pathMutex;
+std::vector<int> shortest_path;
+PathMap DijkstraMap;
+
+//控制相关
+Navigation* navigator=nullptr;
+
+TimedLockData<PointCloud> scan_cloud;
+TimedLockData<PointCloud::Ptr> localmap_cloud;
+TimedLockData<Eigen::Matrix4d> cur_pose;
+double final_cost=0;
+
+void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
+{
+    scan_cloud.reset(*cloud,0.1);
+}
+
+void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
+{
+    localmap_cloud.reset(cloud,0.1);
+}
+
+void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
+{
+  cur_pose.reset(tranform);
+  //Eigen::Vector3d eulerAngle=tranform.topLeftCorner(3,3).eulerAngles(2,1,0);
+  Pose2d pose(tranform(0,3),tranform(1,3),0);
+  Navigation::GetInstance()->ResetPose(pose);
+}
+
+void FinalCostCallback(double cost)
+{
+  final_cost=cost;
+}
+
+void CloudCallback(TimeCloud tc,int handle)
+{
+    double timeSpan =tc.cloud->points.back().normal_x;
+    laserCloud.reset(new pcl::PointCloud<PointType>);
+    laserCloud->reserve(15000*N_SCANS);
+
+    PointType point;
+    for(const auto& p : tc.cloud->points)
+    {
+
+        //std::cout<<point.normal_y<<"   "<<point.normal_x<<"   "<<timeSpan<<std::endl;
+        if (p.normal_y > N_SCANS - 1) continue;
+        if (p.x < 0.01) continue;
+        if (!pcl_isfinite(p.x) ||
+                !pcl_isfinite(p.y) ||
+                !pcl_isfinite(p.z))
+        {
+            continue;
+        }
+        point.x=p.x;
+        point.y=p.y;
+        point.z=p.z;
+        point.normal_x = p.normal_x / timeSpan; //normao_x  time
+        point.intensity=p.intensity;
+        int line=int(p.normal_y);
+        point.normal_y=LidarFeatureExtractor::_int_as_float(line);
+        laserCloud->push_back(point);
+      /*std::cout<<" x:"<<point.x<<" y:"<<point.y<<" z:"<<point.z
+               <<" intnesity:"<<point.intensity<<" normal_x:"<<point.normal_x
+               <<" normal_y:"<<point.normal_y<<std::endl;*/
+    }
+
+    tc.cloud=laserCloud;
+    pMaper->AddCloud(tc);
+
+}
+
+void ImuDataCallback(ImuData imu,int handle){
+  pMaper->AddImu(imu);
+}
+
+bool InitLidar(double freq);
+bool InitMap(std::string config_file);
+bool InitDijkstraMap();
+
+void pangolinSpinOnce(PangolinViewer* viewer)
+{
+
+  if(cur_pose.timeout()==false)
+    viewer->ShowRealtimePose(cur_pose.Get());
+
+  //绘制变换后的扫描点
+  double ptSize=1;
+  double alpha=1;
+
+  PointCloud scan=scan_cloud.Get();
+  if(scan.size()>0)
+    viewer->DrawCloud(scan.makeShared(),1,1,1,alpha,ptSize);
+
+  //地图点
+  ptSize=0.1;
+  alpha=0.2;
+  viewer->DrawCloud(pMaper->GetMapCorner(),1,0,0,alpha,ptSize);
+  viewer->DrawCloud(pMaper->GetMapSurf(),0.1,1,0,alpha,ptSize);
+  viewer->DrawCloud(pMaper->GetMapNonf(),0.1,0,1,alpha,ptSize);
+
+  viewer->ShowSlamCost(final_cost);
+  if(pMaper!= nullptr)
+    viewer->ShowSlamQueueTime(pMaper->_lidarMsgQueue.size(),pMaper->frame_tm_);
+
+  //绘制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);
+}
+void OnStartBtn(std::string lvxfile)
+{
+  if(lddc!= nullptr)
+  {
+    if (lvxfile=="")
+      lddc->Start();
+    else
+      lddc->Start(lvxfile);
+  }
+}
+void OnStopBtn()
+{
+  if(lddc!= nullptr)
+    lddc->Stop();
+}
+void OnFreqChanged(int freq)
+{
+  if(lddc!= nullptr)
+    lddc->SetPublishFrq(freq);
+}
+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;
+}
+
+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;
+  }
+
+  //初始化雷达
+  double publish_freq  = 10.0;
+  if(!InitLidar(publish_freq))
+    return -3;
+
+
+  PangolinViewer* viewer=PangolinViewer::CreateViewer();
+  viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
+  viewer->Join();
+
+  TimerRecord::PrintAll();
+  lddc->Stop();
+  pMaper->completed();
+  delete pMaper;
+  delete lddc;
+  delete viewer;
+  return 0;
+}
+
+bool InitMap(std::string config_file)
+{
+  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
+  if (!fsSettings.isOpened())
+  {
+    std::cout << "config_file error: cannot open " << config_file << std::endl;
+    return false;
+  }
+  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
+  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
+  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
+  Use_seg = static_cast<int>(fsSettings["Use_seg"]);
+
+  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
+  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
+  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
+  int PartNum = static_cast<int>(fsSettings["PartNum"]);
+  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
+  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
+  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
+  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);
+
+  int ivox_nearby_type = static_cast<int>(fsSettings["ivox_nearby_type"]);
+  int max_ivox_node = static_cast<int>(fsSettings["max_ivox_node"]);
+  float ivox_resolution = static_cast<float>(fsSettings["ivox_node_solution"]);
+
+  laserCloud.reset(new pcl::PointCloud<PointType>);
+
+  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,
+                                                    NumCurvSize,
+                                                    DistanceFaraway,
+                                                    NumFlat,
+                                                    PartNum,
+                                                    FlatThreshold,
+                                                    BreakCornerDis,
+                                                    LidarNearestDis,
+                                                    KdTreeCornerOutlierDis);
+
+  //map 参数
+  std::string map_dir="../map/";
+  float filter_parameter_corner = 0.2;
+  float filter_parameter_surf = 0.4;
+  int IMU_Mode = 2;
+
+  double vecTlb[]={1.0, 0.0, 0.0, -0.05512,
+          0.0, 1.0, 0.0, -0.02226,
+          0.0, 0.0, 1.0,  0.0297,
+          0.0, 0.0, 0.0,  1.0};
+
+  // set extrinsic matrix between lidar & IMU
+  Eigen::Matrix3d R;
+  Eigen::Vector3d t;
+  R << vecTlb[0], vecTlb[1], vecTlb[2],
+          vecTlb[4], vecTlb[5], vecTlb[6],
+          vecTlb[8], vecTlb[9], vecTlb[10];
+  t << vecTlb[3], vecTlb[7], vecTlb[11];
+  Eigen::Quaterniond qr(R);
+  R = qr.normalized().toRotationMatrix();
+
+
+  int WINDOWSIZE = 0;
+  if (IMU_Mode < 2)
+    WINDOWSIZE = 1;
+  else
+    WINDOWSIZE = 20;
+
+  pMaper = new Mapper(WINDOWSIZE, lidarFeatureExtractor,Use_seg,filter_parameter_corner, filter_parameter_surf,
+                      ivox_nearby_type, max_ivox_node, ivox_resolution);
+  pMaper->InitRT(R, t);
+  pMaper->SetOdomCallback(OdometryCallback);
+  pMaper->SetCloudMappedCallback(CloudFullCallback);
+  pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
+  pMaper->SetFinalCostCallback(FinalCostCallback);
+  pMaper->LoadMap(map_dir + "/featurebbt_transformed.txt");
+
+  double RPI = M_PI / 180.0;
+  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 11.5 * RPI);
+  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
+  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
+  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
+
+  Eigen::Matrix3d rotation_matrix;
+  rotation_matrix = yawAngle * pitchAngle * rollAngle;
+  Eigen::Matrix4d transform;
+  transform.topLeftCorner(3, 3) = rotation_matrix;
+  transform.topRightCorner(3, 1) = Eigen::Vector3d(0.46, 0.48, 0.73);
+  pMaper->SetInitPose(transform);
+
+  printf("  window size :%d\n--------------", WINDOWSIZE);
+  return true;
+}
+
+bool InitDijkstraMap()
+{
+  DijkstraMap.AddVertex(1, 2.3, 3.1);
+  DijkstraMap.AddVertex(2, 19.2, 3.1);
+  DijkstraMap.AddVertex(3, 59.7, 3.3);
+  DijkstraMap.AddVertex(4, 98.4, 3.3);
+  DijkstraMap.AddVertex(5, 98.5, -13);
+  DijkstraMap.AddVertex(6, 59.5, -13);
+  DijkstraMap.AddVertex(7, 20, 18);
+
+  bool ret=true;
+  ret=ret&&DijkstraMap.AddEdge(1, 2, false);
+  ret=ret&&DijkstraMap.AddEdge(2, 3, 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 InitLidar(double publish_freq)
+{
+  LivoxSdkVersion _sdkversion;
+  GetLivoxSdkVersion(&_sdkversion);
+  const int32_t kSdkVersionMajorLimit = 2;
+  if (_sdkversion.major < kSdkVersionMajorLimit) {
+    printf("The SDK version[%d.%d.%d] is too low\n", _sdkversion.major,
+           _sdkversion.minor, _sdkversion.patch);
+    return false;
+  }
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 0.1) {
+    publish_freq = 0.1;
+  } else {
+    publish_freq = publish_freq;
+  }
+  lddc= new livox_ros::Lddc(publish_freq);
+
+  lddc->SetCloudCallback(CloudCallback);
+  lddc->SetImuCallback(ImuDataCallback);
+  return true;
+}

pangolinViewer.h

@@ -0,0 +1,89 @@
+//
+// Created by zx on 22-12-2.
+//
+
+#ifndef LIO_LIVOX_CPP__PANGOLINVIEWER_H_
+#define LIO_LIVOX_CPP__PANGOLINVIEWER_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>
+#include "segment/LidarFeatureExtractor.h"
+#include <thread>
+#include "dijkstra/dijkstra.h"
+#include "define/typedef.h"
+
+
+class PangolinViewer
+{
+ public:
+    typedef void(*ViewerSpinOnceCallback)(PangolinViewer*);
+    typedef void(*StartBtnCallback)(std::string);
+    typedef void(*StopBtnCallback)();
+    typedef void(*FreqChangedCallback)(int);
+    typedef bool(*DijkstraBtnCallback)(int ,int ,float& ,std::vector<int>&);
+ public:
+    static PangolinViewer* CreateViewer();
+
+    void SetCallbacks(ViewerSpinOnceCallback spinOnce,StartBtnCallback startBtn,
+     StopBtnCallback stopBtn,FreqChangedCallback freqChanged,DijkstraBtnCallback dijkstra);
+    void ShowRealtimePose(Eigen::Matrix4d pose);
+    void ShowSlamCost(double cost);
+    void ShowSlamQueueTime(int queue,double tm);
+    void DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
+     double alpha,double psize);
+    void DrawDijkastraMap(PathMap& map,std::vector<int> path);
+    void Join();
+    ~PangolinViewer();
+
+ private:
+    static void loop(PangolinViewer* p);
+    PangolinViewer();
+
+    void Init();
+
+ protected:
+    static PangolinViewer* viewer_;
+    std::thread* thread_;
+
+
+    pangolin::GlFont *text_font_ ;
+    pangolin::OpenGlRenderState* s_cam_;
+    pangolin::View *d_cam_ ;
+
+    pangolin::Var<bool>* lvx_checkbox_;
+    pangolin::Var<std::string>* lvx_file_;
+
+    pangolin::Var<bool>* start_button_;
+    pangolin::Var<bool>* stop_button_;
+    pangolin::Var<double>* freq_double_;
+
+    pangolin::Var<std::string>* timequeue_string_;
+    pangolin::Var<std::string>* xyz_string_;
+    pangolin::Var<std::string>* rpy_string_;
+    pangolin::Var<std::string>* cost_string_;
+
+    pangolin::Var<int>* beg_Int_;
+    pangolin::Var<int>* end_Int_;
+    pangolin::Var<std::string>* path_string_;
+    pangolin::Var<std::string>* distance_string_;
+
+    std::vector<int> shortest_path_;
+
+    ViewerSpinOnceCallback spinOnce_callback_=nullptr;
+    StartBtnCallback startBtn_callback_=nullptr;
+    StopBtnCallback stopBtn_callback_= nullptr;
+    FreqChangedCallback freqChanged_callback_= nullptr;
+    DijkstraBtnCallback dijkstraBtn_callback_= nullptr;
+
+
+};
+
+
+#endif //LIO_LIVOX_CPP__PANGOLINVIEWER_H_