剥离导航部分，纯定位

CMakeLists.txt

@@ -57,7 +57,6 @@ include_directories(
 		lio/include
 		lio/src
 		${Pangolin_INCLUDE_DIRS}
-		${catkin_INCLUDE_DIRS}
 		${EIGEN3_INCLUDE_DIR}
 		${PCL_INCLUDE_DIRS}
 		${CERES_INCLUDE_DIRS}
@@ -107,15 +106,7 @@ add_executable(${PROJECT_NAME}_node
 		livox/common/comm/gps_protocol.cpp
 
 		pangolinViewer.cpp
-		dijkstra/dijkstra.cpp
-		MPC/loaded_mpc.cpp
-		MPC/trajectory.cpp
-		MPC/pose2d.cpp
-		MPC/navigation.cpp
 
-		MPC/monitor/communication_socket.cpp
-		MPC/monitor/monitor.cpp
-		MPC/monitor/Plc_socket.cpp
 
     )
 
@@ -140,15 +131,6 @@ target_link_libraries(${PROJECT_NAME}_node
 	)
 
 
-add_executable(dijkstra_sample
-		dijkstraSample.cpp
-		dijkstra/dijkstra.cpp
-		)
-
-target_link_libraries(dijkstra_sample
-		pango_display
-		)
-
 #---------------------------------------------------------------------------------------
 # end of CMakeList.txt
 #---------------------------------------------------------------------------------------

MPC/loaded_mpc.cpp

@@ -1,380 +0,0 @@
-//
-// Created by zx on 22-12-1.
-//
-
-#include "loaded_mpc.h"
-#include <cppad/cppad.hpp>
-#include <cppad/ipopt/solve.hpp>
-
-size_t N = 20;                  //优化考虑后面多少步
-
-size_t nx = 0;
-size_t ny = nx + N;
-size_t nth = ny + N;
-size_t nv = nth + N;
-size_t ndlt = nv + N;
-
-class FG_eval_half_agv {
- public:
-    // Fitted polynomial coefficients
-    Eigen::VectorXd m_coeffs;                 //曲线方程
-    Eigen::VectorXd m_statu;                  //当前状态
-    Eigen::VectorXd m_condition;              //搜索条件参数
-    FG_eval_half_agv(Eigen::VectorXd coeffs,Eigen::VectorXd statu,Eigen::VectorXd condition) {
-      m_coeffs=coeffs;
-      m_statu=statu;
-      m_condition=condition;
-    }
-
-    typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;
-    void operator()(ADvector& fg, const ADvector& vars) {
-      // Implementing MPC below
-      // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
-      // The cost is stored is the first element of `fg`.
-      // Any additions to the cost should be added to `fg[0]`.
-      fg[0] = 0;
-      double dt=m_condition[0];
-      double ref_v=m_condition[1];
-      double v=m_statu[0];
-      double delta=m_statu[1];
-      double wheelbase=m_statu[2];
-
-      // Reference State Cost
-      // Below defines the cost related the reference state and
-      // any anything you think may be beneficial.
-
-      // Weights for how "important" each cost is - can be tuned
-      const int y_cost_weight = 12000;
-      const int th_cost_weight = 1000;
-      const int v_cost_weight = 20;
-      const int vth_cost_weight = 100;
-      const int a_cost_weight = 1;
-      const int ath_cost_weight=10;
-
-      // Cost for CTE, psi error and velocity
-      for (int t = 0; t < N; t++) {
-        CppAD::AD<double> xt=vars[nx+t];
-        CppAD::AD<double> fx = m_coeffs[0] + m_coeffs[1] * xt + m_coeffs[2] * pow(xt, 2) + m_coeffs[3] * pow(xt, 3);
-        fg[0] += y_cost_weight * CppAD::pow(vars[ny+t]-fx, 2);
-
-        //朝向loss
-        CppAD::AD<double> fth = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
-        fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
-
-        //目标速度loss
-        fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-ref_v,2);
-
-      }
-
-      // Costs for steering (delta) and acceleration (a)
-
-      for (int t = 0; t < N-1; t++) {
-        //速度，加速度，前轮角 weight loss
-        fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt+t], 2);
-        fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
-        fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt+t+1]-vars[ndlt+t], 2);
-      }
-      fg[0]*=0.01;
-
-      /////////////////////
-      fg[1 + nx] = vars[nx]-vars[nv]*dt;
-      fg[1 + ny] = vars[ny];
-      CppAD::AD<double> w0=vars[nv]/wheelbase*CppAD::tan(vars[ndlt]);
-      fg[1 + nth] = vars[nth]-w0*dt;
-
-      //位姿约束
-      for (int t = 1; t < N; t++) {
-        // State at time t + 1
-        CppAD::AD<double> x1 = vars[nx + t];
-        CppAD::AD<double> y1 = vars[ny + t];
-        CppAD::AD<double> th1 = vars[nth + t];
-
-        // State at time t
-        CppAD::AD<double> x0 = vars[nx + t -1];
-        CppAD::AD<double> y0 = vars[ny + t -1];
-        CppAD::AD<double> th0 = vars[nth + t-1];
-        CppAD::AD<double> v0 = vars[nv + t-1];
-
-        CppAD::AD<double> w_0=vars[nv+t-1]/wheelbase*CppAD::tan(vars[ndlt+t-1]);
-
-        // Setting up the rest of the model constraints
-        fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
-        fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
-        fg[1 + nth + t] = th1 - (th0 + w_0 * dt);
-
-      }
-      //加速度和dlt约束
-      fg[1 + nv]=(vars[nv]-v)/dt;
-      fg[1+ndlt]=(vars[ndlt]-delta)/dt;
-      for(int t=1;t<N;++t)
-      {
-        fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
-        fg[1+ndlt+t]=(vars[ndlt+t]-vars[ndlt+t-1])/dt;
-      }
-
-    }
-};
-
-LoadedMPC::LoadedMPC(double wheelbase){
-  wheelbase_=wheelbase;
-}
-LoadedMPC::~LoadedMPC(){}
-
-bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
-                                         std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory)
-{
-  auto start=std::chrono::steady_clock::now();
-  // State vector holds all current values neede for vars below
-  Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
-  double line_velocity = statu[3];
-  double angular_velocity = statu[4];
-  std::vector<Pose2d> filte_poses;
-  if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == false)
-  {
-    printf( "filte path failed ...\n");
-    return false;
-  }
-  select_traj= Trajectory(filte_poses);
-
-  //将选中点移动到小车坐标系
-  std::vector<Pose2d> transform_poses;
-  for (int i = 0; i < filte_poses.size(); i++)
-  {
-    double x = filte_poses[i].x() - pose_agv.x();
-    double y = filte_poses[i].y() - pose_agv.y();
-    transform_poses.push_back(Pose2d(x,y,0).rotate(-pose_agv.theta()));
-  }
-
-  Eigen::VectorXd coef = fit_path(transform_poses);
-
-  //优化
-  bool ok = true;
-  typedef CPPAD_TESTVECTOR(double) Dvector;
-
-  //根据当前点和目标点距离,计算目标速度
-
-  double ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
-  //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0
-
-  float gradient=fabs(target.x()-pose_agv.x())>1e-6?(target.y()-pose_agv.y())/(target.x()-pose_agv.x()):500.0;
-  double direction=gradient2theta(gradient,target.x()>=pose_agv.x());
-
-  //        pose_agv.theta()*k,direction*k,k*fabs(pose_agv.theta()-direction));
-  if (fabs(direction-pose_agv.theta()) >M_PI/2.0)
-    ref_velocity = -ref_velocity;
-
-
-  double dt = 0.1;
-  double min_velocity = 0.05;
-  double max_velocity = 1.9;
-  double max_dlt=5*M_PI/180.0;
-  double max_acc_line_velocity=1.9/(N*dt);
-  double max_acc_dlt=5*M_PI/180.0;
-
-  size_t n_vars = N * 5;
-  size_t n_constraints = N * 5;
-
-  Dvector vars(n_vars);
-  for (int i = 0; i < n_vars; i++)
-  {
-    vars[i] = 0.0;
-  }
-
-  Dvector vars_lowerbound(n_vars);
-  Dvector vars_upperbound(n_vars);
-
-  for (int i = 0; i < n_vars; i++)
-  {
-    vars_lowerbound[i] = -1.0e19;
-    vars_upperbound[i] = 1.0e19;
-  }
-
-  //// limit  v
-  for (int i = nv; i < nv + N; i++)
-  {
-    vars_lowerbound[i] = min_velocity;
-    vars_upperbound[i] = max_velocity;
-  }
-
-  ////limint dlt
-  for (int i = ndlt; i < ndlt + N; i++)
-  {
-    vars_lowerbound[i] = -max_dlt;
-    vars_upperbound[i] = max_dlt;
-  }
-
-  // Lower and upper limits for the constraints
-  Dvector constraints_lowerbound(n_constraints);
-  Dvector constraints_upperbound(n_constraints);
-  for (int i = 0; i < n_constraints; i++)
-  {
-    constraints_lowerbound[i] = 0;
-    constraints_upperbound[i] = 0;
-  }
-  //// acc v
-  for (int i = nv; i < nv + N; i++)
-  {
-    constraints_lowerbound[i] = -max_acc_line_velocity;
-    constraints_upperbound[i] = max_acc_line_velocity;
-  }
-  //// acc ndlt
-  for (int i = ndlt; i < ndlt + N; i++)
-  {
-    constraints_lowerbound[i] = -max_acc_dlt;
-    constraints_upperbound[i] = max_acc_dlt;
-  }
-
-
-  Eigen::VectorXd statu_velocity(3);
-  if(line_velocity>max_velocity)
-  {
-    line_velocity=max_velocity;
-    printf(" input +v limited\n");
-  }
-  if(line_velocity<min_velocity)
-  {
-    line_velocity=min_velocity;
-    printf(" input -v limited\n");
-  }
-
-  double omga=0;
-  if(abs(line_velocity)>0.000001)
-  {
-    omga=angular_velocity*wheelbase_/tan(line_velocity);
-  }
-  if(omga>max_dlt)
-  {
-    omga = max_dlt;
-    printf(" input +dlt limited\n");
-  }
-  if(omga<-max_dlt)
-  {
-    omga = -max_dlt;
-    printf(" input -dlt limited\n");
-  }
-
-  statu_velocity << line_velocity, omga,wheelbase_;
-
-  Eigen::VectorXd condition(2);
-  condition << dt, ref_velocity;
-  FG_eval_half_agv fg_eval(coef, statu_velocity, condition);
-
-  // options for IPOPT solver
-  std::string options;
-  // Uncomment this if you'd like more print information
-  options += "Integer print_level  0\n";
-  options += "Sparse  true        forward\n";
-  options += "Sparse  true        reverse\n";
-  options += "Numeric max_cpu_time          0.5\n";
-  // place to return solution
-  CppAD::ipopt::solve_result<Dvector> solution;
-
-  // solve the problem
-  CppAD::ipopt::solve<Dvector, FG_eval_half_agv>(
-          options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
-          constraints_upperbound, fg_eval, solution);
-
-  auto now=std::chrono::steady_clock::now();
-  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - start);
-  double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
-
-  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
-  if (ok == false)
-  {
-    printf(" mpc failed statu : %d  input: %.4f  %.4f  \n",solution.status,line_velocity,
-            angular_velocity);
-    return false;
-  }
-
-  // Cost
-  auto cost = solution.obj_value;
-
-  out.clear();
-
-  double solve_velocity=solution.x[nv];
-  /*if(fabs(solve_velocity)<min_velocity)
-  {
-    solve_velocity= solve_velocity>0?min_velocity:-min_velocity;
-  }*/
-
-  out.push_back(solve_velocity);
-  double omg=solve_velocity/wheelbase_*tan(solution.x[ndlt]);
-  out.push_back(omg);
-  printf(" input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",line_velocity,
-         angular_velocity,solve_velocity,omg,ref_velocity,time);
-
-  optimize_trajectory.clear();
-  for (int i = 0; i < N; ++i)
-  {
-    Pose2d pose(solution.x[nx + i], solution.x[ny + i], solution.x[nth + i]);
-    optimize_trajectory.push_point(pose_agv+pose.rotate(pose_agv.theta()));
-  }
-
-  return true;
-}
-
-bool LoadedMPC::filte_Path(const Pose2d& point,const Pose2d& target,const Trajectory& trajectory,
-                            std::vector<Pose2d>& poses,int point_num)
-{
-  double gradient=0;
-  if(fabs((target-point).x())==0)
-    gradient=200.0;
-  else
-    gradient= (target-point).y()/(target-point).x();
-
-  double theta=gradient2theta(gradient,target.x()-point.x()>=0);
-
-  if(trajectory.size() < point_num)
-    return false;
-
-  poses.clear();
-
-  //double theta=point.theta();
-
-  //if(fabs(theta)>M_PI/2.0) theta+=M_PI;
-  for (int i = 0; i < trajectory.size(); i++)
-  {
-
-    // 平移加反向旋转到小车坐标系
-    Pose2d offset=trajectory[i]-point;
-    double x = offset.x();
-    double y = offset.y();
-    double trans_x = x * cos(-theta) - y * sin(-theta);
-    double trans_y = x * sin(-theta) + y * cos(-theta);
-    if (trans_x>=0  && poses.size() < point_num)
-    {
-
-      // 旋转到原坐标系
-      float nx=trans_x * cos(theta) - trans_y * sin(theta);
-      float ny=trans_x*sin(theta)+trans_y*cos(theta);
-      Pose2d pose(nx+point.x(),ny+point.y(),trajectory[i].theta());
-
-      poses.push_back(pose);
-
-    }
-  }
-  return true;
-}
-
-Eigen::VectorXd LoadedMPC::fit_path(const std::vector<Pose2d>& trajectory)
-{
-  int order=3;
-  assert(order >= 1 && order <= trajectory.size() - 1);
-  Eigen::MatrixXd A(trajectory.size(), order + 1);
-  Eigen::VectorXd yvals(trajectory.size());
-  for (int i = 0; i < trajectory.size(); i++) {
-    A(i, 0) = 1.0;
-    yvals[i]=trajectory[i].y();
-  }
-
-  for (int j = 0; j < trajectory.size(); j++) {
-    for (int i = 0; i < order; i++) {
-      A(j, i + 1) = A(j, i) * trajectory[j].x();
-    }
-  }
-
-
-  auto Q = A.householderQr();
-  auto result = Q.solve(yvals);
-  return result;
-}

MPC/loaded_mpc.h

@@ -1,35 +0,0 @@
-//
-// Created by zx on 22-12-1.
-//
-
-#ifndef LIO_LIVOX_CPP_MPC_LOADED_MPC_H_
-#define LIO_LIVOX_CPP_MPC_LOADED_MPC_H_
-
-#include <Eigen/Core>
-#include "trajectory.h"
-
-class LoadedMPC
-{
- public:
-    LoadedMPC(double wheelbase);
-    ~LoadedMPC();
-    virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
-                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
-
- protected:
-    /*
-     * 根据当前点和轨迹，选择前方 point_num
-     * point:当前点，target：目标点
-     */
-    static bool filte_Path(const Pose2d& point,const Pose2d& target,
-                           const Trajectory& trajectory,std::vector<Pose2d>& poses,int point_num=10);
-    /*
-     * 根据路径点，拟合曲线
-     */
-    static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
-    double wheelbase_;
-
-};
-
-
-#endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_

MPC/monitor/Plc_socket.cpp

@@ -1,163 +0,0 @@
-//
-// Created by wk on 2022/12/6.
-//
-
-#include "Plc_socket.h"
-Plc_socket::Plc_socket()
-{
-  m_read_callback = NULL;
-}
-
-Plc_socket::~Plc_socket()
-{
-	plc_socket_uninit();
-}
-
-bool Plc_socket::plc_socket_init(std::string ip, unsigned short port)
-{
-
-	m_ip = ip;
-	m_port = port;
-	m_socket.communication_socket_init();
-	bool result = m_socket.communication_socket_connect( m_ip.c_str(),m_port);
-	if (  result )
-	{
-		std::cout << "ip = "<<m_ip<<" -------- port = "<<m_port<< " --------  连接成功！" <<std::endl;
-	}
-	else
-	{
-		std::cout << "ip = "<<m_ip<<" ######## port = "<<m_port<< " ########  连接失败！" <<std::endl;
-
-	}
-	//启动接收线程
-	m_read_data_condition = true;
-	mp_read_data_thread = new std::thread(&Plc_socket::read_plc_data_thread, this);
-	//启动写入线程
-	m_write_data_condition = true;
-	mp_write_data_thread = new std::thread(&Plc_socket::write_plc_data_thread, this);
-	//启动重连线程
-	m_reconnect_condition = true;
-	mp_reconnect_thread = new std::thread(&Plc_socket::reconnect_thread, this);
-
-	return true;
-
-}
-void Plc_socket::plc_socket_uninit()
-{
-	m_reconnect_condition = false;
-	m_read_data_condition = false;
-	m_write_data_condition = false;
-	if (mp_reconnect_thread)
-	{
-		mp_reconnect_thread->join();
-		delete mp_reconnect_thread;
-		mp_reconnect_thread = NULL;
-	}
-	if (mp_read_data_thread)
-	{
-		mp_read_data_thread->join();
-		delete mp_read_data_thread;
-		mp_read_data_thread = NULL;
-	}
-	if (mp_write_data_thread)
-	{
-		mp_write_data_thread->join();
-		delete mp_write_data_thread;
-		mp_write_data_thread = NULL;
-	}
-	m_read_callback = NULL;
-	m_socket.communication_socket_uninit();
-}
-void Plc_socket::read_plc_data_thread()
-{
-	std::cout<< " Plc_socket::read_plc_data_thread  start "<< std::endl;
-
-	//客户端连接会自动重连 因此这里只需要检查服务端是否断开
-	while (m_read_data_condition)
-	{
-		std::this_thread::sleep_for(std::chrono::milliseconds(1));
-		if (m_read_data_condition)
-		{
-			std::this_thread::yield();
-			char buf[2048];
-			memset(buf, 0, 2048);
-			//循环接收消息
-			int length = m_socket.communication_socket_recv(buf, sizeof(buf));
-			//大于0有效数据
-			if( length > 0)
-			{
-				if (m_read_callback != NULL)
-          m_read_callback(buf, client_);
-			}
-		}
-	}
-	std::cout << " Plc_socket::read_plc_data_thread  end "<< this <<std::endl;
-}
-void Plc_socket::write_plc_data_thread()
-{
-	std::cout<< " Plc_socket::write_plc_data_thread  start "<< this <<std::endl;
-
-	while (m_write_data_condition)
-	{
-		std::this_thread::sleep_for(std::chrono::milliseconds(1));
-		if (m_write_data_condition)
-		{
-			std::this_thread::yield();
-			//循环检测是否有消息
-			if(m_write_data_queue.size() > 0)
-			{
-				std::string data = m_write_data_queue.front();
-				int send_length = m_socket.communication_socket_send(data.c_str(),data.length());
-				//成功发送就清除此消息
-				if (send_length >0)
-					m_write_data_queue.pop();
-			}
-		}
-	}
-	std::cout<< " Plc_socket::write_plc_data_thread  end "<< this <<std::endl;
-
-}
-void Plc_socket::reconnect_thread()
-{
-	std::cout<< " Plc_socket::reconnect_thread  start "<< this <<std::endl;
-
-	//客户端连接会自动重连 因此这里只需要检查服务端是否断开
-	while (m_reconnect_condition)
-	{
-		std::this_thread::sleep_for(std::chrono::milliseconds(100));
-		if (m_reconnect_condition)
-		{
-			std::this_thread::yield();
-			//检查服务端连接是否正常
-			if ( !m_socket.is_connected() )
-			{
-				//关闭此前的连接
-				m_socket.communication_socket_uninit();
-				//重新初始化连接
-				m_socket.communication_socket_init();
-				//尝试重连
-				bool result = m_socket.communication_socket_connect(m_ip.c_str(),m_port);
-				if (  result )
-				{
-					std::cout << "ip = "<<m_ip<<" -------- port = "<<m_port<< " --------  重连成功！" <<std::endl;
-				}
-				else
-				{
-					std::cout << "ip = "<<m_ip<<" ######## port = "<<m_port<< " ########  重连失败！" <<std::endl;
-				}
-			}
-		}
-	}
-	std::cout << " Plc_socket::reconnect_thread  end "<< this <<std::endl;
-}
-//设置回调
-void Plc_socket::set_read_callback(ReadCallback rcb,void* client)
-{
-	m_read_callback = rcb;
-	client_=client;
-}
-//消息入队
-void Plc_socket::write_data(std::string data)
-{
-	m_write_data_queue.push(data);
-}

MPC/monitor/Plc_socket.h

@@ -1,56 +0,0 @@
-//
-// Created by wk on 2022/12/6.
-//
-
-#ifndef PLC_SOCKET_TEST_PLC_SOCKET_H
-#define PLC_SOCKET_TEST_PLC_SOCKET_H
-
-#include <string.h>
-#include <string>
-#include <queue>
-#include "communication_socket.h"
-
-typedef void (*ReadCallback)(char*,void*);
-
-
-class Plc_socket
-{
-public:
-	Plc_socket();
-	virtual ~Plc_socket();
-public://API functions
-	bool plc_socket_init(std::string ip, unsigned short port);//初始化
-	void set_read_callback(ReadCallback rcb,void* client);//设置回调函数
-	void write_data(std::string data);//写入PLC数据
-	void plc_socket_uninit();//初始化
-
-private:
-	//读取数据线程
-	void read_plc_data_thread();
-	//写入数据线程
-	void write_plc_data_thread();
-	//自动重连线程
-	void reconnect_thread();
-
-private:
-	Communication_socket m_socket;	//socket
-	ReadCallback m_read_callback;	//接收数据回调
-	void* client_= nullptr;
-	std::string m_ip;				//IP地址
-	unsigned short m_port;  		//端口
-
-	std::thread* mp_reconnect_thread;				//重连线程
-	bool m_reconnect_condition;						//重连线程条件变量
-
-	std::thread* mp_read_data_thread;//读取线程
-	bool m_read_data_condition;//读取线程的条件变量
-
-	std::thread* mp_write_data_thread;				//写线程
-	bool m_write_data_condition;						//写线程条件变量
-
-	std::queue<std::string> m_write_data_queue;//写入数据queue
-
-};
-
-
-#endif //PLC_SOCKET_TEST_PLC_SOCKET_H

MPC/monitor/communication_socket.cpp

@@ -1,330 +0,0 @@
-#include "communication_socket.h"
-#include "string.h"
-Communication_socket::Communication_socket()
-{
-}
-Communication_socket::~Communication_socket()
-{
-}
-
-bool Communication_socket::communication_socket_init()				 //创建套接字
-{
-
-	//创建socket （TCP/IP协议 TCP）
-	m_socket = socket(AF_INET, SOCK_STREAM, 0);    //直接创建socket返回给Communication_socket的成员
-	if (m_socket == -1)
-	{
-		printf(" create tcp socket failed ");
-		return false;
-	}
-	else
-	{
-		//printf(" create tcp socket successed ");
-		return true;
-	}
-
-}
-
-bool Communication_socket::communication_socket_bind(const char *ip,unsigned short port)  //绑定并监听端口号(服务端用)
-{
-	sockaddr_in saddr;              //数据结构
-	saddr.sin_family = AF_INET;     //协议
-	saddr.sin_port = htons(port);   //端口，主机字节序（小端方式）转换成网络字节序（大端方式）
-	saddr.sin_addr.s_addr = inet_addr(ip);   //绑定IP
-
-	if (bind(m_socket, (sockaddr*)&saddr, sizeof(saddr)) != 0)
-	{
-		printf(" tcp bind ip:%s port:%d failed\n",ip,port);
-
-		return false;
-	}
-	printf(" tcp bind ip:%s port:%d success\n",ip,port);
-	return true;
-}
-
-bool Communication_socket::communication_socket_listen(unsigned short num) //最大连接数
-{
-	int re = listen(m_socket, num);   //套接字，最大请求队列的长度   进入阻塞状态
-	if(!re)
-	{
-		printf(" tcp socket listen start ");
-		return true;
-	}
-	else
-	{
-		printf(" tcp socket listen failed ");
-		return false;
-	}
-}
-
-bool Communication_socket::set_block(bool isblock)  //设置阻塞模式  （希望只有在connect的时候是非阻塞的，而接收数据时候是阻塞的）
-{
-	if (m_socket <= 0)
-	{
-		printf(" set tcp socket block failed\n ");
-		return false;
-	}
-
-	int flags = fcntl(m_socket, F_GETFL, 0);  //获取socket的属性
-	if (flags < 0)
-		return false; //获取属性出错
-	if (isblock)
-	{
-		flags = flags&~O_NONBLOCK;  //把非阻塞这位设为0
-	}
-	else
-	{
-		flags = flags | O_NONBLOCK; //把非阻塞这位设为1
-	}
-	if (fcntl(m_socket, F_SETFL, flags))
-		return false;  //把标准位设回去
-
-//	if (!isblock)
-//		printf("set tcp socket not block success\n");
-//	if (isblock)
-//		printf("set tcp socket block success\n");
-
-	return true;
-}
-
-bool Communication_socket::communication_socket_connect(const char *ip, unsigned short port , int sec)
-{
-	if (m_socket <= 0)
-		return false;
-
-	sockaddr_in saddr;   //设置连接对象的结构体
-	saddr.sin_family = AF_INET;
-	saddr.sin_port = htons(port);
-	saddr.sin_addr.s_addr = inet_addr(ip);  //字符串转整型
-
-	set_block(false);    //将socket改成非阻塞模式，此时它会立即返回  所以通过fd_set
-	fd_set rfds, wfds;	    //文件句柄数组，在这个数组中，存放当前每个文件句柄的状态
-
-	if (connect(m_socket, (sockaddr*)&saddr, sizeof(saddr)) != 0)   //此时connect马上返回，状态为未成功连接
-	{
-
-		FD_ZERO(&rfds);  //首先把文件句柄的数组置空
-		FD_ZERO(&wfds);
-		FD_SET(m_socket, &rfds);   //把sock的网络句柄加入到该句柄数组中
-		FD_SET(m_socket, &wfds);
-
-
-		timeval tm;  //超时参数的结构体
-		tm.tv_sec = sec;
-		tm.tv_usec = 0;
-
-		int selres = select(m_socket + 1, &rfds, &wfds, NULL, &tm);   //（阻塞函数）(监听的文件句柄的最大值加1，可读序列文件列表，可写的序列文件列表，错误处理，超时)使用select监听文件序列set是否有可读可写，这里监听set数组（里面只有sock），只要其中的句柄有一个变得可写（在这里是sock连接成功了以后就会变得可写，就返回），就返回
-		switch (selres)
-		{
-
-			case -1:
-				printf("select error\n");
-				return false;
-			case 0:
-				printf("select time out\n");
-				return false;
-			default:
-				if (FD_ISSET(m_socket, &rfds) || FD_ISSET(m_socket, &wfds))
-				{
-					connect(m_socket, (sockaddr*)&saddr, sizeof(saddr));    //再次连接一次进行确认
-					int err = errno;
-					if  (err == EISCONN||err == EINPROGRESS)     //已经连接到该套接字 或 套接字为非阻塞套接字，且连接请求没有立即完成
-					{
-						//printf("connect %s : %d finished(success).\n",ip,port);
-						set_block(true);   //成功之后重新把sock改成阻塞模式，以便后面发送/接收数据
-						return true;
-					}
-					else
-					{
-						printf("connect %s : %d finished(failed). errno = %d\n",ip,port,errno);
-						// printf("FD_ISSET(sock_fd, &rfds): %d\n FD_ISSET(sock_fd, &wfds): %d\n", FD_ISSET(sock_fd, &rfds) , FD_ISSET(sock_fd, &wfds));
-						return false;
-					}
-				}
-				else
-				{
-					printf("connect %s : %d finished(failed).",ip,port);
-					return false;
-				}
-		}
-
-	}
-	else  //连接正常
-	{
-		set_block(true);   //成功之后重新把sock改成阻塞模式，以便后面发送/接收数据
-		printf("connect %s : %d finished(success).\n",ip,port);
-		return true;
-	}
-}
-
-bool Communication_socket::communication_socket_connect(int sec)
-{
-	if (m_socket <= 0)	return false;
-
-	sockaddr_in saddr;   //设置连接对象的结构体
-	saddr.sin_family = AF_INET;
-	saddr.sin_port = htons(m_server_port);
-	saddr.sin_addr.s_addr = inet_addr(m_server_ip);  //字符串转整型
-
-	set_block(false);    //将socket改成非阻塞模式，此时它会立即返回  所以通过fd_set
-	fd_set rfds, wfds;	    //文件句柄数组，在这个数组中，存放当前每个文件句柄的状态
-
-	if (connect(m_socket, (sockaddr*)&saddr, sizeof(saddr)) != 0)   //此时connect马上返回，状态为未成功连接
-	{
-
-		FD_ZERO(&rfds);  //首先把文件句柄的数组置空
-		FD_ZERO(&wfds);
-		FD_SET(m_socket, &rfds);   //把sock的网络句柄加入到该句柄数组中
-		FD_SET(m_socket, &wfds);
-
-
-		timeval tm;  //超时参数的结构体
-		tm.tv_sec = sec;
-		tm.tv_usec = 0;
-
-		int selres = select(m_socket + 1, &rfds, &wfds, NULL, &tm);   //（阻塞函数）(监听的文件句柄的最大值加1，可读序列文件列表，可写的序列文件列表，错误处理，超时)使用select监听文件序列set是否有可读可写，这里监听set数组（里面只有sock），只要其中的句柄有一个变得可写（在这里是sock连接成功了以后就会变得可写，就返回），就返回
-		switch (selres)
-		{
-
-			case -1:
-				printf("select error\n");
-				return false;
-			case 0:
-				printf("select time out\n");
-				return false;
-			default:
-				if (FD_ISSET(m_socket, &rfds) || FD_ISSET(m_socket, &wfds))
-				{
-					connect(m_socket, (sockaddr*)&saddr, sizeof(saddr));    //再次连接一次进行确认
-					int err = errno;
-					if  (err == EISCONN||err == EINPROGRESS)     //已经连接到该套接字 或 套接字为非阻塞套接字，且连接请求没有立即完成
-					{
-						//printf("connect %s : %d finished(success).\n",m_server_ip,m_server_port);
-						set_block(true);   //成功之后重新把sock改成阻塞模式，以便后面发送/接收数据
-						return true;
-					}
-					else
-					{
-						printf("connect %s : %d finished(failed). errno = %d\n",m_server_ip,m_server_port,errno);
-						// printf("FD_ISSET(sock_fd, &rfds): %d\n FD_ISSET(sock_fd, &wfds): %d\n", FD_ISSET(sock_fd, &rfds) , FD_ISSET(sock_fd, &wfds));
-						return false;
-					}
-				}
-				else
-				{
-					printf("connect %s : %d finished(failed).",m_server_ip,m_server_port);
-					return false;
-				}
-		}
-
-	}
-	else  //连接正常
-	{
-		printf("connect %s : %d finished(success).\n",m_server_ip,m_server_port);
-		set_block(true);   //成功之后重新把sock改成阻塞模式，以便后面发送/接收数据
-		return true;
-	}
-}
-
-
-Communication_socket Communication_socket::Accept()                   //返回Communication_socket对象，接收连接
-{
-	Communication_socket tcp;     //先定义一个Communication_socket对象，一会返回它
-
-	sockaddr_in caddr;
-	socklen_t len = sizeof(caddr);
-
-	tcp.m_socket = accept(m_socket, (sockaddr*)&caddr, &len);  //（阻塞）接收连接  ，会创建一个新的socket，一般扔到一个单独线程与这个客户端进行单独通信，之前的sock只用来建立连接
-	if (tcp.m_socket <= 0)
-		return tcp;   //出错
-	printf("accept client socket %d\n", tcp.m_socket);
-	char *ip = inet_ntoa(caddr.sin_addr);				 //解析出IP地址  ，转换到字符串
-
-	strcpy(tcp.m_client_ip, ip);
-	tcp.m_client_port = ntohs(caddr.sin_port);		 //解析出端口，转换成主机字节序
-	printf("client ip is %s,port is %d\n", tcp.m_client_ip, tcp.m_client_port);  //打印ip和端口
-	return tcp;
-
-}
-
-void Communication_socket::communication_socket_uninit()                    //关闭连接
-{
-	if (m_socket <= 0)
-	{
-		printf("socket %d error \n", m_socket);  //打印ip和端口
-		return;  //socket出错
-	}
-	close(m_socket);
-}
-
-int Communication_socket::communication_socket_recv(char *buf, int size)                      //接收数据
-{
-	return recv(m_socket, buf, size, 0);
-}
-
-int Communication_socket::communication_socket_send(const char *buf, int size)					     //发送数据
-{
-	int sendedSize = 0;   //已发送成功的长度
-
-	while (sendedSize != size)   //若没发送完成，则从断点开始继续发送 直到完成
-	{
-		try
-		{
-      if(is_connected())
-      {
-        int len = send(m_socket, buf + sendedSize, size - sendedSize, 0);
-        if (len <= 0)
-          break;
-        sendedSize += len;
-      }
-
-		}
-		catch(std::exception e)
-		{
-			std::cout << " send error 断线---" << std::endl;
-			break;
-		}
-
-	}
-	return sendedSize;
-}
-
-int Communication_socket::set_recv_timeout(int sec)   //设置tcp接收超时
-{
-	struct timeval tcp_rev_time;
-	tcp_rev_time.tv_sec = sec;
-	tcp_rev_time.tv_usec = 0;
-	if (setsockopt(m_socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&tcp_rev_time, sizeof(tcp_rev_time))<0)
-	{
-		printf("set tcp receive failed.\n");
-		return -1;
-	}
-	printf("set tcp recv timeout success. %d seconds.\n", sec);
-	return 0;
-}
-
-int Communication_socket::set_send_timeout(int sec)   //设置tcp发送超时
-{
-	struct timeval tcp_send_time;
-	tcp_send_time.tv_sec = sec;
-	tcp_send_time.tv_usec = 0;
-	if (setsockopt(m_socket, SOL_SOCKET, SO_SNDTIMEO, (char *)&tcp_send_time, sizeof(tcp_send_time))<0)
-	{
-		printf("set tcp send failed.\n");
-		return -1;
-	}
-	printf("set tcp recv timeout success. %d seconds.\n", sec);
-	return 0;
-}
-bool Communication_socket::is_connected()
-{
-	struct tcp_info info;
-	int len=sizeof(info);
-	getsockopt(m_socket, IPPROTO_TCP, TCP_INFO, &info, (socklen_t *)&len);
-	if((info.tcpi_state==TCP_ESTABLISHED))
-	{
-		return true;
-	}
-	return false;
-}
-

MPC/monitor/communication_socket.h

@@ -1,45 +0,0 @@
-//
-// Created by wk on 2022/12/6.
-//
-
-#ifndef PLC_SOCKET_TEST_COMMUNICATION_SOCKET_H
-#define PLC_SOCKET_TEST_COMMUNICATION_SOCKET_H
-#include <string>
-#include <iostream>
-#include <arpa/inet.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <pthread.h>
-#include<netinet/tcp.h>
-#include <vector>
-#include <thread>
-class Communication_socket
-{
-public:
-	Communication_socket();
-	~Communication_socket();
-
-	bool communication_socket_init();				 //创建套接字
-	bool communication_socket_bind(const char *ip,unsigned short port);  //绑定端口号
-	bool communication_socket_listen(unsigned short num = 10); //监听数量
-	bool set_block(bool isblock);  //设置阻塞模式  （希望只有在connect的时候是非阻塞的，而接收数据时候是阻塞的）
-	bool communication_socket_connect(const char *ip, unsigned short port, int sec = 5);
-	bool communication_socket_connect(int sec = 5);
-	Communication_socket Accept();                   //返回Communication_socket对象，接收连接
-	void communication_socket_uninit();							//关闭连接
-	int communication_socket_recv(char *buf, int size);          //接收数据
-	int communication_socket_send(const char *buf, int size);	//发送数据
-	bool is_connected();
-
-	int set_recv_timeout(int sec = 1);			 //设置udp接收超时
-	int set_send_timeout(int sec = 1);		     //设置udp发送超时
-private:
-	char m_client_ip[16];						//存放接收到的client ip
-	unsigned short m_client_port;				//存放接收到的client port
-
-	char m_server_ip[16];   			//tcp_serverip 服务端ip
-	int m_socket;						//tcp客户端的socket，init自动生成
-	unsigned short m_server_port;		//端口
-};
-
-#endif //PLC_SOCKET_TEST_COMMUNICATION_SOCKET_H

MPC/monitor/monitor.cpp

@@ -1,112 +0,0 @@
-//
-// Created by zx on 22-12-7.
-//
-
-#include "monitor.h"
-void Stringsplit(const std::string& str, const char split, std::vector<std::string>& res)
-{
-  if (str == "")        return;
-  //在字符串末尾也加入分隔符，方便截取最后一段
-  std::string strs = str + split;
-  size_t pos = strs.find(split);
-
-  // 若找不到内容则字符串搜索函数返回 npos
-  while (pos != strs.npos)
-  {
-    std::string temp = strs.substr(0, pos);
-    res.push_back(temp);
-    //去掉已分割的字符串,在剩下的字符串中进行分割
-    strs = strs.substr(pos + 1, strs.size());
-    pos = strs.find(split);
-  }
-}
-
-
-Monitor* Monitor::g_Monitor= nullptr;
-Monitor* Monitor::get_instance()
-{
-  if(g_Monitor== nullptr)
-  {
-    g_Monitor=new Monitor();
-  }
-  return g_Monitor;
-}
-
-Monitor::Monitor(){
-  set_read_callback(recv_callback,this);
-}
-Monitor::~Monitor()
-{
-
-}
-
-
-void Monitor::set_speed(double v,double a)
-{
-  v_=v;
-  a_=a;
-  char buf[255]={0};
-  sprintf(buf,"V:%f;A:%f",v,a);
-  write_data(buf);
-
-}
-
-void Monitor::set_statucallback(StatuCallback callback)
-{
-  callback_=callback;
-}
-
-void Monitor::recv_callback(char* data,void* p)
-{
-  Monitor* pMonitor= (Monitor*)p;
-  if(pMonitor== nullptr)
-    return;
-
-  std::string recv(data);
-  std::vector<std::string> splits;
-  Stringsplit(recv,';',splits);
-  if(splits.size()!=3)
-    return ;
-
-  std::string pose_str=splits[0];
-  std::string v_str=splits[1];
-  std::string a_str=splits[2];
-
-  std::vector<std::string> pose_split;
-  Stringsplit(pose_str,':',pose_split);
-  if(pose_split.size()==2)
-  {
-    std::string pose_value=pose_split[1];
-    std::vector<std::string> value_strs;
-    Stringsplit(pose_value,',',value_strs);
-    if(value_strs.size()==3)
-    {
-      float x=atof(value_strs[0].c_str());
-      float y=atof(value_strs[1].c_str());
-      float theta=atof(value_strs[2].c_str());
-      pMonitor->mtx_.lock();
-      pMonitor->pose_=Pose2d(x,y,theta);
-      pMonitor->mtx_.unlock();
-      //std::cout<<"pose:"<<pMonitor->pose_;
-    }
-  }
-
-  std::vector<std::string> v_split;
-  Stringsplit(v_str,':',v_split);
-  if(v_split.size()==2)
-  {
-    pMonitor->v_=atof(v_split[1].c_str());
-    //std::cout<<"; v:"<<pMonitor->v_;
-  }
-
-  std::vector<std::string> a_split;
-  Stringsplit(a_str,':',a_split);
-  if(a_split.size()==2)
-  {
-    pMonitor->a_=atof(a_split[1].c_str());
-    //std::cout<<"; a:"<<pMonitor->a_;
-  }
-  //std::cout<<std::endl;
-  if(pMonitor->callback_!= nullptr)
-    pMonitor->callback_(pMonitor->pose_,pMonitor->v_,pMonitor->a_);
-}

MPC/monitor/monitor.h

@@ -1,38 +0,0 @@
-//
-// Created by zx on 22-12-7.
-//
-
-#ifndef LIO_LIVOX_CPP_MPC_MONITOR_MONITOR_H_
-#define LIO_LIVOX_CPP_MPC_MONITOR_MONITOR_H_
-#include "Plc_socket.h"
-#include "../pose2d.h"
-#include <mutex>
-
-typedef void (*StatuCallback)(const Pose2d&,double,double);
-class Monitor:public Plc_socket
-{
- public:
-    static Monitor* get_instance();
-    ~Monitor();
-
-    void set_speed(double v,double a);
-
-    void set_statucallback(StatuCallback callback);
-
-    static void recv_callback(char* data,void* p);
-
- protected:
-    Monitor();
-    static Monitor* g_Monitor;
-    std::mutex mtx_;
-    Pose2d pose_;
-    double v_=0;
-    double a_=0;
-
-    StatuCallback callback_=nullptr;
-
-
-};
-
-
-#endif //LIO_LIVOX_CPP_MPC_MONITOR_MONITOR_H_

MPC/navigation.cpp

@@ -1,318 +0,0 @@
-//
-// Created by zx on 22-12-2.
-//
-
-#include "navigation.h"
-#include "loaded_mpc.h"
-
-Navigation* Navigation::navigation_=nullptr;
-Navigation* Navigation::GetInstance()
-{
-  if(navigation_== nullptr)
-  {
-    navigation_=new Navigation();
-  }
-  return navigation_;
-}
-Navigation::~Navigation()
-{
-
-}
-
-void Navigation::ResetStatu(double v,double a)
-{
-  timedV_.reset(v,0.05);
-  timedA_.reset(a,0.05);
-}
-
-void Navigation::ResetPose(const Pose2d& pose)
-{
-  timedPose_.reset(pose,0.1);
-}
-
-bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
-{
-  if(running_==true)
-  {
-    if(pause_)
-    {
-      pause_=false;
-      return true;
-    }
-    printf(" navigation is running pls cancel before\n");
-    return false;
-  }
-  if(thread_!= nullptr)
-  {
-    if(thread_->joinable())
-      thread_->join();
-    delete thread_;
-  }
-
-    //add 先检查当前点与起点的距离
-    global_trajectorys_=global_trajectorys;
-    thread_=new std::thread(&Navigation::navigatting,this);
-    return true;
-
-}
-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::moveToPoint(const Pose2d& pose,const Pose2d& distance)
-{
-  Pose2d thresh=Pose2d::abs(distance);
-  bool yCompleted=false;
-  bool xCompleted=false;
-  bool aCompleted=false;
-  while(cancel_==false)
-  {
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    if (pause_ == true)
-    {
-      //发送暂停消息
-      continue;
-    }
-    if (timedPose_.timeout() == true)
-    {
-      printf(" navigation Error:Pose is timeout \n");
-      break;
-    }
-
-    //计算目标pose在当前pose下的pose
-    Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
-    if (Pose2d::abs(diff).y() < thresh.y())
-      yCompleted=true;
-    else if(Pose2d::abs(diff).y()>2.0*thresh.y())
-      yCompleted=false;
-
-    if (Pose2d::abs(diff).x() < thresh.x())
-      xCompleted=true;
-    else if(Pose2d::abs(diff).x() >2.0* thresh.x())
-      xCompleted=false;
-
-    if (Pose2d::abs(diff).theta() < thresh.theta())
-      aCompleted=true;
-    else
-      aCompleted=false;
-
-    if(yCompleted&&xCompleted&&aCompleted)
-    {
-      Monitor::get_instance()->set_speed(0,0);
-      return true;
-    }
-
-    //先旋转使得Y=0摆直，再移动x，再X最后旋转
-    if(yCompleted==false)
-    {
-      double theta=atan(diff.y()/diff.x());
-
-      double maxtheta=20.*M_PI/180.0;
-      double mintheta=5.*M_PI/180.0;
-      if(abs(theta)<mintheta)
-      {
-        theta=(theta>0)?mintheta:-mintheta;
-      }
-      if(abs(theta)>maxtheta)
-      {
-        theta=(theta>0)?maxtheta:-maxtheta;
-      }
-      Monitor::get_instance()->set_speed(0,theta);
-      printf("旋转 diff.y:%.5f\n",diff.y());
-      continue;
-
-    }
-    if(xCompleted==false){
-      double v=diff.x()/10.0;
-      double maxv=1.9;
-      double minv=0.1;
-      if(abs(v)<minv)
-      {
-        v=(v>0)?minv:-minv;
-      }
-      if(abs(v)>maxv)
-      {
-        v=(v>0)?maxv:-maxv;
-      }
-      Monitor::get_instance()->set_speed(v,0);
-      printf("x移动 diff.x():%.5f\n",diff.x());
-      continue;
-    }
-    if(aCompleted==false){
-      double theta=diff.theta()/5.0;
-      double maxtheta=20.*M_PI/180.0;
-      double mintheta=5.*M_PI/180.0;
-      if(abs(theta)<mintheta)
-      {
-        theta=(theta>0)?mintheta:-mintheta;
-      }
-      if(abs(theta)>maxtheta)
-      {
-        theta=(theta>0)?maxtheta:-maxtheta;
-      }
-      Monitor::get_instance()->set_speed(0,theta);
-      printf("旋转 diff.theta():%.5f\n",diff.theta());
-    }
-
-  }
-  if(cancel_==true)
-    return false;
-  return true;
-}
-
-//待完成
-bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
-{
-  if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
-  {
-    printf(" MPC once Error:Pose/V/A is timeout \n");
-    return false;
-  }
-  if(traj.size()==0)
-  {
-    return false;
-  }
-
-  Pose2d pose=timedPose_.Get();
-  double velocity=timedV_.Get();    //从plc获取状态
-  double angular=timedA_.Get();
-
-  LoadedMPC MPC(2.7);
-  Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
-
-  statu[0]=pose.x();
-  statu[1]=pose.y();
-  statu[2]=pose.theta();
-  statu[3]=velocity;
-  statu[4]=angular;
-
-  Trajectory optimize_trajectory;
-  Trajectory selected_trajectory;
-  bool ret= MPC.solve(traj,traj[traj.size()-1],statu,out,selected_trajectory,optimize_trajectory);
-  predict_traj_.reset(optimize_trajectory,1);
-  selected_traj_.reset(selected_trajectory,1);
-  return ret;
-}
-
-Trajectory Navigation::selected_trajectory()
-{
-  return selected_traj_.Get();
-}
-Trajectory Navigation::predict_trajectory()
-{
-  return predict_traj_.Get();
-}
-
-bool Navigation::execute_edge(const Trajectory& child)
-{
-  if(child.size()==0)
-    return true;
-  Pose2d head=child[0];
-  Pose2d tail=child[child.size()-1];
-
-  Pose2d normal_diff(0.05,0.05,0.5*M_PI/180.0);
-  Pose2d mpc_diff(0.03,0.03,0.5*M_PI/180.0);
-
-  Trajectory traj=child;
-  bool init_start_pose=false;
-  while(cancel_==false)
-  {
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    if(pause_==true)
-    {
-      //发送暂停消息
-      continue;
-    }
-
-    if(timedPose_.timeout()==true)
-    {
-      printf(" navigation Error:Pose is timeout \n");
-      break;
-    }
-
-    if(Pose2d::abs(timedPose_.Get()-tail)<mpc_diff)
-    {
-      printf(" edge navigation completed!!!\n");
-      Monitor::get_instance()->set_speed(0,0);
-      return true;
-    }
-
-    //如果与起点差距过大先运动到起点
-    if(false==init_start_pose)
-    {
-      init_start_pose= moveToPoint(head, normal_diff);
-      continue;
-    }
-    else{
-      //一次变速
-      std::vector<double> out;
-      if(mpc_once(traj,out)==false)
-      {
-        Monitor::get_instance()->set_speed(0,0);
-        return false;
-      }
-      Monitor::get_instance()->set_speed(out[0],out[1]);
-    }
-
-  }
-  return false;
-}
-
-void Navigation::navigatting()
-{
-  pause_=false;
-  cancel_=false;
-  running_=true;
-  printf(" navigation beg...\n");
-
-#if 0
-  Trajectory gtraj;
-  while(global_trajectorys_.empty()==false)
-  {
-    gtraj=gtraj+global_trajectorys_.front();
-    global_trajectorys_.pop();
-  }
-  execute_edge(gtraj);
-#else
-  while(global_trajectorys_.empty()==false)
-  {
-    Trajectory traj=global_trajectorys_.front();
-    if(execute_edge(traj))
-    {
-      global_trajectorys_.pop();
-    }
-    else
-    {
-      break;
-    }
-  }
-#endif
-  Monitor::get_instance()->set_speed(0,0);
-  if(cancel_==true)
-    printf(" navigation sia canceled\n");
-  printf(" navigation end...\n");
-  if(global_trajectorys_.size()==0)
-    printf("navigation completed!!!\n");
-  running_=false;
-}

MPC/navigation.h

@@ -1,53 +0,0 @@
-//
-// 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 "monitor/monitor.h"
-#include <queue>
-#include <thread>
-class Navigation
-{
- public:
-    static Navigation* GetInstance();
-    ~Navigation();
-
-    void ResetPose(const Pose2d& pose);
-    void ResetStatu(double v,double a);
-    bool Start(std::queue<Trajectory> global_trajectorys);
-    void Cancel();
-    void Pause();
-
-    Trajectory selected_trajectory();
-    Trajectory predict_trajectory();
-
- protected:
-    Navigation();
-    bool execute_edge(const Trajectory& child);
-    bool moveToPoint(const Pose2d& pose,const Pose2d& distance);
-    bool mpc_once(const Trajectory& traj,std::vector<double>& out);
-    void navigatting();
-
- protected:
-    static Navigation* navigation_;
-    std::mutex  mtx_;
-    TimedLockData<Pose2d> timedPose_;           //当前位姿
-    TimedLockData<double> timedV_;              //底盘数据
-    TimedLockData<double> timedA_;
-    std::queue<Trajectory> global_trajectorys_; //多段轨迹
-    std::thread* thread_= nullptr;
-    bool running_=false;
-    bool pause_=false;
-    bool cancel_=false;
-
-    TimedLockData<Trajectory> selected_traj_;
-    TimedLockData<Trajectory> predict_traj_;
-};
-
-
-#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_

MPC/pose2d.cpp

@@ -1,56 +0,0 @@
-//
-// Created by zx on 22-12-1.
-//
-//
-// Created by zx on 2020/9/9.
-//
-
-#include "pose2d.h"
-#include <math.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;
-  return Pose2d(x,y,new_theta);
-}

MPC/pose2d.h

@@ -1,82 +0,0 @@
-//
-// 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>
-/*
- * 带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;
-    }
-    Pose2d operator-(const Pose2d& pose)const
-    {
-      float x=m_x-pose.x();
-      float y=m_y-pose.y();
-      float theta=m_theta-pose.theta();
-      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();
-      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_

MPC/trajectory.cpp

@@ -1,225 +0,0 @@
-//
-// 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 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,int point_count)
-{
-
-  Trajectory trajectory;
-  double angle =gen_axis_angle(start,end);
-  double dt=1.0/float(point_count);
-
-  //生成离散点,间距
-  Pose2d diff=end-start;
-  for(int i=0;i<point_count;++i)
-  {
-    double x=start.x()+i*dt*diff.x();
-    double y=start.y()+i*dt*diff.y();
-    trajectory.push_point(Pose2d(x,y,angle));
-  }
-  trajectory.push_point(Pose2d(end.x(),end.y(),angle));
-  return trajectory;
-}
-

MPC/trajectory.h

@@ -1,46 +0,0 @@
-//
-// 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,int point_count=100);
-
- 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_

dijkstra/dijkstra.cpp

@@ -1,168 +0,0 @@
-//
-// 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;
-  int weight;
-  int 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));
-    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

@@ -1,85 +0,0 @@
-//
-// 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
-
-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_

dijkstraSample.cpp

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

main.cpp

@@ -10,10 +10,8 @@
 
 #include "pangolinViewer.h"
 #include "segment/LidarFeatureExtractor.h"
-
-#include "dijkstra/dijkstra.h"
 #include "define/timedlockdata.hpp"
-#include "MPC/navigation.h"
+
 
 //雷达相关
 livox_ros::Lddc *lddc= nullptr;
@@ -28,20 +26,12 @@ 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;
-
-//控制相关
 
 TimedLockData<PointCloud> scan_cloud;
 TimedLockData<PointCloud::Ptr> localmap_cloud;
 TimedLockData<Eigen::Matrix4d> cur_pose;
 double final_cost=0;
 
-bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs);
 
 void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 {
@@ -55,12 +45,7 @@ void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 
 void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
 {
-  return ;
   cur_pose.reset(tranform);
-  Eigen::Matrix3d rotation=tranform.topLeftCorner(3,3);
-  Eigen::Vector3d eulerAngle=rotation.eulerAngles(2,1,0);
-  Pose2d pose(tranform(0,3),tranform(1,3),eulerAngle[0]);
-  Navigation::GetInstance()->ResetPose(pose);
 }
 
 void FinalCostCallback(double cost)
@@ -109,25 +94,9 @@ void ImuDataCallback(ImuData imu,int handle){
   pMaper->AddImu(imu);
 }
 
-void monitor_statu_callback(const Pose2d& pose,double v,double a)
-{
-  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(0,Eigen::Vector3d::UnitX()));
-  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(0,Eigen::Vector3d::UnitY()));
-  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(pose.theta(),Eigen::Vector3d::UnitZ()));
-  Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;
-  Eigen::Matrix4d transform;
-  transform.topLeftCorner(3,3)=rotation_matrix;
-  Eigen::Vector3d T(pose.x(),pose.y(),5);
-  transform.topRightCorner(3,1)=T;
-  cur_pose.reset(transform);
-
-  Navigation::GetInstance()->ResetPose(pose);
-  Navigation::GetInstance()->ResetStatu(v,a);
-}
 
 bool InitLidar(double freq);
 bool InitMap(std::string config_file);
-bool InitDijkstraMap();
 
 void pangolinSpinOnce(PangolinViewer* viewer)
 {
@@ -154,18 +123,6 @@ void pangolinSpinOnce(PangolinViewer* viewer)
   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);
-
-  Trajectory select_traj=Navigation::GetInstance()->selected_trajectory();
-  Trajectory predict_traj=Navigation::GetInstance()->predict_trajectory();
-  viewer->DrawTrajectory(select_traj,1,1,1,0.5);
-  viewer->DrawTrajectory(predict_traj,0,1,0,1.0);
 }
 void OnStartBtn(std::string lvxfile)
 {
@@ -187,37 +144,7 @@ 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;
-}
 
-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);
-  Navigation::GetInstance()->Start(global_trajectorys);
-}
-void navigation_stop()
-{
-  Navigation::GetInstance()->Cancel();
-}
 
 int main(int argc, char** argv)
 {
@@ -226,26 +153,18 @@ int main(int argc, char** argv)
   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;
 
-  Monitor::get_instance()->set_statucallback(monitor_statu_callback);
-  Monitor::get_instance()->plc_socket_init("127.0.0.1",30001);
+
 
   PangolinViewer* viewer=PangolinViewer::CreateViewer();
-  viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
-  viewer->SetNavigationCallbacks(navigation_start,navigation_stop);
+  viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged);
   viewer->Join();
 
-  Monitor::get_instance()->plc_socket_uninit();
   TimerRecord::PrintAll();
   lddc->Stop();
   pMaper->completed();
@@ -349,27 +268,6 @@ bool InitMap(std::string config_file)
   return true;
 }
 
-bool InitDijkstraMap()
-{
-  DijkstraMap.AddVertex(1, 0, 0);
-  DijkstraMap.AddVertex(2, 19.2, 0);
-  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;
@@ -393,25 +291,3 @@ bool InitLidar(double publish_freq)
   lddc->SetImuCallback(ImuDataCallback);
   return true;
 }
-
-bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
-{
-  if(nodes_id.size()<2)
-    return false;
-  for(int i=0;i<nodes_id.size()-1;++i)
-  {
-    float x1,y1,x2,y2;
-    map.GetVertexPos(nodes_id[i],x1,y1);
-    map.GetVertexPos(nodes_id[i+1],x2,y2);
-
-    Pose2d start(x1,y1,0);
-    Pose2d end(x2,y2,0);
-    Trajectory traj=Trajectory::create_line_trajectory(start,end);
-    if(traj.size()>1)
-      trajs.push(traj);
-    else
-      return false;
-
-  }
-  return true;
-}

pangolinViewer.cpp

@@ -3,7 +3,6 @@
 //
 
 #include "pangolinViewer.h"
-#include "MPC/navigation.h"
 PangolinViewer* PangolinViewer::viewer_= nullptr;
 
 PangolinViewer* PangolinViewer::CreateViewer()
@@ -40,21 +39,14 @@ void PangolinViewer::Join()
   }
 }
 
-void PangolinViewer::SetNavigationCallbacks(StartBtnCallback startBtn,
-                            StopBtnCallback stopBtn)
-{
-  naviagtion_startBtn_callback_=startBtn;
-  navigation_stopBtn_callback_=stopBtn;
-}
 
 void PangolinViewer::SetCallbacks(ViewerSpinOnceCallback spinOnce,StartLocateBtnCallback startBtn,
-                  StopBtnCallback stopBtn,FreqChangedCallback freqChanged,DijkstraBtnCallback dijkstra)
+                  StopBtnCallback stopBtn,FreqChangedCallback freqChanged)
 {
   spinOnce_callback_=spinOnce;
   startBtn_callback_=startBtn;
   stopBtn_callback_=stopBtn;
   freqChanged_callback_=freqChanged;
-  dijkstraBtn_callback_=dijkstra;
 }
 
 void PangolinViewer::Init()
@@ -89,63 +81,6 @@ void PangolinViewer::Init()
   rpy_string_=new pangolin::Var<std::string>("ui.rpy_String", "rpy");
   cost_string_=new pangolin::Var<std::string>("ui.final_cost", "cost");
 
-  beg_Int_=new pangolin::Var<int>("ui.beg", 1, 1, 7);
-  end_Int_=new pangolin::Var<int>("ui.end", 5, 1, 7);
-  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):%.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;
-      }
-
-    }
-
-  });
-
-  pangolin::Var<std::function<void(void)>> navigation_start("ui.navigation_start", [&]()
-  {
-    if(naviagtion_startBtn_callback_!=nullptr)
-      naviagtion_startBtn_callback_();
-  });
-
-
-  pangolin::Var<std::function<void(void)>> navigation_end("ui.navigation_cancel", [&]()
-  {
-    if(navigation_stopBtn_callback_!= nullptr)
-      navigation_stopBtn_callback_();
-
-  });
-
 }
 
 void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
@@ -163,88 +98,6 @@ void PangolinViewer::DrawCloud(PointCloud::Ptr cloud,double r,double g,double b,
   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() , 5);
-
-  }
-  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=5;
-
-  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 PangolinViewer::ShowRealtimePose(Eigen::Matrix4d pose)
 {
   double l=2;

pangolinViewer.h

@@ -16,9 +16,8 @@
 #include <pangolin/handler/handler.h>
 #include "segment/LidarFeatureExtractor.h"
 #include <thread>
-#include "dijkstra/dijkstra.h"
 #include "define/typedef.h"
-#include "MPC/navigation.h"
+
 
 class PangolinViewer
 {
@@ -28,22 +27,17 @@ class PangolinViewer
     typedef void(*StartBtnCallback)();
     typedef void(*StopBtnCallback)();
     typedef void(*FreqChangedCallback)(int);
-    typedef bool(*DijkstraBtnCallback)(int ,int ,float& ,std::vector<int>&);
  public:
     static PangolinViewer* CreateViewer();
 
     void SetCallbacks(ViewerSpinOnceCallback spinOnce,StartLocateBtnCallback startBtn,
-     StopBtnCallback stopBtn,FreqChangedCallback freqChanged,DijkstraBtnCallback dijkstra);
+     StopBtnCallback stopBtn,FreqChangedCallback freqChanged);
 
-    void SetNavigationCallbacks(StartBtnCallback startBtn,
-                      StopBtnCallback stopBtn);
     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 DrawTrajectory(const Trajectory& traj,double r,double g,double b,double ptsize);
     void Join();
     ~PangolinViewer();
 
@@ -74,21 +68,12 @@ class PangolinViewer
     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;
     StartLocateBtnCallback startBtn_callback_=nullptr;
     StopBtnCallback stopBtn_callback_= nullptr;
     FreqChangedCallback freqChanged_callback_= nullptr;
-    DijkstraBtnCallback dijkstraBtn_callback_= nullptr;
 
-    StartBtnCallback naviagtion_startBtn_callback_=nullptr;
-    StopBtnCallback navigation_stopBtn_callback_= nullptr;
 
 };