新增monitor，mpc，实现多段边导航

CMakeLists.txt

@@ -91,13 +91,6 @@ add_executable(${PROJECT_NAME}_node
 		lio/src/lio/ceresfunc.cpp
 		lio/src/lio/ndtMap.cpp
 
-		pangolinViewer.cpp
-		dijkstra/dijkstra.cpp
-		MPC/loaded_mpc.cpp
-		MPC/trajectory.cpp
-		MPC/pose2d.cpp
-		MPC/navigation.cpp
-
 		lio/src/utils/TimerRecord.cpp
 
 		livox/livox_driver/lvx_file.cpp
@@ -113,6 +106,16 @@ add_executable(${PROJECT_NAME}_node
 		livox/common/comm/sdk_protocol.cpp
 		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
 
     )
 

MPC/loaded_mpc.cpp

@@ -6,7 +6,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 
-size_t N = 10;                  //优化考虑后面多少步
+size_t N = 20;                  //优化考虑后面多少步
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -112,8 +112,9 @@ LoadedMPC::LoadedMPC(){}
 LoadedMPC::~LoadedMPC(){}
 
 bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
-                                         std::vector<double>& out,Trajectory& optimize_trajectory)
+                                         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];
@@ -124,8 +125,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     printf( "filte path failed ...\n");
     return false;
   }
-
-  Trajectory filte_trajectory = Trajectory(filte_poses);
+  select_traj= Trajectory(filte_poses);
 
   //将选中点移动到小车坐标系
   std::vector<Pose2d> transform_poses;
@@ -157,12 +157,12 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     ref_velocity = -ref_velocity;
 
 
+  double dt = 0.1;
   double min_velocity = 0.1;
   double max_velocity = 1.9;
   double max_angular_velocity=5*M_PI/180.0;
-  double max_acc_line_velocity=0.5;
-  double max_acc_angular_velocity=2*M_PI/180.0;
-  double dt = 0.1;
+  double max_acc_line_velocity=1.9/(N*dt);
+  double max_acc_angular_velocity=5*M_PI/180.0;
 
   size_t n_vars = N * 5;
   size_t n_constraints = N * 5;
@@ -185,8 +185,8 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   //// limit  v
   for (int i = nv; i < nv + N; i++)
   {
-    vars_lowerbound[i] = ref_velocity>0?0:-max_velocity;
-    vars_upperbound[i] = ref_velocity>0?max_velocity:-0;
+    vars_lowerbound[i] = -max_velocity;
+    vars_upperbound[i] = max_velocity;
   }
 
   ////limint vth
@@ -239,7 +239,11 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
           options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
           constraints_upperbound, fg_eval, solution);
 
-  // Check some of the solution values
+  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)
   {
@@ -254,16 +258,16 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   out.clear();
 
   double solve_velocity=solution.x[nv];
-  if(fabs(solve_velocity)<min_velocity)
+  /*if(fabs(solve_velocity)<min_velocity)
   {
     solve_velocity= solve_velocity>0?min_velocity:-min_velocity;
-  }
+  }*/
 
   out.push_back(solve_velocity);
   out.push_back(solution.x[nvth]);
 
-  printf(" input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f\n",line_velocity,
-         angular_velocity,solve_velocity,solution.x[nvth],ref_velocity);
+  printf(" input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",line_velocity,
+         angular_velocity,solve_velocity,solution.x[nvth],ref_velocity,time);
 
   optimize_trajectory.clear();
   for (int i = 0; i < N; ++i)

MPC/loaded_mpc.h

@@ -14,7 +14,7 @@ class LoadedMPC
     LoadedMPC();
     ~LoadedMPC();
     virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
-                       std::vector<double>& out,Trajectory& optimize_trajectory);
+                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
 
  protected:
     /*

MPC/monitor/Plc_socket.cpp

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

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

@@ -0,0 +1,330 @@
+#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

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

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

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

@@ -19,6 +19,12 @@ 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);
@@ -77,12 +83,15 @@ 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)
     {
       //发送暂停消息
-      std::this_thread::sleep_for(std::chrono::milliseconds(100));
       continue;
     }
     if (timedPose_.timeout() == true)
@@ -93,23 +102,79 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
 
     //计算目标pose在当前pose下的pose
     Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
-    if (Pose2d::abs(diff) < thresh)
+    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)
     {
-      printf(" Move to pt complete\n");
-      break;
+      Monitor::get_instance()->set_speed(0,0);
+      return true;
     }
+
     //先旋转使得Y=0摆直，再移动x，再X最后旋转
-    if(Pose2d::abs(diff).y()>thresh.y())
+    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;
 
-    }else if(Pose2d::abs(diff).x()>thresh.x()){
-      printf("y移动 diff.x():%.5f\n",diff.x());
     }
-    else if(Pose2d::abs(diff).theta()>thresh.theta()){
+    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());
     }
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
   }
   if(cancel_==true)
     return false;
@@ -117,11 +182,11 @@ bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
 }
 
 //待完成
-bool Navigation::mpc_once(const Trajectory& traj)
+bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
 {
-  if(timedPose_.timeout()==true)
+  if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
   {
-    printf(" MPC once Error:Pose is timeout \n");
+    printf(" MPC once Error:Pose/V/A is timeout \n");
     return false;
   }
   if(traj.size()==0)
@@ -130,27 +195,47 @@ bool Navigation::mpc_once(const Trajectory& traj)
   }
 
   Pose2d pose=timedPose_.Get();
-  double velocity=0;    //从plc获取状态
-  double angular=0;
+  double velocity=timedV_.Get();    //从plc获取状态
+  double angular=timedA_.Get();
+
   LoadedMPC MPC;
-  Eigen::VectorXd statu;//agv状态
-  statu<<pose.x(),pose.y(),pose.theta(),velocity,angular;
+  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;
 
-  std::vector<double> out;
   Trajectory optimize_trajectory;
-  return MPC.solve(traj,traj[traj.size()-1],statu,out,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_child_trajectory(const Trajectory& child)
+bool Navigation::execute_edge(const Trajectory& child)
 {
   if(child.size()==0)
     return true;
-  Pose2d head,tail;
+  Pose2d head=child[0];
+  Pose2d tail=child[child.size()-1];
 
-  Pose2d normal_diff(0.2,0.2,10.*M_PI/180.0);
-  Pose2d mpc_diff(0.2,0.2,0.5*M_PI/180.0);
+  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));
@@ -168,26 +253,26 @@ bool Navigation::execute_child_trajectory(const Trajectory& child)
 
     if(Pose2d::abs(timedPose_.Get()-tail)<mpc_diff)
     {
-      printf(" navigation completed!!!\n");
+      printf(" edge navigation completed!!!\n");
+      Monitor::get_instance()->set_speed(0,0);
       return true;
     }
 
-    head=child[0];
-    tail=child[child.size()-1];
     //如果与起点差距过大先运动到起点
-    if(false==Pose2d::abs(timedPose_.Get()-traj[0])<normal_diff)
+    if(false==init_start_pose)
     {
-      if (false == moveToPoint(head, normal_diff))
-        return false;
-      else
-        continue;
+      init_start_pose= moveToPoint(head, normal_diff);
+      continue;
     }
     else{
       //一次变速
-      if(mpc_once(traj)==false)
+      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]);
     }
 
   }
@@ -200,10 +285,20 @@ void Navigation::navigatting()
   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_child_trajectory(traj))
+    if(execute_edge(traj))
     {
       global_trajectorys_.pop();
     }
@@ -212,6 +307,8 @@ void Navigation::navigatting()
       break;
     }
   }
+#endif
+  Monitor::get_instance()->set_speed(0,0);
   if(cancel_==true)
     printf(" navigation sia canceled\n");
   printf(" navigation end...\n");

MPC/navigation.h

@@ -8,6 +8,7 @@
 #include "../define/timedlockdata.hpp"
 #include "pose2d.h"
 #include "trajectory.h"
+#include "monitor/monitor.h"
 #include <queue>
 #include <thread>
 class Navigation
@@ -17,26 +18,35 @@ class Navigation
     ~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_child_trajectory(const Trajectory& child);
+    bool execute_edge(const Trajectory& child);
     bool moveToPoint(const Pose2d& pose,const Pose2d& distance);
-    bool mpc_once(const Trajectory& traj);
+    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_;
 };
 
 

MPC/trajectory.cpp

@@ -20,7 +20,7 @@ Pose2d Trajectory::operator[](int index) const
 }
 Trajectory& Trajectory::operator+(const Trajectory& other)
 {
-  m_pose_vector.insert(m_pose_vector.begin(),
+  m_pose_vector.insert(m_pose_vector.end(),
                        other.m_pose_vector.begin(),other.m_pose_vector.end());
   return *this;
 }

config/horizon_config.yaml

@@ -19,3 +19,4 @@ 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"

main.cpp

@@ -35,7 +35,6 @@ std::vector<int> shortest_path;
 PathMap DijkstraMap;
 
 //控制相关
-Navigation* navigator=nullptr;
 
 TimedLockData<PointCloud> scan_cloud;
 TimedLockData<PointCloud::Ptr> localmap_cloud;
@@ -56,9 +55,11 @@ void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
 
 void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
 {
+  return ;
   cur_pose.reset(tranform);
-  //Eigen::Vector3d eulerAngle=tranform.topLeftCorner(3,3).eulerAngles(2,1,0);
-  Pose2d pose(tranform(0,3),tranform(1,3),0);
+  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);
 }
 
@@ -108,6 +109,22 @@ 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();
@@ -144,6 +161,11 @@ void pangolinSpinOnce(PangolinViewer* viewer)
   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)
 {
@@ -215,11 +237,15 @@ int main(int argc, char** argv)
   if(!InitLidar(publish_freq))
     return -3;
 
+  Monitor::get_instance()->set_statucallback(monitor_statu_callback);
+  Monitor::get_instance()->plc_socket_init("192.168.2.180",30001);
+
   PangolinViewer* viewer=PangolinViewer::CreateViewer();
   viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
   viewer->SetNavigationCallbacks(navigation_start,navigation_stop);
   viewer->Join();
 
+  Monitor::get_instance()->plc_socket_uninit();
   TimerRecord::PrintAll();
   lddc->Stop();
   pMaper->completed();
@@ -255,6 +281,7 @@ bool InitMap(std::string config_file)
   int ivox_nearby_type = static_cast<int>(fsSettings["ivox_nearby_type"]);
   int max_ivox_node = static_cast<int>(fsSettings["max_ivox_node"]);
   float ivox_resolution = static_cast<float>(fsSettings["ivox_node_solution"]);
+  std::string map_file=static_cast<string>(fsSettings["map_file"]);
 
   laserCloud.reset(new pcl::PointCloud<PointType>);
 
@@ -269,7 +296,6 @@ bool InitMap(std::string config_file)
                                                     KdTreeCornerOutlierDis);
 
   //map 参数
-  std::string map_dir="../map/";
   float filter_parameter_corner = 0.2;
   float filter_parameter_surf = 0.4;
   int IMU_Mode = 2;
@@ -303,10 +329,11 @@ bool InitMap(std::string config_file)
   pMaper->SetCloudMappedCallback(CloudFullCallback);
   pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
   pMaper->SetFinalCostCallback(FinalCostCallback);
-  pMaper->LoadMap(map_dir + "/featurebbt_transformed.txt");
+  printf("slam map file：%s\n",map_file.c_str());
+  pMaper->LoadMap(map_file);
 
   double RPI = M_PI / 180.0;
-  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 11.5 * RPI);
+  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 11.5* RPI);
   Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
   Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
   Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));

pangolinViewer.cpp

@@ -78,7 +78,7 @@ void PangolinViewer::Init()
           .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
 
   lvx_checkbox_=new pangolin::Var<bool>("ui.lvx_Checkbox", true, true);
-  lvx_file_=new pangolin::Var<std::string> ("ui.lvx_file_String", "../map/lidarbag/start1.lvx");
+  lvx_file_=new pangolin::Var<std::string> ("ui.lvx_file_String", "../map/bbt/start1.lvx");
 
   start_button_=new pangolin::Var<bool>("ui.start_Button", false, false);
   stop_button_ =new pangolin::Var<bool>("ui.stop_Button", false, false);
@@ -163,12 +163,27 @@ 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=3.6;
+  float z=5;
 
   glPointSize(20.0);
 // 开始画点

pangolinViewer.h

@@ -18,7 +18,7 @@
 #include <thread>
 #include "dijkstra/dijkstra.h"
 #include "define/typedef.h"
-
+#include "MPC/navigation.h"
 
 class PangolinViewer
 {
@@ -43,6 +43,7 @@ class PangolinViewer
     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();