提交agv ui tool，生成动作指令集

CMakeLists.txt

@@ -0,0 +1,96 @@
+# Copyright(c) 2019 livoxtech limited.
+cmake_minimum_required(VERSION 2.8.3)
+
+
+#---------------------------------------------------------------------------------------
+# Start livox_ros_driver project
+#---------------------------------------------------------------------------------------
+
+project(NavUI)
+
+#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+
+#---------------------------------------------------------------------------------------
+# find package and the dependecy
+#---------------------------------------------------------------------------------------
+find_package(Boost 1.54 REQUIRED COMPONENTS
+	system
+	thread
+	chrono
+	)
+
+find_package (Eigen3 REQUIRED CONFIG QUIET)
+find_package(Pangolin 0.8 REQUIRED)
+find_package(PCL REQUIRED)
+FIND_PACKAGE(Protobuf REQUIRED)
+FIND_PACKAGE(Glog REQUIRED)
+
+#---------------------------------------------------------------------------------------
+# Set default build to release
+#---------------------------------------------------------------------------------------
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
+endif()
+
+#---------------------------------------------------------------------------------------
+# Compiler config
+#---------------------------------------------------------------------------------------
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+
+include_directories(
+		define
+		/usr/local/include
+		${Pangolin_INCLUDE_DIRS}
+		${EIGEN3_INCLUDE_DIR}
+		${PCL_INCLUDE_DIRS}
+		)
+link_directories("/usr/local/lib")
+## PCL library
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+#---------------------------------------------------------------------------------------
+# generate excutable and add libraries
+#---------------------------------------------------------------------------------------
+add_executable(${PROJECT_NAME}
+		main.cpp
+		define/TimerRecord.cpp
+		pangolinViewer.cpp
+		dijkstra/dijkstra.cpp
+
+		dijkstra/trajectory.cpp
+		dijkstra/pose2d.cpp
+
+		emqx/mqttmsg.cpp
+		emqx/paho_client.cpp
+		emqx/message.pb.cc
+		tool/pathcreator.cpp
+		emqx/terminator_emqx.cpp
+		)
+target_compile_options(${PROJECT_NAME}
+		PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
+		)
+
+#---------------------------------------------------------------------------------------
+# link libraries
+#---------------------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME}
+		pango_display
+		${PCL_LIBRARIES}
+		${Boost_LIBRARY}
+		${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES}
+		libglog.a libgflags.a
+		paho-mqtt3a
+		-lpthread
+		ipopt
+		)
+
+#---------------------------------------------------------------------------------------
+# precompile macro and compile option
+#---------------------------------------------------------------------------------------
+
+

config/horizon_config.yaml

@@ -0,0 +1,22 @@
+%YAML:1.0
+
+
+# switches
+Lidar_Type: 0    # 0-horizon
+Used_Line: 6    # lines used for lio, set to 1~6
+Feature_Mode: 0    # 0(false) or 1(true)
+NumCurvSize: 2
+DistanceFaraway: 100 # [m]  <DistanceFaraway near / >DistanceFaraway far
+NumFlat: 3 # nums of one part's flat feature
+PartNum: 150 # nums of one scan's parts
+FlatThreshold: 0.02 # cloud curvature threshold of flat feature
+BreakCornerDis: 1 # break distance of break points
+LidarNearestDis: 0.1 # if(depth < LidarNearestDis) do not use this point 
+KdTreeCornerOutlierDis: 0.2 # corner filter threshold
+Use_seg: 0 # use segment algorithm
+map_skip_frame: 2
+
+ivox_nearby_type: 26 #6  18 26
+max_ivox_node: 500
+ivox_node_solution: 0.3
+map_file: "../map/bbt/featurebbt_transformed.txt"

config/navigation.prototxt

@@ -0,0 +1,17 @@
+Agv_emqx
+{
+	NodeId:"agv1"
+	ip:"127.0.0.1"
+	port:1883
+	pubTopic:"AgvCmd/cmd1"
+	subTopic:"AgvStatu/robot1"
+}
+
+Terminal_emqx
+{
+	NodeId:"agv1"
+	ip:"192.168.2.100"
+	port:1883
+	pubTopic:"NavStatu/nav1"
+	subTopic:"NavCmd/cmd1"
+}

define/TimerRecord.cpp

@@ -0,0 +1,31 @@
+//
+// Created by zx on 22-11-22.
+//
+
+#include "TimerRecord.h"
+#include <algorithm>
+//
+// Created by zx on 22-11-3.
+//
+
+
+void TimerRecord::PrintAll()
+{
+  std::cout <<">> ===== Printing run time ====="<<std::endl;
+  for (const auto& r : records_) {
+    auto v = std::minmax_element(r.second.timesqueue_.begin(), r.second.timesqueue_.end());
+
+    std::cout<< "> [ " << r.first << " ] average time usage: "
+             << std::accumulate(r.second.timesqueue_.begin(), r.second.timesqueue_.end(), 0.0) /
+                     double(r.second.timesqueue_.size())
+             << " ms , called times: " << r.second.timesqueue_.size()<<
+             "   max:"<<*v.second<<"ms , min:"<<*v.first<<"ms"<<std::endl;
+  }
+  std::cout << ">>> ===== Printing run time end ====="<<std::endl;
+}
+
+std::map<std::string,FuncRecord> TimerRecord::records_;//=std::map<std::string,FuncRecord>();
+std::mutex TimerRecord::mutex_;
+
+
+

define/TimerRecord.h

@@ -0,0 +1,66 @@
+//
+// Created by zx on 22-11-3.
+//
+
+#ifndef SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_
+#define SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_
+
+#include <map>
+#include <mutex>
+#include <string>
+#include <vector>
+#include <chrono>
+#include <numeric>
+#include <iostream>
+#include <algorithm>
+
+struct FuncRecord
+{
+ public:
+    FuncRecord()=default;
+    FuncRecord(std::string name,double time)
+    {
+        name_=name;
+        timesqueue_.emplace_back(time);
+    }
+
+    std::string name_;
+    std::vector<double> timesqueue_;
+};
+
+class TimerRecord
+{
+ public:
+
+ public:
+    template<class F>
+    inline static void Execute(F func, const std::string &func_name)
+    {
+      auto t1 = std::chrono::high_resolution_clock::now();
+      func();
+      auto t2 = std::chrono::high_resolution_clock::now();
+      auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
+      mutex_.lock();
+      if (records_.find(func_name) != records_.end())
+      {
+        records_[func_name].timesqueue_.emplace_back(time_used);
+      }
+      else
+      {
+        records_.insert({func_name, FuncRecord(func_name, time_used)});
+      }
+      mutex_.unlock();
+    }
+
+    static void PrintAll();
+
+
+ protected:
+    static std::mutex mutex_;
+    static std::map<std::string, FuncRecord> records_;
+};
+
+
+
+
+#endif //SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_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_

define/typedef.h

@@ -0,0 +1,37 @@
+//
+// Created by zx on 22-11-2.
+//
+
+#ifndef SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
+#define SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+typedef pcl::PointXYZINormal PointType;
+typedef pcl::PointCloud<PointType> PointCloud;
+typedef struct
+{
+    double timebase;
+    pcl::PointCloud<PointType>::Ptr cloud;
+}TimeCloud;
+
+typedef void(*odomCallback)(Eigen::Matrix4d,double);
+typedef void(*CloudMappedCallback)(pcl::PointCloud<PointType>::Ptr,double);
+typedef void(*CostCallback)(double);
+
+
+typedef struct
+{
+    double x;
+    double y;
+    double z;
+}Data3d;
+typedef struct
+{
+    double timebase;
+    Data3d angular_velocity;
+    Data3d linear_acceleration;
+
+} ImuData;
+
+#endif //SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_

dijkstra/dijkstra.cpp

@@ -0,0 +1,168 @@
+//
+// Created by zx on 22-11-29.
+//
+
+#include "dijkstra.h"
+#include <math.h>
+
+EdgeNode::EdgeNode(int key, float weight){
+  this->key = key;
+  this->weight = weight;
+  this->next = NULL;
+}
+
+Graph::Graph(bool directed){
+  this->directed = directed;
+  for(int i = 1; i < (MAXV + 1); i ++){
+    this->edges[i] = NULL;
+  }
+}
+
+Graph::~Graph(){
+  //to do
+}
+
+void Graph::insert_edge(int x, int y, float weight, bool directed){
+  if(x > 0 && x < (MAXV + 1) && y > 0 && y < (MAXV + 1)){
+    EdgeNode *edge = new EdgeNode(y, weight);
+    edge->next = this->edges[x];
+    this->edges[x] = edge;
+    if(!directed){
+      insert_edge(y, x, weight, true);
+    }
+  }
+}
+
+void Graph::print(){
+  for(int v = 1; v < (MAXV + 1); v ++){
+    if(this->edges[v] != NULL){
+      std::cout << "Vertex " << v << " has neighbors: " << std::endl;
+      EdgeNode *curr = this->edges[v];
+      while(curr != NULL){
+        std::cout << curr->key << std::endl;
+        curr = curr->next;
+      }
+    }
+  }
+}
+
+void init_vars(bool discovered[], float distance[], int parent[]){
+  for(int i = 1; i < (MAXV + 1); i ++){
+    discovered[i] = false;
+    distance[i] = std::numeric_limits<float>::max();
+    parent[i] = -1;
+  }
+}
+
+void dijkstra_shortest_path(Graph *g, int parent[], float distance[], int start){
+
+  bool discovered[MAXV + 1];
+  EdgeNode *curr;
+  int v_curr;
+  int v_neighbor;
+  float weight;
+  float smallest_dist;
+
+  init_vars(discovered, distance, parent);
+
+  distance[start] = 0;
+  v_curr = start;
+
+  while(discovered[v_curr] == false){
+
+    discovered[v_curr] = true;
+    curr = g->edges[v_curr];
+
+    while(curr != NULL){
+
+      v_neighbor = curr->key;
+      weight = curr->weight;
+
+      if((distance[v_curr] + weight) < distance[v_neighbor]){
+        distance[v_neighbor] = distance[v_curr] + weight;
+        parent[v_neighbor] = v_curr;
+      }
+      curr = curr->next;
+    }
+
+    //set the next current vertex to the vertex with the smallest distance
+    smallest_dist = std::numeric_limits<float>::max();
+    for(int i = 1; i < (MAXV + 1); i ++){
+      if(!discovered[i] && (distance[i] < smallest_dist)){
+        v_curr = i;
+        smallest_dist = distance[i];
+      }
+    }
+  }
+}
+
+void get_shortest_path(int v, int parent[],std::vector<int>& path){
+  if(v > 0 && v < (MAXV + 1) && parent[v] != -1){
+    path.push_back( parent[v]) ;
+    get_shortest_path(parent[v], parent,path);
+  }
+
+}
+
+void print_distances(int start, float distance[]){
+  for(int i = 1; i < (MAXV + 1); i ++){
+    if(distance[i] != std::numeric_limits<float>::max()){
+      std::cout << "Shortest distance from " << start << " to " << i << " is: " << distance[i] << std::endl;
+    }
+  }
+}
+
+
+PathMap::PathMap(){
+  grapher_ = new Graph(false);
+}
+PathMap::~PathMap()
+{
+  delete grapher_;
+}
+
+void PathMap::AddVertex(int id,float x,float y)
+{
+  node_[id]={x,y};
+  printf(" Add node %f,%f  nodes :%d\n",x,y,node_.size());
+}
+bool PathMap::AddEdge(int id1,int id2,bool directed)
+{
+  if(node_.find(id1)!=node_.end() && node_.find(id2)!=node_.end())
+  {
+    float x1=node_[id1].x;
+    float y1=node_[id1].y;
+    float x2=node_[id2].x;
+    float y2=node_[id2].y;
+    float distance=sqrt(pow(x2-x1,2)+pow(y2-y1,2))+EXP;
+    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;
+}
+
+bool PathMap::GetShortestPath(int id1,int id2,std::vector<int>& path,float& shortest_distance)
+{
+  if(node_.find(id1)!=node_.end() && node_.find(id2)!=node_.end())
+  {
+    dijkstra_shortest_path(grapher_, parent_, distance_, id1);
+    get_shortest_path(id2, parent_, path);
+    shortest_distance=distance_[id2];
+    return true;
+  }
+  return false;
+}
+
+bool PathMap::GetVertexPos(int id,float& x,float& y)
+{
+  if(node_.find(id)!=node_.end())
+  {
+    x=node_[id].x;
+    y=node_[id].y;
+    return true;
+  }
+  return false;
+}
+

dijkstra/dijkstra.h

@@ -0,0 +1,85 @@
+//
+// Created by zx on 22-11-29.
+//
+
+#ifndef LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_
+#define LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_
+
+
+//Purpose: Implementation of Dijkstra's algorithm which finds the shortest
+//path from a start node to every other node in a weighted graph.
+//Time complexity: O(n^2)
+#include <iostream>
+#include <limits>
+#include <vector>
+#include <map>
+
+#define MAXV 1000
+#define EXP 0.0001
+class EdgeNode{
+ public:
+    int key;
+    float weight;
+    EdgeNode *next;
+    EdgeNode(int, float);
+};
+
+
+class Graph{
+    bool directed;
+ public:
+    EdgeNode *edges[MAXV + 1];
+    Graph(bool);
+    ~Graph();
+    void insert_edge(int, int, float, bool);
+    void print();
+};
+
+void init_vars(bool discovered[], int distance[], int parent[]);
+
+void dijkstra_shortest_path(Graph *g, int parent[], float distance[], int start);
+
+void get_shortest_path(int v, int parent[],std::vector<int>& path);
+
+void print_distances(int start, float distance[]);
+
+class PathMap
+{
+ public:
+    typedef struct
+    {
+      float x;
+      float y;
+    }MapNode;
+    typedef struct
+    {
+      int id1;
+      int id2;
+      bool direct;
+    }MapEdge;
+ public:
+    PathMap();
+    ~PathMap();
+
+    void AddVertex(int id,float x,float y);
+    bool AddEdge(int id1,int id2,bool directed);
+
+    bool GetShortestPath(int id1,int id2,std::vector<int>& path,float& shortest_distance);
+
+    bool GetVertexPos(int id,float& x,float& y);
+    std::map<int,MapNode> GetNodes()const{return node_;};
+    std::vector<MapEdge> GetEdges()const{return edge_;}
+
+ protected:
+    std::map<int,MapNode> node_;
+    std::vector<MapEdge>  edge_;
+    Graph *grapher_;
+    int parent_[MAXV + 1];
+    float distance_[MAXV + 1];
+
+
+};
+
+
+
+#endif //LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_

dijkstra/pose2d.cpp

@@ -0,0 +1,68 @@
+//
+// Created by zx on 22-12-1.
+//
+//
+// Created by zx on 2020/9/9.
+//
+
+#include "pose2d.h"
+
+Pose2d::Pose2d()
+        :m_x(0),m_y(0),m_theta(0)
+{
+}
+Pose2d::Pose2d(float x,float y,float theta)
+        :m_x(x),m_y(y),m_theta(theta)
+{
+}
+
+Pose2d::~Pose2d()
+{}
+
+std::ostream& operator<<(std::ostream &out,const Pose2d& point)
+{
+  out<<"[x:"<<point.x()<<", y:"<<point.y()<<", theta:"<<point.theta()*180.0/M_PI<<"°]";
+  return out;
+}
+const float Pose2d::gridient() const
+{
+  double gradient=tanf(m_theta);
+  if(fabs(gradient)>200)
+    return 200.0*(gradient/fabs(gradient));
+  return gradient;
+}
+
+Pose2d Pose2d::PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose)
+{
+  Pose2d diff=world_pose-axis_pose;
+  Pose2d nPose=diff.rotate(-axis_pose.theta());
+  return Pose2d(nPose.x(),nPose.y(),diff.theta());
+}
+
+float Pose2d::distance(const Pose2d& pose1,const Pose2d& pose2)
+{
+  Pose2d offset=pose1-pose2;
+  return sqrt(offset.x()*offset.x()+offset.y()*offset.y());
+}
+
+Pose2d Pose2d::rotate(float theta)const
+{
+  double cs=cos(theta);
+  double sn=sin(theta);
+  float x=cs*this->x()-sn*this->y();
+  float y=sn*this->x()+cs*this->y();
+  float new_theta=this->theta()+theta;
+
+  //转换到 [-pi/2, pi/2]下
+  int n=int(new_theta/(M_PI));
+  if(new_theta<-M_PI/2)
+  {
+    new_theta += M_PI*(n+1);
+  }
+  if(new_theta>M_PI/2)
+  {
+    new_theta -= M_PI*(n+1);
+  }
+
+  return Pose2d(x,y,new_theta);
+}

dijkstra/pose2d.h

@@ -0,0 +1,109 @@
+//
+// Created by zx on 22-12-1.
+//
+
+#ifndef LIO_LIVOX_CPP_MPC_POSE2D_H_
+#define LIO_LIVOX_CPP_MPC_POSE2D_H_
+
+#include <iostream>
+#include <atomic>
+#include <math.h>
+/*
+ * 带direction的二维点,与x轴正方向逆时针为正方向
+ */
+
+class Pose2d
+{
+ public:
+    Pose2d();
+    Pose2d(float x,float y,float theta);
+    Pose2d(const Pose2d& point)
+    {
+      m_x=point.x();
+      m_y=point.y();
+      m_theta=point.theta();
+    }
+    ~Pose2d();
+    Pose2d& operator=(const Pose2d& point)
+    {
+      m_x=point.x();
+      m_y=point.y();
+      m_theta=point.theta();
+      return *this;
+    }
+
+    /*
+     * 两位姿相减，角度在-PI ,PI
+     */
+    Pose2d operator-(const Pose2d& pose)const
+    {
+
+      float x=m_x-pose.x();
+      float y=m_y-pose.y();
+      float theta=m_theta-pose.theta();
+
+      const double PI=3.14159265;
+      if(theta<0)
+        theta=theta+2*PI;
+
+      if(theta>=2*PI)
+        theta-=2*PI;
+
+      if(theta>PI)
+        theta=theta-2*PI;
+
+      return Pose2d(x,y,theta);
+    }
+    Pose2d operator+(const Pose2d& pose)const
+    {
+      float x=m_x+pose.x();
+      float y=m_y+pose.y();
+      float theta=m_theta+pose.theta();
+      //转换到 [-pi/2, pi/2]下
+      int n=int(theta/(M_PI));
+      if(theta<-M_PI/2)
+      {
+        theta += M_PI*(n+1);
+      }
+      if(theta>M_PI/2)
+      {
+        theta -= M_PI*(n+1);
+      }
+      return Pose2d(x,y,theta);
+    }
+    static Pose2d abs(const Pose2d& pose)
+    {
+      Pose2d ret(std::abs(pose.x()),std::abs(pose.y()),std::abs(pose.theta()));
+      return ret;
+    }
+    bool operator<(const Pose2d& pose)const
+    {
+      return x()<pose.x()&&y()<pose.y()&&theta()<pose.theta();
+    }
+
+    /*
+     * 将点顺时针旋转theta
+     */
+    Pose2d rotate(float theta)const;
+    friend std::ostream& operator<<(std::ostream &out,const Pose2d& point);
+
+
+    const float x() const {return m_x;};
+    const float y() const {return m_y;};
+    const float theta() const {return m_theta;}
+
+    const float gridient()const;
+
+    static float distance(const Pose2d& pose1,const Pose2d& pose2);
+    static Pose2d PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose);
+
+ protected:
+    std::atomic<float>                      m_x;
+    std::atomic<float>                      m_y;
+    std::atomic<float>                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
+};
+
+
+
+
+#endif //LIO_LIVOX_CPP_MPC_POSE2D_H_

dijkstra/trajectory.cpp

@@ -0,0 +1,228 @@
+//
+// Created by zx on 22-12-1.
+//
+//
+// Created by zx on 2020/9/9.
+//
+#include <iostream>
+#include "trajectory.h"
+
+Trajectory::Trajectory(){}
+
+Trajectory::Trajectory(const std::vector<Pose2d>& points)
+{
+  m_pose_vector=points;
+}
+
+Pose2d Trajectory::operator[](int index) const
+{
+  return m_pose_vector[index];
+}
+Trajectory& Trajectory::operator+(const Trajectory& other)
+{
+  m_pose_vector.insert(m_pose_vector.end(),
+                       other.m_pose_vector.begin(),other.m_pose_vector.end());
+  return *this;
+}
+
+void Trajectory::push_point(const Pose2d& point)
+{
+  m_pose_vector.push_back(point);
+}
+
+/*
+ * 根据梯度和方向计算角度,x轴正，逆时针为正
+ */
+double gradient2theta(double gradient,bool direction)
+{
+  //计算方向 在第几象限
+  int Quadrant=1;
+  if(gradient>0)
+  {
+    if(direction)
+      Quadrant=1;
+    else
+      Quadrant=3;
+  }
+  else
+  {
+    if(direction)
+      Quadrant=4;
+    else
+      Quadrant=2;
+  }
+
+  double angle=0;
+  if(fabs(gradient)>=200.0)
+  {
+    angle=M_PI/2.0;
+  }
+  else
+  {
+    angle=atan(fabs(gradient));
+  }
+  // angle === (0,M_PI/2.0)
+  if(Quadrant==1)
+  {
+
+  }
+  if(Quadrant==2)
+  {
+    angle=M_PI-angle;
+  }
+  if(Quadrant==3)
+  {
+    angle+=M_PI;
+  }
+  if(Quadrant==4)
+  {
+    angle=-angle;
+  }
+  return angle;
+}
+
+
+double Trajectory::gen_axis_angle(const Pose2d& point1,const Pose2d& point2)
+{
+
+  double delta_x=point2.x()-point1.x();
+  double delta_y=point2.y()-point1.y();
+  double gradient=0;
+  if(delta_x==0)
+  {
+    if(delta_y>0)
+      gradient=200;
+    else if(delta_y<0)
+      gradient=-200;
+    else
+      return 0;
+  }
+  else
+  {
+    gradient=delta_y/delta_x;
+  }
+
+  return gradient2theta(gradient,delta_x>=0);
+}
+
+
+/*
+ * 计算曲线P 在(x1,x2)的线积分
+ * 默认以 （x2-x1）/100  为步长
+ */
+double integral_path(double x1,double x2,Eigen::MatrixXd P)
+{
+  double a=P(0,0);
+  double b=P(1,0);
+  double c=P(2,0);
+  double d=P(3,0);
+  double delta=(x2-x1)/100.0;
+
+  double t=x1;
+  double integral=0.0;
+  //循环条件为 t位于 x1和x2 之间
+  while((x2-t)*(t-x1)>=0)
+  {
+    t+=delta;
+    double graidient=3.0*a*pow(t,2.0)+2.0*b*t+c;
+    integral+=sqrt(delta*delta+graidient*graidient);
+  }
+  return integral;
+
+}
+
+Trajectory Trajectory::create_trajectory(const Pose2d& start,const Pose2d& end,int point_count)
+{
+  int extend_num=point_count/2;
+
+  Trajectory trajectory;
+
+  double angle =gen_axis_angle(start,end);
+
+  Pose2d pose1=start.rotate(-angle);
+  Pose2d pose2=end.rotate(-angle);
+
+  double gradient1=pose1.gridient();
+  double gradient2=pose2.gridient();
+
+  //三阶多项式差值，生成 曲线
+  double x0=pose1.x();
+  double y0=pose1.y();
+  double x1=pose2.x();
+  double y1=pose2.y();
+
+  Eigen::Matrix<double,4,4> A=Eigen::MatrixXd::Zero(4,4);    //系数矩阵A
+  Eigen::Matrix<double,4,1> B=Eigen::MatrixXd::Zero(4,1);    //值矩阵B
+  A(0,0)=pow(x0,3.0);
+  A(0,1)=pow(x0,2.0);
+  A(0,2)=x0;
+  A(0,3)=1.0;
+
+  A(1,0)=pow(x1,3.0);
+  A(1,1)=pow(x1,2.0);
+  A(1,2)=x1;
+  A(1,3)=1.0;
+
+  A(2,0)=3.0*pow(x0,2.0);
+  A(2,1)=2.0*x0;
+  A(2,2)=1.0;
+
+  A(3,0)=3.0*pow(x1,2.0);
+  A(3,1)=2.0*x1;
+  A(3,2)=1.0;
+
+  B(0,0)=y0;
+  B(1,0)=y1;
+  B(2,0)=gradient1;
+  B(3,0)=gradient2;
+  Eigen::MatrixXd P=A.inverse()*B;
+
+  double a=P(0,0);
+  double b=P(1,0);
+  double c=P(2,0);
+  double d=P(3,0);
+
+  //步长
+  double delta_x=(x1-x0)/float(point_count);
+  //生成离散点,间距
+  for(double x=x0-delta_x*extend_num;x<x1+delta_x*extend_num;x+=delta_x)
+  {
+    double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
+    double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
+    trajectory.push_point(Pose2d(x,y,gradient2theta(gradient)));
+  }
+
+  //将轨迹点反变换到原坐标系
+  Trajectory trajectory_map;
+  for(int i=0;i<trajectory.size();++i)
+  {
+    Pose2d point=trajectory[i];
+    Pose2d new_point=point.rotate(angle);
+    trajectory_map.push_point(new_point);
+  }
+
+  return trajectory_map;
+}
+
+Trajectory Trajectory::create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt)
+{
+
+  Trajectory trajectory;
+  double angle =gen_axis_angle(start,end);
+
+  int point_count=int(Pose2d::distance(start,end)/dt);
+  //生成离散点,间距
+  Pose2d diff=end-start;
+  float solution_x=diff.x()/float(point_count);
+  float solution_y=diff.y()/float(point_count);
+  for(int i=0;i<point_count;++i)
+  {
+    double x=start.x()+i*solution_x;
+    double y=start.y()+i*solution_y;
+    //printf(" point x :%f  y:%f\n",x,y);
+    trajectory.push_point(Pose2d(x,y,angle));
+  }
+  trajectory.push_point(Pose2d(end.x(),end.y(),angle));
+  return trajectory;
+}
+

dijkstra/trajectory.h

@@ -0,0 +1,47 @@
+//
+// Created by zx on 22-12-1.
+//
+
+#ifndef LIO_LIVOX_CPP_MPC_TRAJECTORY_H_
+#define LIO_LIVOX_CPP_MPC_TRAJECTORY_H_
+
+#include "pose2d.h"
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+class Trajectory
+{
+ public:
+    Trajectory();
+    Trajectory(const std::vector<Pose2d>& points);
+    Trajectory(const Trajectory& other)= default;
+    Trajectory& operator=(const Trajectory& other)= default;
+    Pose2d operator[](int index) const;
+    Trajectory& operator+(const Trajectory& other);
+    unsigned int size()const{return m_pose_vector.size();}
+    void clear(){m_pose_vector.clear();}
+    void push_point(const Pose2d& point);
+
+    static Trajectory create_trajectory(const Pose2d& start,const Pose2d& end,int point_count=100);
+    static Trajectory create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt=0.05);
+    static double gen_axis_angle(const Pose2d& point1,const Pose2d& point2);
+
+ protected:
+    std::vector<Pose2d>             m_pose_vector;
+};
+
+/*
+ * 根据梯度和方向计算角度,x轴正，逆时针为正
+ */
+double gradient2theta(double gradient,bool direction=true);
+
+double gen_axis_angle(const Pose2d& point1,const Pose2d& point2);
+/*
+ * 计算曲线P 在(x1,x2)的线积分
+ * 默认以 （x2-x1）/100  为步长
+ */
+double integral_path(double x1,double x2,Eigen::MatrixXd P);
+
+
+#endif //LIO_LIVOX_CPP_MPC_TRAJECTORY_H_

dijkstraSample.cpp

@@ -0,0 +1,292 @@
+//
+// 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;
+}

emqx/mqttmsg.cpp

@@ -0,0 +1,289 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#include "mqttmsg.h"
+#include <string.h>
+
+#include <google/protobuf/util/json_util.h>
+using namespace google::protobuf;
+MqttMsg::MqttMsg()
+{
+  data_= nullptr;
+  length_=0;
+}
+MqttMsg::MqttMsg(char* data,const int length)
+{
+  data_=(char*)malloc(length);
+  memcpy(data_,data,length);
+  length_=length;
+}
+MqttMsg::MqttMsg(const MqttMsg& msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+  data_=(char*)malloc(msg.length_);
+  memcpy(data_,msg.data_,msg.length_);
+  length_=msg.length_;
+}
+MqttMsg& MqttMsg::operator=(const MqttMsg& msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+  data_=(char*)malloc(msg.length_);
+  memcpy(data_,msg.data_,msg.length_);
+  length_=msg.length_;
+  return *this;
+}
+
+char* MqttMsg::data()const{
+  return data_;
+}
+int MqttMsg::length()const{
+  return length_;
+}
+
+
+
+
+MqttMsg::~MqttMsg(){
+  if(data_!= nullptr)
+  {
+    free(data_);
+    data_ = nullptr;
+  }
+  length_=0;
+}
+
+/*
+void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  AGVStatu statu;
+  statu.set_x(x);
+  statu.set_y(y);
+  statu.set_theta(theta);
+  statu.set_v(v);
+  statu.set_vth(vth);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(statu,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vth)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+  AGVStatu statu;
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &statu);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+
+  if(ret.ok())
+  {
+    x=statu.x();
+    y=statu.y();
+    theta=statu.theta();
+    v=statu.v();
+    vth=statu.vth();
+
+  }
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavSpeed(const NavMessage::Speed& speed)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(speed,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+
+}
+
+bool MqttMsg::toNavSpeed(NavMessage::Speed& speed)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+
+    //printf("%s\n",buf);
+    ret = util::JsonStringToMessage(buf, &speed);
+    printf("change speed v:%f,  vth:%f\n",speed.v(),speed.vth());
+
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+
+  free(buf);
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavCmd(const NavMessage::NavCmd& cmd)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavCmd(NavMessage::NavCmd& cmd)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &cmd);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
+
+  return ret.ok();
+}
+
+void MqttMsg::fromNavStatu(const NavMessage::NavStatu &statu)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(statu,&json_str,option);
+
+  length_=json_str.length();
+  data_=(char*)malloc(length_);
+
+  memcpy(data_,json_str.c_str(),length_);
+}
+
+bool MqttMsg::toNavStatu(NavMessage::NavStatu &statu)
+{
+  if(strlen(data())<length_)
+    return false;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &statu);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+    return false;
+  }
+  free(buf);
+
+  return ret.ok();
+}
+*/
+
+void MqttMsg::fromProtoMessage(const google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_)
+    free(data_);
+
+  google::protobuf::util::JsonPrintOptions option;
+  option.always_print_primitive_fields=true;
+  std::string json_str;
+  google::protobuf::util::MessageToJsonString(message,&json_str,option);
+
+  length_=json_str.size();
+  data_=(char*)malloc(length_);
+  memcpy(data_,json_str.c_str(),length_);
+}
+bool MqttMsg::toProtoMessage(google::protobuf::Message& message)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(data_== nullptr)
+    return false;
+
+
+  util::Status ret;
+  char* buf=(char*)malloc(length_+1);
+  memset(buf,0,length_+1);
+  memcpy(buf,data(),length_);
+  try
+  {
+    ret = util::JsonStringToMessage(buf, &message);
+  }
+  catch (std::exception &e)
+  {
+    printf(" exp:%s\n",e.what());
+  }
+  free(buf);
+  return ret.ok();
+}

emqx/mqttmsg.h

@@ -0,0 +1,55 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#ifndef PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
+#define PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_
+#include "message.pb.h"
+#include <mutex>
+
+class MqttMsg
+{
+ public:
+    enum Dtype
+    {
+      ePointCloudXYZ,
+      ePointCloudXYZI,
+      eString,
+      eImage,
+      eAGVStatu,
+      eAGVSpeed
+    };
+ public:
+    MqttMsg();
+    MqttMsg(char* data,const int length);
+    MqttMsg(const MqttMsg& msg);
+    MqttMsg& operator=(const MqttMsg& msg);
+    ~MqttMsg();
+
+    char* data()const;
+    int length()const;
+
+
+    void fromProtoMessage(const google::protobuf::Message& messge);
+    bool toProtoMessage(google::protobuf::Message& message);
+
+    /*void fromStatu(double x,double y,double theta,double v,double vth);
+    void fromNavSpeed(const NavMessage::Speed& speed);
+    void fromNavCmd(const NavMessage::NavCmd& cmd);
+    void fromNavStatu(const NavMessage::NavStatu& statu);
+
+    bool toStatu(double& x,double& y,double& theta,double& v,double& vth);
+    bool toNavSpeed(NavMessage::Speed& speed);
+    bool toNavCmd(NavMessage::NavCmd& cmd);
+    bool toNavStatu(NavMessage::NavStatu& statu);*/
+
+
+ protected:
+    char* data_=nullptr;
+    int length_=0;
+    std::mutex mutex_;
+
+};
+
+
+#endif //PAHOC_SAMPLE_SAMPLES_DEVICE_MSG_H_

emqx/paho_client.cpp

@@ -0,0 +1,192 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#include "paho_client.h"
+
+
+Paho_client::Paho_client(std::string clientid){
+  clientid_=clientid;
+  context_= nullptr;
+  connected_=false;
+}
+Paho_client::~Paho_client(){}
+void Paho_client::set_maxbuf(int size)
+{
+  create_opts_.maxBufferedMessages=size+256;
+}
+bool Paho_client::connect(std::string address,int port){
+  char connstr[255]={0};
+  sprintf(connstr,"mqtt://%s:%d",address.c_str(),port);
+  int rc;
+
+  if ((rc=MQTTAsync_createWithOptions(&client_,connstr,clientid_.c_str(),MQTTCLIENT_PERSISTENCE_NONE,NULL,&create_opts_)
+          != MQTTASYNC_SUCCESS))
+  {
+    printf("Failed to create client object, return code %d\n", rc);
+    return false;
+  }
+
+  if ((rc = MQTTAsync_setCallbacks(client_, this, connlost, messageArrived, NULL)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to set callback, return code %d\n", rc);
+    return false;
+  }
+
+  conn_opts_.keepAliveInterval = 20;
+  conn_opts_.cleansession = 1;
+  conn_opts_.onSuccess = onConnect;
+  conn_opts_.onFailure = onConnectFailure;
+  conn_opts_.context = this;
+
+  if ((rc = MQTTAsync_connect(client_, &conn_opts_)) == MQTTASYNC_SUCCESS)
+  {
+    printf("Connected success \n");
+  }
+
+  address_=address;
+  port_=port;
+  return true;
+}
+bool Paho_client::disconnect(){
+  return true;
+}
+void Paho_client::publish(const std::string& topic,int QOS,const MqttMsg& msg){
+  if(connected_==false)
+  {
+    printf(" pls connect before publish\n");
+    return;
+  }
+  pub_opts_.onSuccess = onSend;
+  pub_opts_.onFailure = onSendFailure;
+  pub_opts_.context = client_;
+
+  pubmsg_.payload = msg.data();
+  pubmsg_.payloadlen = msg.length();
+  pubmsg_.qos = QOS;
+  pubmsg_.retained = 0;
+  int rc=-1;
+  if ((rc = MQTTAsync_sendMessage(client_, topic.c_str(), &pubmsg_, &pub_opts_)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start sendMessage, return code %d\n", rc);
+  }
+
+}
+void Paho_client::subcribe(std::string topic,int QOS,ArrivedCallback callback,void* context){
+  arrivedCallback_=callback;
+  context_=context;
+  MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer;
+  printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n"
+         "\n", topic.c_str(), clientid_.c_str(), QOS);
+  opts.onSuccess = onSubscribe;
+  opts.onFailure = onSubscribeFailure;
+  opts.context = this;
+  int rc=-1;
+
+  if ((rc = MQTTAsync_subscribe(client_, topic.c_str(), QOS, &opts)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start subscribe, return code %d\n", rc);
+    return;
+  }
+
+}
+
+int Paho_client::messageArrived(void* context, char* topicName, int topicLen, MQTTAsync_message* message)
+{
+  Paho_client* cl=(Paho_client*)context;
+  MqttMsg msg((char*)message->payload,message->payloadlen);
+  if(cl->arrivedCallback_!= nullptr)
+    cl->arrivedCallback_(topicName,message->qos,msg,cl->context_);
+  MQTTAsync_freeMessage(&message);
+  MQTTAsync_free(topicName);
+  return 1;
+}
+
+
+void Paho_client::connlost(void *context, char *cause)
+{
+  Paho_client* cl=(Paho_client*)context;
+  MQTTAsync client = cl->client_;
+  MQTTAsync_connectOptions conn_opts = cl->conn_opts_;
+  int rc;
+
+  printf("\nConnection lost\n");
+  printf("     cause: %s\n", cause);
+  printf("Reconnecting\n");
+  conn_opts.keepAliveInterval = 20;
+  conn_opts.cleansession = 1;
+  conn_opts.onSuccess = onConnect;
+  conn_opts.onFailure = onConnectFailure;
+  conn_opts.context = context;
+  cl->connected_=false;
+  if ((rc = MQTTAsync_connect(client, &conn_opts)) == MQTTASYNC_SUCCESS)
+  {
+    printf(" Reconnected success\n");
+    cl->connected_=true;
+  }
+}
+
+
+void Paho_client::onConnectFailure(void* context, MQTTAsync_failureData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s,Connect failed, rc %d\n", cl->clientid_.c_str(),response ? response->code : 0);
+
+  int rc=-1;
+  if ((rc = MQTTAsync_connect(cl->client_, &(cl->conn_opts_))) == MQTTASYNC_SUCCESS)
+  {
+    printf(" Reconnected success\n");
+  }
+
+}
+
+void Paho_client::onSubscribe(void* context, MQTTAsync_successData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s  Subscribe succeeded\n",cl->clientid_.c_str());
+
+}
+
+void Paho_client::onSubscribeFailure(void* context, MQTTAsync_failureData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  printf("clientid:%s Subscribe failed, rc %d\n",cl->clientid_.c_str(), response->code);
+}
+
+
+void Paho_client::onConnect(void* context, MQTTAsync_successData* response)
+{
+  Paho_client* cl=(Paho_client*)context;
+  cl->connected_=true;
+}
+
+
+void Paho_client::onSendFailure(void* context, MQTTAsync_failureData* response)
+{
+  MQTTAsync client = (MQTTAsync)context;
+  MQTTAsync_disconnectOptions opts = MQTTAsync_disconnectOptions_initializer;
+  int rc;
+
+  printf("Message send failed token %d error code %d\n", response->token, response->code);
+  opts.onSuccess = onDisconnect;
+  opts.onFailure = onDisconnectFailure;
+  opts.context = client;
+  if ((rc = MQTTAsync_disconnect(client, &opts)) != MQTTASYNC_SUCCESS)
+  {
+    printf("Failed to start disconnect, return code %d\n", rc);
+  }
+}
+
+void Paho_client::onSend(void* context, MQTTAsync_successData* response)
+{
+  // This gets called when a message is acknowledged successfully.
+}
+void Paho_client::onDisconnectFailure(void* context, MQTTAsync_failureData* response)
+{
+  printf("Disconnect failed\n");
+}
+
+void Paho_client::onDisconnect(void* context, MQTTAsync_successData* response)
+{
+  printf("Successful disconnection\n");
+}

emqx/paho_client.h

@@ -0,0 +1,58 @@
+//
+// Created by zx on 23-2-22.
+//
+
+#ifndef PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_
+#define PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_
+#include "MQTTAsync.h"
+#include <string>
+#include "mqttmsg.h"
+
+typedef void (*ArrivedCallback)(std::string topic,int QOS,MqttMsg& msg,void* context);
+
+class Paho_client
+{
+ public:
+    Paho_client(std::string clientid);
+    ~Paho_client();
+    void set_maxbuf(int size);
+    bool connect(std::string address,int port);
+    bool disconnect();
+    void subcribe(std::string topic,int QOS,ArrivedCallback callback,void* context);
+    void publish(const std::string& topic,int QOS,const MqttMsg& msg);
+
+    bool isconnected(){return connected_;}
+
+ protected:
+    static int messageArrived(void* context, char* topicName, int topicLen, MQTTAsync_message* message);
+    static void connlost(void *context, char *cause);
+    static void onConnectFailure(void* context, MQTTAsync_failureData* response);
+    static void onSubscribe(void* context, MQTTAsync_successData* response);
+    static void onSubscribeFailure(void* context, MQTTAsync_failureData* response);
+    static void onConnect(void* context, MQTTAsync_successData* response);
+    static void onSendFailure(void* context, MQTTAsync_failureData* response);
+    static void onSend(void* context, MQTTAsync_successData* response);
+    static void onDisconnectFailure(void* context, MQTTAsync_failureData* response);
+    static void onDisconnect(void* context, MQTTAsync_successData* response);
+
+ protected:
+
+    bool connected_;
+    std::string clientid_;
+    std::string address_;
+    int port_;
+
+    MQTTAsync client_;
+    MQTTAsync_connectOptions conn_opts_ = MQTTAsync_connectOptions_initializer;
+
+    MQTTAsync_message pubmsg_ = MQTTAsync_message_initializer;
+    MQTTAsync_responseOptions pub_opts_ = MQTTAsync_responseOptions_initializer;
+    MQTTAsync_createOptions create_opts_=MQTTAsync_createOptions_initializer;
+
+    ArrivedCallback arrivedCallback_= nullptr;
+    void* context_;
+
+};
+
+
+#endif //PAHOC_SAMPLE_SAMPLES_PAHO_CLIENT_H_

emqx/terminator_emqx.cpp

@@ -0,0 +1,69 @@
+//
+// Created by zx on 23-4-11.
+//
+
+#include "terminator_emqx.h"
+#include "unistd.h"
+Terminator_emqx::~Terminator_emqx()
+{
+  if(client_!= nullptr){
+    client_->disconnect();
+    delete client_;
+    client_= nullptr;
+  }
+}
+
+bool Terminator_emqx::Connect(std::string ip,int port)
+{
+
+  if(client_!= nullptr)
+  {
+    client_->disconnect();
+    delete client_;
+  }
+  client_=new Paho_client(nodeId_);
+  bool ret= client_->connect(ip,port);
+  if(ret)
+  {
+    while(!client_->isconnected()) usleep(1000);
+    client_->subcribe(subTopic_,1,StatuArrivedCallback,this);
+  }
+  return ret;
+
+}
+
+void Terminator_emqx::SendNavCmd(const NavMessage::NavCmd& cmd)
+{
+  MqttMsg msg;
+  msg.fromProtoMessage(cmd);
+  if(client_)
+    client_->publish(pubTopic_,1,msg);
+  else
+    printf("Send NavCmd failed : emqx client disconnected...\n");
+}
+
+void Terminator_emqx::set_statu_arrived(NavStatuCallback callback, void *context)
+{
+  StatuArrivedCallback_=callback;
+  context_=context;
+}
+
+
+Terminator_emqx::Terminator_emqx(std::string nodeId,std::string pubTopic,std::string subTopic)
+    :client_(nullptr),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic)
+{
+  StatuArrivedCallback_= nullptr;
+  context_= nullptr;
+}
+
+void Terminator_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
+{
+  Terminator_emqx* terminator=(Terminator_emqx*)context;
+  double x=0,y=0,theta=0,v=0,vth=0;
+  NavMessage::NavStatu statu;
+  if(msg.toProtoMessage(statu))
+  {
+    if (terminator->StatuArrivedCallback_)
+      terminator->StatuArrivedCallback_(statu,terminator->context_);
+  }
+}

emqx/terminator_emqx.h

@@ -0,0 +1,36 @@
+//
+// Created by zx on 23-4-11.
+//
+
+#ifndef NAVIGATION_TERMINATOR_EMQX_H
+#define NAVIGATION_TERMINATOR_EMQX_H
+#include <mutex>
+#include "paho_client.h"
+
+typedef void (*NavStatuCallback)(const NavMessage::NavStatu& statu,void* context);
+
+class Terminator_emqx {
+public:
+    Terminator_emqx(std::string nodeId,std::string pubTopic,std::string subTopic);
+    ~Terminator_emqx();
+    bool Connect(std::string ip,int port);
+    void set_statu_arrived(NavStatuCallback callback,void* context);
+    void SendNavCmd(const NavMessage::NavCmd& cmd);
+
+protected:
+
+    NavStatuCallback StatuArrivedCallback_;
+    static void StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context);
+
+    std::mutex mtx_;
+
+    Paho_client* client_;
+    void* context_;
+    std::string nodeId_;
+    std::string pubTopic_;
+    std::string subTopic_;
+public:
+    MqttMsg msg_;
+};
+
+#endif //NAVIGATION_TERMINATOR_EMQX_H

main.cpp

@@ -0,0 +1,290 @@
+
+#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;
+
+}

message.proto

@@ -0,0 +1,57 @@
+syntax = "proto3";
+package NavMessage;
+message AGVStatu {
+  float x=1;
+  float y=2;
+  float theta=3;
+  float v=4;
+  float vth=5;
+}
+
+message Speed {
+  int32 type=1; // 0原地旋转，1横移，2前进，3MPC巡线, 其他：停止
+  float v=2;
+  float vth=3;
+}
+
+
+message Pose2d
+{
+  float x=1;
+  float y=2;
+  float theta=3;
+}
+
+message Trajectory
+{
+  repeated Pose2d poses=1;
+}
+
+message Action
+{
+  int32 type=1; // 1:原地调整，2：巡线
+  Pose2d target=2;
+  Pose2d target_diff=3;
+}
+
+
+message NavCmd
+{
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
+  string key=2;
+  repeated Action actions=3;
+}
+
+message NavStatu
+{
+  bool statu=1; //0:ready  1:running
+  string key=2; // 任务唯一码
+  repeated Action unfinished_actions=3;  //未完成的动作，第一个即为当前动作
+  AGVStatu current_pose=4; //当前位姿
+  bool pose_timeout=5;
+  bool twist_timeout=6;   //速度是否超时
+  Trajectory selected_traj=7;
+  Trajectory predict_traj=8;
+
+}
+

pangolinViewer.cpp

@@ -0,0 +1,336 @@
+//
+// 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::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::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, 1, 7);
+  end_Int_=new pangolin::Var<int>("ui.end", 5, 1, 7);
+  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_();
+  });
+
+
+}
+
+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::ShowRealtimeStatu(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.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));
+  }
+}

pangolinViewer.h

@@ -0,0 +1,97 @@
+//
+// 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 <thread>
+#include "dijkstra/dijkstra.h"
+#include "dijkstra/trajectory.h"
+#include "define/typedef.h"
+
+class PangolinViewer
+{
+ public:
+    typedef void(*ViewerSpinOnceCallback)(PangolinViewer*);
+    typedef void(*StartLocateBtnCallback)(std::string);
+    typedef void(*StartBtnCallback)();
+    typedef void(*PauseBtnCallback)();
+    typedef void(*StopBtnCallback)();
+    typedef void(*FreqChangedCallback)(int);
+    typedef bool(*DijkstraBtnCallback)(int ,int ,float& ,std::vector<int>&);
+ public:
+    static PangolinViewer* CreateViewer();
+
+    void SetCallbacks(ViewerSpinOnceCallback spinOnce,DijkstraBtnCallback dijkstra);
+
+    void SetNavigationCallbacks(StartBtnCallback startBtn,
+                                PauseBtnCallback pauseBtn,
+                      StopBtnCallback stopBtn);
+    void ShowRealtimeStatu(Eigen::Matrix4d pose,double velocity,double angular);
+
+    void DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
+     double alpha,double psize);
+    void DrawDijkastraMap(PathMap& map,std::vector<int> path);
+    void DrawTrajectory(const Trajectory& traj,double r,double g,double b,double ptsize);
+    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>* velocity_string_;
+
+    pangolin::Var<int>* beg_Int_;
+    pangolin::Var<int>* end_Int_;
+
+    pangolin::Var<double>* v_;
+    pangolin::Var<double>* a_;
+
+    pangolin::Var<std::string>* path_string_;
+    pangolin::Var<std::string>* distance_string_;
+
+    std::vector<int> shortest_path_;
+
+    ViewerSpinOnceCallback spinOnce_callback_=nullptr;
+    DijkstraBtnCallback dijkstraBtn_callback_= nullptr;
+
+    StartBtnCallback naviagtion_startBtn_callback_=nullptr;
+    PauseBtnCallback naviagtion_pauseBtn_callback_=nullptr;
+    StopBtnCallback navigation_stopBtn_callback_= nullptr;
+
+};
+
+
+#endif //LIO_LIVOX_CPP__PANGOLINVIEWER_H_

proto.sh

@@ -0,0 +1,4 @@
+
+protoc -I=./ message.proto --cpp_out=./emqx
+
+

tool/StdCondition.cpp

@@ -0,0 +1,35 @@
+#include "StdCondition.h"
+
+StdCondition::StdCondition():m_value(false)
+{
+}
+
+StdCondition::StdCondition(bool init):m_value(init)
+{
+}
+
+StdCondition::~StdCondition()
+{
+}
+bool StdCondition::isTrue(StdCondition* scd)
+{
+	if (scd == 0)return false;
+	return scd->m_value;
+}
+
+void StdCondition::Wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_cv.wait(loc,std::bind(isTrue,this));
+}
+bool StdCondition::WaitFor(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	return m_cv.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(isTrue, this));
+}
+void StdCondition::Notify(bool istrue)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_value = istrue;
+	m_cv.notify_all();
+}

tool/StdCondition.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <thread>  
+#include <mutex>  
+#include <chrono>  
+#include <condition_variable>
+
+class StdCondition
+{
+public:
+	StdCondition();
+	StdCondition(bool init);
+	~StdCondition();
+	void Wait();
+	bool WaitFor(unsigned int millisecond);
+	void Notify(bool istrue);
+
+protected:
+	static bool isTrue(StdCondition* scd);
+protected:
+	bool m_value;
+	std::mutex m_mutex;
+	std::condition_variable m_cv;
+};
+

tool/pathcreator.cpp

@@ -0,0 +1,94 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdio.h>
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+std::string PathCreator::GetCurPath()
+{
+    return m_current_path;
+}
+bool PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+        if('/' != dirName[0])
+        {
+            fullPath = getcwd(nullptr, 0);
+            beginCmpPath = fullPath.size();
+            fullPath = fullPath + "/" + dirName;
+        }
+        else
+        {
+            //Absolute path
+            fullPath = dirName;
+            beginCmpPath = 1;
+        }
+        if (fullPath[fullPath.size() - 1] != '/')
+        {
+            fullPath += "/";
+        }
+        endCmpPath = fullPath.size();
+
+        //create dirs;
+        for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+        {
+            if('/' == fullPath[i])
+            {
+                std::string curPath = fullPath.substr(0, i);
+                if(access(curPath.c_str(), F_OK) != 0)
+                {
+                    if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                    {
+                        printf("mkdir(%s) failed\n", curPath.c_str());
+                        return false;
+                    }
+                }
+            }
+        }
+        m_current_path=fullPath;
+        return true;
+
+}
+
+bool PathCreator::CreateDatePath(std::string root, bool add_time)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    if (add_time)
+    {
+        sprintf(buf, "%s/%d%02d%02d-%02d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday,
+                t->tm_hour,
+                t->tm_min,
+                t->tm_sec);
+    }
+    else
+    {
+        sprintf(buf, "%s/%d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday);
+    }
+    return Mkdir(buf);
+}

tool/pathcreator.h

@@ -0,0 +1,17 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    std::string GetCurPath();
+    bool Mkdir(std::string dir);
+    bool CreateDatePath(std::string root, bool add_time = true);
+protected:
+    std::string m_current_path;
+};
+
+#endif // PATHCREATOR_H

tool/proto_tool.cpp

@@ -0,0 +1,42 @@
+
+
+
+#include "proto_tool.h"
+#include <fcntl.h>
+#include<unistd.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+
+//读取protobuf 配置文件，转化为protobuf参数形式
+//input：	prototxt_path ：prototxt文件路径
+//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+bool proto_tool::read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter)
+{
+	int fd = open(prototxt_path.c_str(), O_RDONLY);
+	if (fd == -1) return false;
+	FileInputStream* input = new FileInputStream(fd);
+	bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+	delete input;
+	close(fd);
+	return success;
+}
+
+
+
+
+
+
+
+
+
+
+
+

tool/proto_tool.h

@@ -0,0 +1,56 @@
+
+
+
+
+
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
+
+#include "../tool/singleton.h"
+#include <istream>
+#include <google/protobuf/message.h>
+
+class proto_tool:public Singleton<proto_tool>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<proto_tool>;
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	proto_tool(const proto_tool&)=delete;
+	proto_tool& operator =(const proto_tool&)= delete;
+	~proto_tool()=default;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	proto_tool()=default;
+
+
+public:
+	//读取protobuf 配置文件，转化为protobuf参数形式
+	//input：	prototxt_path ：prototxt文件路径
+	//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+	static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter);
+};
+
+
+
+
+#endif //__PROTO_TOOL_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

tool/singleton.cpp

@@ -0,0 +1,4 @@
+
+#include "singleton.h"
+
+

tool/singleton.h

@@ -0,0 +1,81 @@
+
+/* Singleton 是单例类的模板。
+ * https://www.cnblogs.com/sunchaothu/p/10389842.html
+ * 单例 Singleton 是设计模式的一种，其特点是只提供唯一一个类的实例,具有全局变量的特点，在任何位置都可以通过接口获取到那个唯一实例;
+ * 全局只有一个实例：static 特性，同时禁止用户自己声明并定义实例（把构造函数设为 private 或者 protected）
+ * Singleton 模板类对这种方法进行了一层封装。
+ * 单例类需要从Singleton继承。
+ * 子类需要将自己作为模板参数T 传递给 Singleton<T> 模板;
+ * 同时需要将基类声明为友元，这样Singleton才能调用子类的私有构造函数。
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来操作 唯一的实例。
+ * */
+
+#ifndef __SINGLIETON_H
+#define __SINGLIETON_H
+
+//#include <iostream>
+
+template<typename T>
+class Singleton
+{
+public:
+	//获取单例的引用
+	static T& get_instance_references()
+	{
+		static T instance;
+		return instance;
+	}
+	//获取单例的指针
+	static T* get_instance_pointer()
+	{
+		return &(get_instance_references());
+	}
+
+	virtual ~Singleton()
+	{
+//		std::cout<<"destructor called!"<<std::endl;
+	}
+
+	Singleton(const Singleton&)=delete;					//关闭拷贝构造函数
+	Singleton& operator =(const Singleton&)=delete;		//关闭赋值函数
+
+protected:
+	//构造函数需要是 protected，这样子类才能继承；
+	Singleton()
+	{
+//		std::cout<<"constructor called!"<<std::endl;
+	}
+
+};
+
+/*
+// 如下是 使用样例：
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+
+class DerivedSingle:public Singleton<DerivedSingle>
+{
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<DerivedSingle>;
+public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	DerivedSingle(const DerivedSingle&)=delete;
+	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+	DerivedSingle()=default;
+};
+
+int main(int argc, char* argv[]){
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
+	return 0;
+}
+
+ */
+
+#endif

tool/threadSafeQueue.h

@@ -0,0 +1,101 @@
+//
+// Created by zx on 2019/12/17.
+//
+
+#ifndef SRC_THREADSAFEQUEUE_H
+#define SRC_THREADSAFEQUEUE_H
+
+
+#include <queue>
+#include <memory>
+#include <mutex>
+#include <condition_variable>
+template<typename T>
+class threadsafe_queue
+{
+private:
+    mutable std::mutex mut;
+    std::queue<T> data_queue;
+    std::condition_variable data_cond;
+public:
+    threadsafe_queue() {}
+    threadsafe_queue(threadsafe_queue const& other)
+    {
+        std::lock_guard<std::mutex> lk(other.mut);
+        data_queue = other.data_queue;
+    }
+    ~threadsafe_queue()
+    {
+        while (!empty())
+        {
+            try_pop();
+        }
+    }
+    size_t size()
+    {
+        return data_queue.size();
+    }
+    void push(T new_value)//��Ӳ���
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        data_queue.push(new_value);
+        data_cond.notify_one();
+    }
+    void wait_and_pop(T& value)//ֱ����Ԫ�ؿ���ɾ��Ϊֹ
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        value = data_queue.front();
+        data_queue.pop();
+    }
+    std::shared_ptr<T> wait_and_pop()
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    //ֻ���� �� pop
+    bool front(T& value)
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return false;
+        value = data_queue.front();
+        return true;
+    }
+
+    bool try_pop(T& value)//������û�ж���Ԫ��ֱ�ӷ���
+    {
+        if (data_queue.empty())
+            return false;
+        std::lock_guard<std::mutex> lk(mut);
+        value = data_queue.front();
+        data_queue.pop();
+        return true;
+    }
+    std::shared_ptr<T> try_pop()
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return std::shared_ptr<T>();
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    bool empty() const
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        return data_queue.empty();
+    }
+    void clear()
+    {
+        while (!empty()) {
+            try_pop();
+        }
+    }
+};
+
+
+#endif //SRC_THREADSAFEQUEUE_H