20210926, msg and tool alignment.

add outofrange detection, plate center finder,
fix lidar data in queue mem leak,
fix lidar crash in decontruction.

communication/communication_message.h

@@ -48,7 +48,8 @@ public:
 		eParkspace_force_update_response_msg = 0x39,//手动修改车位反馈消息
 		eParkspace_confirm_alloc_request_msg = 0x3A,//确认分配车位请求消息
 		eParkspace_confirm_alloc_response_msg = 0x3B,//确认分配车位反馈消息
-
+        eParkspace_refresh_request_msg=0x3C,        //请求更新车位数据
+        eParkspace_allocation_data_response_msg=0x3D,
 
 		eStore_command_request_msg=0x41,        //终端停车请求消息
 		eStore_command_response_msg=0x42,       //停车请求反馈消息
@@ -67,9 +68,18 @@ public:
 		eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
 		eProcess_manual_operation_msg=0xb1,             //针对流程的手动消息
 
-
+		eDispatch_plan_request_msg          = 0xe0,     //调度总规划的请求(用于启动整个调度算法)(调度管理->调度算法)
+		eDispatch_plan_response_msg         = 0xe1,     //调度总规划的答复(调度算法->调度管理)
+		eDispatch_control_request_msg       = 0xe2,     //调度控制的任务请求(调度算法->调度管理)
+		eDispatch_control_response_msg      = 0xe3,     //调度控制的任务答复(调度管理->调度算法)
+		eDispatch_manager_status_msg        = 0xea,     //调度管理的设备状态消息(调度底下所有硬件设备状态的汇总)
+		eDispatch_manager_data_msg          = 0xeb,     //调度管理的设备详细的数据信息
+		
+		
 		eGround_detect_request_msg=0xf0,        //地面雷达测量请求消息
     	eGround_detect_response_msg=0xf1,       //地面雷达测量反馈消息
+		eGround_status_msg=0xf2,                //地面雷达状态消息
+
 
 	};
 
@@ -86,8 +96,11 @@ public:
 		eMeasurer=0x0300,
 		//测量单元
 		eMeasurer_sift_server=0x0301,
+
+		//调度机构
+		eDispatch_mamager=0x0400,
 		//调度机构
-		eDispatch=0x0400,
+		eDispatch_control=0x0401,
 		//...
 
 		//地面测量单元

communication/communication_message.h.orig

@@ -0,0 +1,128 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#ifndef NNXX_TESTS_COMMUNICATION_MESSAGE_H
+#define NNXX_TESTS_COMMUNICATION_MESSAGE_H
+
+#include "../error_code/error_code.h"
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+//#include <iosfwd>
+
+#include <string>
+#include "../message/message_base.pb.h"
+
+
+class Communication_message
+{
+public:
+	//消息类型定义,每个在网络上传输的消息必须含有这个属性
+	enum Message_type
+	{
+		eBase_msg=0x00,
+		eCommand_msg=0x01,                      //指令消息
+
+
+		eLocate_status_msg=0x11,                //定位模块状态消息
+		eLocate_request_msg=0x12,               //定位请求消息
+		eLocate_response_msg=0x13,              //定位反馈消息
+
+		eLocate_sift_request_msg = 0x14,            //预测算法请求消息
+		eLocate_sift_response_msg = 0x15,           //预测算法反馈消息
+
+		eDispatch_status_msg=0x21,                //调度模块硬件状态消息
+		eDispatch_request_msg=0x22,              //请求调度消息
+		eDispatch_response_msg=0x23,             //调度结果反馈消息
+
+		eParkspace_allocation_status_msg=0x31,  //车位分配模块状态消息，包括车位信息
+		eParkspace_allocation_request_msg=0x32, //请求分配车位消息
+		eParkspace_allocation_response_msg=0x33,//分配车位结果反馈消息
+		eParkspace_search_request_msg = 0x34,		//查询车位请求消息
+		eParkspace_search_response_msg = 0x35,		//查询车位反馈消息
+		eParkspace_release_request_msg = 0x36,		//释放车位请求消息
+		eParkspace_release_response_msg = 0x37,		//释放车位反馈消息
+		eParkspace_force_update_request_msg = 0x38,	//手动修改车位消息
+		eParkspace_force_update_response_msg = 0x39,//手动修改车位反馈消息
+		eParkspace_confirm_alloc_request_msg = 0x3A,//确认分配车位请求消息
+		eParkspace_confirm_alloc_response_msg = 0x3B,//确认分配车位反馈消息
+
+
+		eStore_command_request_msg=0x41,        //终端停车请求消息
+		eStore_command_response_msg=0x42,       //停车请求反馈消息
+		ePickup_command_request_msg=0x43,       //取车请求消息
+		ePickup_command_response_msg=0x44,       //取车请求反馈消息
+
+
+
+		eStoring_process_statu_msg=0x90,        //停车指令进度条消息
+		ePicking_process_statu_msg=0x91,        //取车指令进度消息
+
+
+		eCentral_controller_statu_msg=0xa0,     //中控系统状态消息
+
+
+		eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
+		eProcess_manual_operation_msg=0xb1,             //针对流程的手动消息
+
+
+		eGround_detect_request_msg=0xf0,        //地面雷达测量请求消息
+    	eGround_detect_response_msg=0xf1,       //地面雷达测量反馈消息
+
+	};
+
+//通讯单元
+	enum Communicator
+	{
+		eEmpty=0x0000,
+		eMain=0x0001,    //主流程
+
+		eTerminor=0x0100,
+		//车位表
+		eParkspace=0x0200,
+		//测量单元
+		eMeasurer=0x0300,
+		//测量单元
+		eMeasurer_sift_server=0x0301,
+		//调度机构
+		eDispatch=0x0400,
+		//...
+
+		//地面测量单元
+		eGround_measurer=0x0f00,
+	};
+public:
+	Communication_message();
+	Communication_message(std::string message_buf);
+	Communication_message(char* p_buf, int size);
+	Communication_message(const Communication_message& other)= default;
+	Communication_message& operator =(const Communication_message& other)= default;
+	~Communication_message();
+public://API functions
+	bool is_over_time();
+public://get or set member variable
+	void reset(const message::Base_info& base_info, std::string receive_string);
+
+	Message_type get_message_type();
+	Communicator get_sender();
+	Communicator get_receiver();
+	std::string& get_message_buf();
+	std::chrono::system_clock::time_point get_receive_time();
+
+protected://member variable
+	Message_type								m_message_type;				//消息类型
+	std::chrono::system_clock::time_point		m_receive_time;				//接收消息的时间点
+	std::chrono::milliseconds					m_timeout_ms;				//超时时间, 整个软件都统一为毫秒
+	Communicator								m_sender;					//发送者
+	Communicator								m_receiver;					//接受者
+
+	std::string									m_message_buf;				//消息数据
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_COMMUNICATION_MESSAGE_H

communication/communication_socket_base.cpp

@@ -39,7 +39,6 @@ Error_manager Communication_socket_base::communication_init_from_protobuf(std::s
 		return Error_manager(COMMUNICATION_READ_PROTOBUF_ERROR,MINOR_ERROR,
 		"Communication_socket_base read_proto_param  failed");
 	}
-
 	return communication_init_from_protobuf(t_communication_parameter_all);
 }
 
@@ -57,8 +56,8 @@ Error_manager Communication_socket_base::communication_init_from_protobuf(Commun
 			return t_error;
 		}
 	}
-	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
-		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
+//	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
+//		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
 	for(int i=0;i<communication_parameter_all.communication_parameters().connect_string_vector_size();++i)
 	{
 		t_error = communication_connect( communication_parameter_all.communication_parameters().connect_string_vector(i) );
@@ -67,7 +66,6 @@ Error_manager Communication_socket_base::communication_init_from_protobuf(Commun
 			return t_error;
 		}
 	}
-
 	//启动通信, run thread
 	communication_run();
 

communication/communication_socket_base.cpp.orig

@@ -0,0 +1,610 @@
+
+
+
+#include "communication_socket_base.h"
+#include "../tool/proto_tool.h"
+
+Communication_socket_base::Communication_socket_base()
+{
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+
+	m_analysis_cycle_time = 1000;//默认1000ms,就自动解析(接受list)
+	m_encapsulate_cycle_time = 1000;//默认1000ms,就自动发送一次状态信息
+}
+
+Communication_socket_base::~Communication_socket_base()
+{
+	communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_socket_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+
+	return  communication_init_from_protobuf(COMMUNICATION_PARAMETER_PATH);
+}
+
+//初始化 通信 模块。从文件读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(std::string prototxt_path)
+{
+	Communication_proto::Communication_parameter_all t_communication_parameter_all;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_communication_parameter_all) )
+	{
+		return Error_manager(COMMUNICATION_READ_PROTOBUF_ERROR,MINOR_ERROR,
+		"Communication_socket_base read_proto_param  failed");
+	}
+
+	return communication_init_from_protobuf(t_communication_parameter_all);
+}
+
+//初始化 通信 模块。从protobuf读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init_from_protobuf() run--- "<< this;
+	Error_manager t_error;
+
+	if ( communication_parameter_all.communication_parameters().has_bind_string() )
+	{
+		t_error = communication_bind(communication_parameter_all.communication_parameters().bind_string());
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
+		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
+	for(int i=0;i<communication_parameter_all.communication_parameters().connect_string_vector_size();++i)
+	{
+		t_error = communication_connect( communication_parameter_all.communication_parameters().connect_string_vector(i) );
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化
+Error_manager Communication_socket_base::communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+	Error_manager t_error;
+
+	t_error = communication_bind(bind_string);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	t_error = communication_connect(connect_string_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+//bind
+Error_manager Communication_socket_base::communication_bind(std::string bind_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind(bind_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_bind() bind::  "<< bind_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::vector<std::string>& connect_string_vector)
+{
+	Error_manager t_error;
+	for (auto iter = connect_string_vector.begin(); iter != connect_string_vector.end(); ++iter)
+	{
+		t_error = communication_connect(*iter);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::string connect_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect(connect_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_connect() connect::  "<< connect_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//启动通信, run thread
+Error_manager Communication_socket_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+	m_receive_condition.reset(false, false, false);
+	mp_receive_data_thread = new std::thread(&Communication_socket_base::receive_data_thread, this);
+	//解析线程默认等待, 需要接受线程去唤醒, 超时1ms, 超时后主动遍历m_receive_data_list
+	m_analysis_data_condition.reset(false, false, false);
+	mp_analysis_data_thread = new std::thread(&Communication_socket_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_socket_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时1ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_socket_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_socket_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+	m_socket.close();
+
+	return Error_code::SUCCESS;
+}
+
+
+void Communication_socket_base::set_analysis_cycle_time(unsigned int analysis_cycle_time)
+{
+	m_analysis_cycle_time = analysis_cycle_time;
+}
+void Communication_socket_base::set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time)
+{
+	m_encapsulate_cycle_time = encapsulate_cycle_time;
+}
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_socket_base::receive_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::receive_data_thread start "<< this;
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (m_receive_condition.is_alive())
+	{
+		m_receive_condition.wait_for_ex(std::chrono::microseconds(1));
+		if ( m_receive_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			std::string t_receive_string;
+			{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+				std::unique_lock<std::mutex> lk(m_mutex);
+				//flags为1, 非阻塞接受消息, 如果接收到消息, 那么接受数据长度大于0
+				t_receive_string = m_socket.recv<std::string>(1);
+			}
+			if ( t_receive_string.size()>0 )
+			{
+				//如果这里接受到了消息, 在这提前解析消息最前面的Base_msg (消息公共内容), 用于后续的check
+				message::Base_msg t_base_msg;
+				if( t_base_msg.ParseFromString(t_receive_string) )
+				{
+					//第一次解析之后转化为, Communication_message, 自定义的通信消息格式
+					Communication_message  * tp_communication_message = new Communication_message;
+					tp_communication_message->reset(t_base_msg.base_info(), t_receive_string);
+					//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+					if ( check_msg(tp_communication_message) == SUCCESS )
+					{
+						bool is_push = m_receive_data_list.push(tp_communication_message);
+						//push成功之后, tp_communication_message内存的管理权限交给链表, 如果失败就要回收内存
+						if ( is_push )
+						{
+							//唤醒解析线程一次,
+							m_analysis_data_condition.notify_all(false, true);
+						}
+						else
+						{
+//						push失败, 就要回收内存
+							delete(tp_communication_message);
+							tp_communication_message = NULL;
+//					return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//										 " m_receive_data_list.push error ");
+						}
+					}
+					else
+					{
+						delete(tp_communication_message);
+						tp_communication_message = NULL;
+					}
+
+
+				}
+				//解析失败, 就当做什么也没发生, 认为接收消息无效,
+			}
+			//没有接受到消息, 返回空字符串
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::receive_data_thread end "<< this;
+	return;
+}
+
+//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+Error_manager Communication_socket_base::check_msg(Communication_message*  p_msg)
+{
+	//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 判断这条消息是不是给我的.
+	//子类重载时, 增加自己模块的判断逻辑, 以后再写.
+	if ( p_msg->get_message_type() == Communication_message::Message_type::eBase_msg
+		 && p_msg->get_receiver() == Communication_message::Communicator::eMain )
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		//无效的消息,
+		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+							 " INVALID_MESSAGE error ");	}
+}
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_socket_base::analysis_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread start "<< this;
+
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_analysis_data_condition.wait_for_millisecond(m_analysis_cycle_time);
+		if ( m_analysis_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			if ( t_pass_flag )
+			{
+				analysis_receive_list();
+			}
+				//如果解析线程超时通过, 那么就定时处理链表残留的消息,
+			else
+			{
+				analysis_receive_list();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread end "<< this;
+	return;
+}
+
+//循环接受链表, 解析消息,
+Error_manager Communication_socket_base::analysis_receive_list()
+{
+	Error_manager t_error;
+	if ( m_receive_data_list.m_termination_flag )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::analysis_receive_list error ");
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_receive_data_list.m_mutex);
+		for (auto iter = m_receive_data_list.m_data_list.begin(); iter != m_receive_data_list.m_data_list.end(); )
+		{
+			Communication_message* tp_msg = **iter;
+			if ( tp_msg == NULL )
+			{
+				iter = m_receive_data_list.m_data_list.erase(iter);
+				//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+			}
+			else
+			{
+				//检查消息是否可以被处理
+				t_error = check_executer(tp_msg);
+				if ( t_error == SUCCESS)
+				{
+					//处理消息
+					t_error = execute_msg(tp_msg);
+//				if ( t_error )
+//				{
+//					//执行结果不管
+//				}
+//				else
+//				{
+//					//执行结果不管
+//				}
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else if( t_error == COMMUNICATION_EXCUTER_IS_BUSY)
+				{
+					//处理器正忙, 那就不做处理, 直接处理下一个
+					//注:这条消息就被保留了下来, wait_for_millisecond 超时通过之后, 会循环检查残留的消息.
+					iter++;
+				}
+				else //if( t_error == COMMUNICATION_ANALYSIS_TIME_OUT )
+				{
+					//超时了就直接删除
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+
+					//注:消息删除之后, 不需要发送答复消息, 发送方也会有超时处理的,  只有 execute_msg 里面可以答复消息
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Communication_socket_base::check_executer(Communication_message*  p_msg)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+	//同时也要判断是否超时, 超时返回 COMMUNICATION_ANALYSIS_TIME_OUT
+	//如果处理器正在忙别的, 那么返回 COMMUNICATION_EXCUTER_IS_BUSY
+
+	if ( p_msg->is_over_time() )
+	{
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+		std::cout << "COMMUNICATION_ANALYSIS_TIME_OUT , " << std::endl;
+		return Error_code::COMMUNICATION_ANALYSIS_TIME_OUT;
+	}
+	else
+	{
+		bool executer_is_ready = false;
+		//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 找到处理模块的实例对象, 查询执行人是否可以处理这条消息
+		//这里子类重载时, 增加判断逻辑, 以后再写.
+		executer_is_ready = true;
+
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+
+		if ( executer_is_ready )
+		{
+			std::cout << "executer_is_ready , " << std::endl;
+			return Error_code::SUCCESS;
+		}
+		else
+		{
+			std::cout << "executer_is_busy , " << std::endl;
+			return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//处理消息
+Error_manager Communication_socket_base::execute_msg(Communication_message*  p_msg)
+{
+	//先将 p_msg 转化为 对应的格式, 使用对应模块的protobuf来二次解析
+	// 不能一直使用 Communication_message*  p_msg, 这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 execute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+	//子类重载, 需要完全重写, 以后再写.
+
+	//注注注注注意了, 本模块只是用来做通信,
+	//在做处理消息的时候, 可能会调用执行者的接口函数,
+	//这里不应该长时间阻塞或者处理复杂的逻辑,
+	//请执行者另开线程来处理任务.
+
+	std::cout << "Communication_socket_base::excute_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+	std::cout << "Communication_socket_base::excute_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_socket_base::send_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::send_data_thread start "<< this;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (m_send_data_condition.is_alive())
+	{
+		m_send_data_condition.wait();
+		if ( m_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			Communication_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = m_send_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+						std::unique_lock<std::mutex> lk(m_mutex);
+						m_socket.send(tp_msg->get_message_buf());
+					}
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			else
+			{
+				//没有取出, 那么应该就是 m_termination_flag 结束了
+//				return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//									 " Communication_socket_base::send_data_thread() error ");
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::send_data_thread end "<< this;
+	return;
+}
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_socket_base::encapsulate_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(m_encapsulate_cycle_time);
+
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_send_data()
+{
+//	char buf[256] = {0};
+//	static unsigned int t_heartbeat = 0;
+//	sprintf(buf, "Communication_socket_base, heartbeat = %d\0\0\0, test\0", t_heartbeat);
+//	t_heartbeat++;
+    return SUCCESS;
+	message::Base_msg t_base_msg;
+	t_base_msg.mutable_base_info()->set_msg_type(message::Message_type::eBase_msg);
+	t_base_msg.mutable_base_info()->set_timeout_ms(5000);
+	t_base_msg.mutable_base_info()->set_sender(message::Communicator::eMain);
+	t_base_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
+
+	Communication_message* tp_msg = new Communication_message(t_base_msg.SerializeAsString());
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(std::string message)
+{
+	Communication_message* tp_msg = new Communication_message(message);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(Communication_message* p_msg)
+{
+	Communication_message* tp_msg = new Communication_message(*p_msg);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}

communication/communication_socket_base.h

@@ -30,7 +30,7 @@
 #include "../communication/communication_message.h"
 
 #include "../message/message_base.pb.h"
-#include "../message/measure_message.pb.h"
+//#include "../message/measure_message.pb.h"
 
 
 

communication/communication_socket_base.h.orig

@@ -0,0 +1,166 @@
+
+
+/*
+ * communication_socket_base 通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ * 重载解析消息和封装消息,
+ *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
+ * 
+ * 
+ * */
+
+#ifndef __COMMUNICATION_SOCKET_BASE__HH__
+#define __COMMUNICATION_SOCKET_BASE__HH__
+
+#include <mutex>
+#include <thread>
+#include <nnxx/message>
+#include <nnxx/socket.h>
+#include <nnxx/bus.h>
+
+#include <glog/logging.h>
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_list.h"
+#include "../tool/thread_condition.h"
+
+#include "../communication/communication.pb.h"
+#include "../communication/communication_message.h"
+
+#include "../message/message_base.pb.h"
+#include "../message/measure_message.pb.h"
+
+
+
+#define COMMUNICATION_PARAMETER_PATH "../setting/communication.prototxt"
+
+class Communication_socket_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+public:
+	Communication_socket_base();
+	Communication_socket_base(const Communication_socket_base& other)= delete;
+	Communication_socket_base& operator =(const Communication_socket_base& other)= delete;
+	~Communication_socket_base();
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+	Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+	Error_manager communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all);
+
+	//初始化
+	virtual Error_manager communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector);
+	//bind
+	virtual Error_manager communication_bind(std::string bind_string);
+	//connect
+	virtual Error_manager communication_connect(std::vector<std::string>& connect_string_vector);
+	//connect
+	virtual Error_manager communication_connect(std::string connect_string);
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+	
+	
+public://get or set member variable
+	void set_analysis_cycle_time(unsigned int analysis_cycle_time);
+	void set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time);
+
+protected:
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	void receive_data_thread();
+
+	//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+	virtual Error_manager check_msg(Communication_message* p_msg);
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//遍历接受链表, 解析消息,
+	Error_manager analysis_receive_list();
+
+	//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+	virtual Error_manager check_executer(Communication_message* p_msg);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Communication_message* p_msg);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	void send_data_thread();
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+public:
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(std::string message);
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Communication_message* p_msg);
+
+protected://member variable
+
+	//通用的网络编程接口, 默认使用总线模式, (网状结构)
+	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS };
+	std::mutex 							m_mutex;				//m_socket的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Communication_message*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+	unsigned int 						m_analysis_cycle_time;			//自动解析的时间周期
+
+	//发送模块,
+	Thread_safe_list<Communication_message*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+	unsigned int 						m_encapsulate_cycle_time;		//自动封装的时间周期
+
+
+
+private:
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //__COMMUNICATION_SOCKET_BASE__HH__

message/measure_message.proto

@@ -150,6 +150,7 @@ message Ground_status_msg
     repeated Cloud_coordinate           cloud=9;     //点云坐标
 }
 
+
 //点云坐标
 message Cloud_coordinate
 {

message/message_base.proto

@@ -30,7 +30,8 @@ enum Message_type
     eParkspace_force_update_response_msg = 0x39;//手动修改车位反馈消息
     eParkspace_confirm_alloc_request_msg = 0x3A;//确认分配车位请求消息
     eParkspace_confirm_alloc_response_msg = 0x3B;//确认分配车位反馈消息
-
+    eParkspace_allocation_data_msg = 0x3C;     //车位分配模块车位数据消息
+    eParkspace_allocation_data_response_msg =0x3D;//车位数据反馈消息
 
     eStore_command_request_msg=0x41;        //终端停车请求消息
     eStore_command_response_msg=0x42;       //停车请求反馈消息
@@ -49,9 +50,18 @@ enum Message_type
     eEntrance_manual_operation_msg=0xb0;            //针对出入口状态操作的手动消息
     eProcess_manual_operation_msg=0xb1;             //针对流程的手动消息
 
+    eDispatch_plan_request_msg          = 0xe0;     //调度总规划的请求(用于启动整个调度算法)(调度管理->调度算法)
+    eDispatch_plan_response_msg         = 0xe1;     //调度总规划的答复(调度算法->调度管理)
+    eDispatch_control_request_msg       = 0xe2;     //调度控制的任务请求(调度算法->调度管理)
+    eDispatch_control_response_msg      = 0xe3;     //调度控制的任务答复(调度管理->调度算法)
+    eDispatch_manager_status_msg        = 0xea;     //调度管理的设备状态消息(调度底下所有硬件设备状态的汇总)
+    eDispatch_manager_data_msg          = 0xeb;     //调度管理的设备详细的数据信息
+
+
     eGround_detect_request_msg=0xf0;        //地面雷达测量请求消息
     eGround_detect_response_msg=0xf1;       //地面雷达测量反馈消息
     eGround_status_msg=0xf2;                //地面雷达状态消息
+
 }
 
 //通讯单元
@@ -65,22 +75,25 @@ enum Communicator
     eParkspace=0x0200;
     //测量单元
     eMeasurer=0x0300;
-    //测量单元
+    //测量单元的服务器
     eMeasurer_sift_server=0x0301;
     //调度机构
-    eDispatch=0x0400;
+    eDispatch_manager=0x0400;
+    //调度机构
+    eDispatch_control=0x0401;
     //...
 
+
     //地面测量单元
 	eGround_measurer=0x0f00;
 }
 ////base message 用于解析未知类型的消息
 message Base_info
 {
-    required Message_type               msg_type=1;
-    optional int32                      timeout_ms=2;
-    required Communicator               sender=3;                       //发送者
-    required Communicator               receiver=4;                     //接收者
+    required Message_type               msg_type=1[default = eBase_msg];
+    optional int32                      timeout_ms=2[default = 0];
+    required Communicator               sender=3[default = eEmpty];                       //发送者
+    required Communicator               receiver=4[default = eEmpty];                     //接收者
 }
 
 // 事件,停车或者取车
@@ -113,65 +126,113 @@ enum Error_level
 
 message Error_manager
 {
-    required int32                      error_code = 1;
-    optional Error_level                error_level = 2;
-    optional string                     error_description = 3;
+    required int32                      error_code = 1[default = 0];
+    optional Error_level                error_level = 2[default = NORMAL];
+    optional string                     error_description = 3[default = ""];
 }
 
 //测量结果结构体
 message Locate_information
 {
-    optional float locate_x = 1;				//整车的中心点x值; 四轮的中心
-    optional float locate_y = 2;				//整车的中心点y值; 四轮的中心
-    optional float locate_angle = 3;			//整车的旋转角; 四轮的旋转角
-    optional float locate_length = 4;		    //整车的长度; 用于规避碰撞
-    optional float locate_width = 5;			//整车的宽度; 用于规避碰撞
-    optional float locate_height = 6;		    //整车的高度; 用于规避碰撞
-    optional float locate_wheel_base = 7;	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
-    optional float locate_wheel_width = 8;	    //整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
-    optional bool locate_correct = 9;		    //整车的校准标记位
-
-    optional float locate_front_theta = 10;	    //整车的前轮的旋转角
+    optional float locate_x = 1[default = 0];				//整车的中心点x值; 四轮的中心
+    optional float locate_y = 2[default = 0];				//整车的中心点y值; 四轮的中心
+    optional float locate_angle = 3[default = 0];			//整车的旋转角; 四轮的旋转角
+    optional float locate_length = 4[default = 0];		    //整车的长度; 用于规避碰撞
+    optional float locate_width = 5[default = 0];			//整车的宽度; 用于规避碰撞
+    optional float locate_height = 6[default = 0];		    //整车的高度; 用于规避碰撞
+    optional float locate_wheel_base = 7[default = 0];	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
+    optional float locate_wheel_width = 8[default = 0];	    //整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
+    optional bool locate_correct = 9[default = false];		    //整车的校准标记位
+
+    optional float locate_front_theta = 10[default = 0];	    //整车的前轮的旋转角
+    
+    optional float uniformed_car_x = 11;        //转角复位后,车辆中心点x
+    optional float uniformed_car_y = 12;        //转角复位后,车辆中心点y
 }
 
 //车辆基本信息
 message Car_info
 {
-    optional float                      car_length=1;           //车长
-    optional float                      car_width=2;            //车宽
-    optional float                      car_height=3;           //车高
-    optional string                     license=4;              //车辆凭证号
+    optional float                      car_length=1[default = 0];           //车长
+    optional float                      car_width=2[default = 0];            //车宽
+    optional float                      car_height=3[default = 0];           //车高
+    optional string                     license=4[default = ""];              //车辆凭证号
+    optional float                      car_wheel_base = 5[default = 0];	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
+    optional float                      car_wheel_width = 6[default = 0];	//整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
 }
 
 //车位状态枚举
 enum Parkspace_status
 {
-    eParkspace_empty            = 0;         //空闲，可分配
-    eParkspace_occupied         = 1;         //被占用，不可分配
-    eParkspace_reserverd        = 2;         //被预约，预约车辆可分配
-    eParkspace_error            = 3;         //车位机械结构或硬件故障
+    eParkspace_status_unknow    = 0;
+    eParkspace_empty            = 1;         //空闲，可分配
+    eParkspace_occupied         = 2;         //被占用，不可分配
+    eParkspace_reserved         = 3;         //被预约，预约车辆可分配
+    eParkspace_locked           = 4;         //临时锁定，不可分配
+    eParkspace_error            = 5;         //车位机械结构或硬件故障
 }
 
+//车位朝向, 小号朝前朝南, 大号朝后朝北
 enum Direction
 {
-    eForward = 1;
-    eBackward = 2;
+    eDirection_unknow = 0;
+    eForward = 1;                   //小号朝前朝南
+    eBackward = 2;                  //大号朝后朝北
+}
+
+//车位分配路线（根据中跑车的路线来定）
+enum Parkspace_path
+{
+	UNKNOW_PATH = 0;
+    OPTIMAL_PATH = 1;
+    LEFT_PATH = 2;
+    RIGHT_PATH = 3;
+    TEMPORARY_CACHE_PATH = 4;
+}
+
+//车位类型
+enum Parkspace_type
+{
+    UNKNOW_PARKSPACE_TYPE = 0;
+    MIN_PARKINGSPACE = 1;//小车位
+    MID_PARKINGSPACE = 2;//中车位
+    BIG_PARKINGSPACE = 3;//大车位
+}
+
+//汽车类型
+enum Car_type
+{
+    UNKNOW_CAR_TYPE = 0;
+    MIN_CAR = 1;//小车
+    MID_CAR = 2;//中车
+    BIG_CAR = 3;//大车
 }
 
 //单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
 message Parkspace_info
 {
-    optional int32              parkspace_id=1;         //车位编号
-    optional int32              index=2;                //同层编号
-    optional Direction          direction=3;            //前后
-    optional int32              floor=4;                //楼层
-    optional float              length=5;               //车位长
-    optional float              width=6;                //车位宽
-    optional float              height=7;               //车位高
-    optional Parkspace_status   parkspace_status=8;     //车位当前状态
-    optional Car_info           car_info=9;              //当前车位存入车辆的凭证号
-    optional string             entry_time=10;          //入场时间
-    optional string             leave_time=11;          //离场时间
+    optional int32              parkingspace_index_id = 1;            //车位ID
+    optional Parkspace_type     parkingspace_type = 2;                //车位类型
+    optional int32              parkingspace_unit_id = 3;            //车位单元号
+    optional int32              parkingspace_label_id = 4;            //车位单元内部ID
+    optional int32              parkingspace_room_id = 5;             //同层编号
+    optional Direction          parkingspace_direction = 6;           //前后
+    optional int32              parkingspace_floor_id = 7;            //楼层
+    optional float              parkingspace_width = 8;               //车位宽
+    optional float              parkingspace_height = 9;              //车位高
+    optional Parkspace_status   parkingspace_status = 10;              //车位当前状态
+    optional Car_info           car_info = 11;                        //车辆信息
+    optional string             entry_time = 12;                      //入场时间
+    optional string             leave_time = 13;                      //离场时间
+
+
+    optional Parkspace_path     parkspace_path = 14;            // 车位分配路线
+    optional float              path_estimate_time = 15;        //车位分配路线 time(s)
+    optional Parkspace_status   parkspace_status_target = 16;     //车位目标状态
+
+    optional Car_type           car_type = 17;                        //车辆类型
+
+
 }
 
 /*
@@ -209,4 +270,24 @@ enum Step_statu
     eWorking=1;
     eError=2;
     eFinished=3;
+}
+
+//调度设备的类型
+enum Dispatch_device_type
+{
+    ROBOT_1                                 = 101;      //一号出口的专用机器手(只能负责1号出口的取车)(目前没有安装,暂时不考虑)
+    ROBOT_2                                 = 102;      //中间的大型机器手   (可以负责1~6号出入口的停车和取车)
+
+    CARRIER_1                               = 200;      //左侧0号电梯井的搬运器(升降电梯 中跑车 小跑车 三合一为搬运器)
+    CARRIER_2                               = 207;      //右侧7号电梯井的搬运器(升降电梯 中跑车 小跑车 三合一为搬运器)
+    CARRIER_3                               = 203;      //中间3楼的搬运器(中跑车 小跑车 二合一为搬运器)(没有电梯, 只能在3楼活动)
+
+    PASSAGEWAY_0                            = 300;      //0号出口(在左侧电梯井, 只能取车)(暂时不存在)
+    PASSAGEWAY_1                            = 301;      //1号出入口
+    PASSAGEWAY_2                            = 302;      //2号出入口
+    PASSAGEWAY_3                            = 303;      //3号出入口
+    PASSAGEWAY_4                            = 304;      //4号出入口
+    PASSAGEWAY_5                            = 305;      //5号出入口
+    PASSAGEWAY_6                            = 306;      //6号出入口
+    PASSAGEWAY_7                            = 307;      //7号出口(在右侧电梯井, 只能取车)
 }

setting/communication.prototxt

@@ -3,7 +3,7 @@
 communication_parameters
 {
 
-  bind_string:"tcp://192.168.1.233:30010"
+  bind_string:"tcp://192.168.1.240:30010"
   connect_string_vector:"tcp://192.168.1.233:30000"
  # connect_string_vector:"tcp://192.168.2.166:9002"
 

setting/velodyne_manager.prototxt

@@ -11,7 +11,7 @@ velodyne_lidars
     port:2368
     model:"VLP16"
     calibrationFile:"../setting/VLP16db.yaml"
-    lidar_id:0
+    lidar_id:6
     max_range:8.0
     min_range:0.1
     min_angle:0
@@ -37,7 +37,7 @@ velodyne_lidars
     port:2369
     model:"VLP16"
     calibrationFile:"../setting/VLP16db.yaml"
-    lidar_id:1
+    lidar_id:5
     max_range:8.0
     min_range:0.1
     min_angle:0
@@ -82,12 +82,18 @@ region
 	maxx:1.6
 	miny:-2.6
 	maxy:2.6
-	minz:0.02
-	maxz:0.2
-    region_id:0
+	minz:0.025
+	maxz:1.0
+    region_id:5
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:1.913
+    plc_offsety:5.998
     lidar_exts
     {
-        lidar_id:0
+        lidar_id:6
         calib
         {
             cx:1.9018
@@ -96,7 +102,7 @@ region
     }
     lidar_exts
     {
-        lidar_id:1
+        lidar_id:5
         calib
         {
             #cx:-4.021775
@@ -135,4 +141,4 @@ region
 #             cy:0.3
 #         }
 #     }
-# }
+# }

system/system_executor.cpp

@@ -284,26 +284,7 @@ Error_manager System_executor::encapsulate_send_status()
 		Common_data::Car_wheel_information	t_car_wheel_information;
 		t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
 
-		//无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
-		message::Locate_information t_locate_information;
-		//= t_multi_status_msg.add_locate_information_realtime();
-		t_locate_information.set_locate_x(t_car_wheel_information.center_x);
-		t_locate_information.set_locate_y(t_car_wheel_information.center_y);
-		t_locate_information.set_locate_angle(t_car_wheel_information.car_angle);
-		t_locate_information.set_locate_length(0);
-		t_locate_information.set_locate_width(0);
-		t_locate_information.set_locate_height(0);
-		t_locate_information.set_locate_wheel_base(t_car_wheel_information.wheel_base);
-		t_locate_information.set_locate_wheel_width(t_car_wheel_information.wheel_width);
-		t_locate_information.set_locate_front_theta(t_car_wheel_information.front_theta);
-		t_locate_information.set_locate_correct(t_car_wheel_information.correctness);
-		t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
-		t_multi_status_msg.set_ground_status(t_car_wheel_information.correctness?message::Ground_statu::Car_correct:message::Ground_statu::Noise);
-
-		t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
-        t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
-        t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
-
+		// 获取区域点云填入信息
 		pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
 		iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
 		if(cloud_count == 5)
@@ -320,6 +301,43 @@ Error_manager System_executor::encapsulate_send_status()
 			tp_cloud->set_z(t_region_cloud->points[j].z);
 		}
 
+		//无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
+		message::Locate_information t_locate_information;
+		//= t_multi_status_msg.add_locate_information_realtime();
+		t_locate_information.set_locate_x(t_car_wheel_information.car_center_x);
+		t_locate_information.set_locate_y(t_car_wheel_information.car_center_y);
+		t_locate_information.set_locate_angle(t_car_wheel_information.car_angle);
+		t_locate_information.set_locate_length(0);
+		t_locate_information.set_locate_width(0);
+		t_locate_information.set_locate_height(0);
+		t_locate_information.set_locate_wheel_base(t_car_wheel_information.car_wheel_base);
+		t_locate_information.set_locate_wheel_width(t_car_wheel_information.car_wheel_width);
+		t_locate_information.set_locate_front_theta(t_car_wheel_information.car_front_theta);
+		t_locate_information.set_locate_correct(t_car_wheel_information.correctness);
+		t_locate_information.set_uniformed_car_x(t_car_wheel_information.uniform_car_x);
+		t_locate_information.set_uniformed_car_y(t_car_wheel_information.uniform_car_y);
+		t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
+		// 当前超界提示仅保留一项
+		if(!t_car_wheel_information.correctness)
+		{
+			if(t_region_cloud->size() > 0)
+				t_multi_status_msg.set_ground_status(message::Ground_statu::Noise);
+			else
+				t_multi_status_msg.set_ground_status(message::Ground_statu::Nothing);
+		}else if(t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
+		{
+			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_left_out);
+		}else if(t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
+		{
+			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_right_out);
+		}else{
+			t_multi_status_msg.set_ground_status(message::Ground_statu::Car_correct);
+		}
+
+		t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
+        t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
+        t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
+
 		std::string t_msg = t_multi_status_msg.SerializeAsString();
 		System_communication::get_instance_references().encapsulate_msg(t_msg);
         // std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
@@ -561,13 +579,13 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 	// ------------------- 切换为velodyne ------------------
 	if(t_car_information_by_velodyne.correctness == true )
 	{
-		t_car_information_result.center_x = t_car_information_by_velodyne.center_x;
-		t_car_information_result.center_y = t_car_information_by_velodyne.center_y;
+		t_car_information_result.car_center_x = t_car_information_by_velodyne.car_center_x;
+		t_car_information_result.car_center_y = t_car_information_by_velodyne.car_center_y;
 
 		t_car_information_result.car_angle = t_car_information_by_velodyne.car_angle;
-		t_car_information_result.wheel_base = t_car_information_by_velodyne.wheel_base;
+		t_car_information_result.car_wheel_base = t_car_information_by_velodyne.car_wheel_base;
 
-		t_car_information_result.wheel_width = t_car_information_by_velodyne.wheel_width;
+		t_car_information_result.car_wheel_width = t_car_information_by_velodyne.car_wheel_width;
 		t_car_information_result.correctness = true;
 	}else{
 		t_result.set_error_level_down(NEGLIGIBLE_ERROR);
@@ -587,15 +605,15 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 	t_ground_detect_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_result.get_error_level());
 	t_ground_detect_response_msg.mutable_error_manager()->set_error_description(t_result.get_error_description(), t_result.get_description_length());
 
-	t_ground_detect_response_msg.mutable_locate_information()->set_locate_x(t_car_information_result.center_x);
-	t_ground_detect_response_msg.mutable_locate_information()->set_locate_y(t_car_information_result.center_y);
+	t_ground_detect_response_msg.mutable_locate_information()->set_locate_x(t_car_information_result.car_center_x);
+	t_ground_detect_response_msg.mutable_locate_information()->set_locate_y(t_car_information_result.car_center_y);
 	t_ground_detect_response_msg.mutable_locate_information()->set_locate_angle(t_car_information_result.car_angle);
 	t_ground_detect_response_msg.mutable_locate_information()->set_locate_length(t_car_information_result.car_length);
 	t_ground_detect_response_msg.mutable_locate_information()->set_locate_width(t_car_information_result.car_width);
 	t_ground_detect_response_msg.mutable_locate_information()->set_locate_height(t_car_information_result.car_height);
-	t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_base(t_car_information_result.wheel_base);
-	t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_width(t_car_information_result.wheel_width);
-	t_ground_detect_response_msg.mutable_locate_information()->set_locate_front_theta(t_car_information_result.front_theta);
+	t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_base(t_car_information_result.car_wheel_base);
+	t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_width(t_car_information_result.car_wheel_width);
+	t_ground_detect_response_msg.mutable_locate_information()->set_locate_front_theta(t_car_information_result.car_front_theta);
 	t_ground_detect_response_msg.mutable_locate_information()->set_locate_correct(t_car_information_result.correctness);
 
 

tool/common_data.cpp

@@ -1,41 +1,319 @@
 //
 // Created by huli on 2020/9/8.
 //
-float center_x;				//整车的中心点x值, 四轮的中心
-float center_y;				//整车的中心点y值, 四轮的中心
-float car_angle;			//整车的车身旋转角,
-float car_length;			//整车的长度, 用于规避碰撞
-float car_width;			//整车的宽度, 用于规避碰撞
-float car_height;			//整车的高度, 用于规避碰撞
-float wheel_base;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
-float wheel_width;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
-float front_theta;			//整车的前轮的旋转角
-
-bool correctness;			//整车的校准标记位
 
 #include "common_data.h"
 
-void Common_data::copy_data(Car_measure_information& car_measure_information_in, Car_wheel_information& car_wheel_information_out)
+void Common_data::copy_data(Car_measure_information& car_measure_information_out, const message::Locate_information& locate_information_in)
 {
-	car_wheel_information_out.center_x = car_measure_information_in.center_x;
-	car_wheel_information_out.center_y = car_measure_information_in.center_y;
+	if ( locate_information_in.has_locate_x() )
+	{
+		car_measure_information_out.car_center_x = locate_information_in.locate_x();
+	}
+	else
+	{
+		car_measure_information_out.car_center_x = 0;
+	}
+	if ( locate_information_in.has_locate_y() )
+	{
+		car_measure_information_out.car_center_y = locate_information_in.locate_y();
+	}
+	else
+	{
+		car_measure_information_out.car_center_y = 0;
+	}
+	if ( locate_information_in.has_locate_angle() )
+	{
+		car_measure_information_out.car_angle = locate_information_in.locate_angle();
+	}
+	else
+	{
+		car_measure_information_out.car_angle = 0;
+	}
+	if ( locate_information_in.has_locate_length() )
+	{
+		car_measure_information_out.car_length = locate_information_in.locate_length();
+	}
+	else
+	{
+		car_measure_information_out.car_length = 0;
+	}
+	if ( locate_information_in.has_locate_width() )
+	{
+		car_measure_information_out.car_width = locate_information_in.locate_width();
+	}
+	else
+	{
+		car_measure_information_out.car_width = 0;
+	}
+	if ( locate_information_in.has_locate_height() )
+	{
+		car_measure_information_out.car_height = locate_information_in.locate_height();
+	}
+	else
+	{
+		car_measure_information_out.car_height = 0;
+	}
+	if ( locate_information_in.has_locate_wheel_base() )
+	{
+		car_measure_information_out.car_wheel_base = locate_information_in.locate_wheel_base();
+	}
+	else
+	{
+		car_measure_information_out.car_wheel_base = 0;
+	}
+	if ( locate_information_in.has_locate_wheel_width() )
+	{
+		car_measure_information_out.car_wheel_width = locate_information_in.locate_wheel_width();
+	}
+	else
+	{
+		car_measure_information_out.car_wheel_width = 0;
+	}
+	if ( locate_information_in.has_locate_front_theta() )
+	{
+		car_measure_information_out.car_front_theta = locate_information_in.locate_front_theta();
+	}
+	else
+	{
+		car_measure_information_out.car_front_theta = 0;
+	}
+	if ( locate_information_in.has_locate_correct() )
+	{
+		car_measure_information_out.correctness = locate_information_in.locate_correct();
+	}
+	else
+	{
+		car_measure_information_out.correctness = false;
+	}
+}
+void Common_data::copy_data(message::Locate_information& locate_information_out, const Car_measure_information& car_measure_information_in)
+{
+	locate_information_out.set_locate_x(car_measure_information_in.car_center_x);
+	locate_information_out.set_locate_y(car_measure_information_in.car_center_y);
+	locate_information_out.set_locate_angle(car_measure_information_in.car_angle);
+	locate_information_out.set_locate_length(car_measure_information_in.car_length);
+	locate_information_out.set_locate_width(car_measure_information_in.car_width);
+	locate_information_out.set_locate_height(car_measure_information_in.car_height);
+	locate_information_out.set_locate_wheel_base(car_measure_information_in.car_wheel_base);
+	locate_information_out.set_locate_wheel_width(car_measure_information_in.car_wheel_width);
+	locate_information_out.set_locate_front_theta(car_measure_information_in.car_front_theta);
+	locate_information_out.set_locate_correct(car_measure_information_in.correctness);
+}
+
+void Common_data::copy_data(Car_information& car_information_out, const message::Car_info& car_info_in)
+{
+	if ( car_info_in.has_license() )
+	{
+		car_information_out.license = car_info_in.license();
+	}
+	else
+	{
+		car_information_out.license.clear();
+	}
+	if ( car_info_in.has_car_length() )
+	{
+		car_information_out.car_length = car_info_in.car_length();
+	}
+	else
+	{
+		car_information_out.car_length = 0;
+	}
+	if ( car_info_in.has_car_width() )
+	{
+		car_information_out.car_width = car_info_in.car_width();
+	}
+	else
+	{
+		car_information_out.car_width = 0;
+	}
+	if ( car_info_in.has_car_height() )
+	{
+		car_information_out.car_height = car_info_in.car_height();
+	}
+	else
+	{
+		car_information_out.car_height = 0;
+	}
+	if ( car_info_in.has_car_wheel_base() )
+	{
+		car_information_out.car_wheel_base = car_info_in.car_wheel_base();
+	}
+	else
+	{
+		car_information_out.car_wheel_base = 0;
+	}
+	if ( car_info_in.has_car_wheel_width() )
+	{
+		car_information_out.car_wheel_width = car_info_in.car_wheel_width();
+	}
+	else
+	{
+		car_information_out.car_wheel_width = 0;
+	}
+}
+void Common_data::copy_data(message::Car_info& car_info_out, const Car_information& car_information_in)
+{
+	car_info_out.set_license(car_information_in.license);
+	car_info_out.set_car_length(car_information_in.car_length);
+	car_info_out.set_car_width(car_information_in.car_width);
+	car_info_out.set_car_height(car_information_in.car_height);
+	car_info_out.set_car_wheel_base(car_information_in.car_wheel_base);
+	car_info_out.set_car_wheel_width(car_information_in.car_wheel_width);
+}
+
+void Common_data::copy_data(Parkspace_information& parkspace_information_out, const message::Parkspace_info& parkspace_info_in)
+{
+
+		parkspace_information_out.parkingspace_index_id = parkspace_info_in.parkingspace_index_id();
+		parkspace_information_out.parkingspace_type = (Common_data::Parkspace_type)parkspace_info_in.parkingspace_type();
+
+		parkspace_information_out.parkingspace_unit_id = parkspace_info_in.parkingspace_unit_id();
+		parkspace_information_out.parkingspace_floor_id = parkspace_info_in.parkingspace_floor_id();
+		parkspace_information_out.parkingspace_room_id = parkspace_info_in.parkingspace_room_id();
+		parkspace_information_out.parkingspace_direction = (Common_data::Direction)parkspace_info_in.parkingspace_direction();
+
+		parkspace_information_out.parkingspace_width = parkspace_info_in.parkingspace_width();
+		parkspace_information_out.parkingspace_height = parkspace_info_in.parkingspace_height();
+		parkspace_information_out.parkingspace_status = (Common_data::Parkspace_status)parkspace_info_in.parkingspace_status();
+
+		copy_data(parkspace_information_out.car_information, (message::Car_info&) parkspace_info_in.car_info());
+		parkspace_information_out.car_entry_time = parkspace_info_in.entry_time();
+		parkspace_information_out.car_leave_time = parkspace_info_in.leave_time();
+
+		parkspace_information_out.parkspace_path = (Common_data::Parkspace_path)parkspace_info_in.parkspace_path();
+		parkspace_information_out.path_estimate_time = parkspace_info_in.path_estimate_time();
+		parkspace_information_out.parkspace_status_target = (Common_data::Parkspace_status)parkspace_info_in.parkspace_status_target();
+
+}
+void Common_data::copy_data(message::Parkspace_info& parkspace_info_out, const Parkspace_information& parkspace_information_in)
+{
+	parkspace_info_out.set_parkingspace_index_id(parkspace_information_in.parkingspace_index_id);
+	parkspace_info_out.set_parkingspace_type((message::Parkspace_type)parkspace_information_in.parkingspace_type);
+	parkspace_info_out.set_parkingspace_unit_id(parkspace_information_in.parkingspace_unit_id);
+	parkspace_info_out.set_parkingspace_room_id(parkspace_information_in.parkingspace_room_id);
+	parkspace_info_out.set_parkingspace_direction((message::Direction)parkspace_information_in.parkingspace_direction);
+	parkspace_info_out.set_parkingspace_floor_id(parkspace_information_in.parkingspace_floor_id);
+	parkspace_info_out.set_parkingspace_width(parkspace_information_in.parkingspace_width);
+	parkspace_info_out.set_parkingspace_height(parkspace_information_in.parkingspace_height);
+	parkspace_info_out.set_parkingspace_status((message::Parkspace_status)parkspace_information_in.parkingspace_status);
+	copy_data(*parkspace_info_out.mutable_car_info(), parkspace_information_in.car_information);
+	parkspace_info_out.set_entry_time(parkspace_information_in.car_entry_time);
+	parkspace_info_out.set_leave_time(parkspace_information_in.car_leave_time);
+	parkspace_info_out.set_parkspace_path((message::Parkspace_path)parkspace_information_in.parkspace_path);
+	parkspace_info_out.set_path_estimate_time(parkspace_information_in.path_estimate_time);
+	parkspace_info_out.set_parkspace_status_target((message::Parkspace_status)parkspace_information_in.parkspace_status_target);
+}
+
+
+
+
+
+
+
+
+
+void Common_data::transform_data(Car_information& car_information_out, const Car_measure_information& car_measure_information_in)
+{
+	car_information_out.license.clear();
+	car_information_out.car_length = car_measure_information_in.car_length;
+	car_information_out.car_width = car_measure_information_in.car_width;
+	car_information_out.car_height = car_measure_information_in.car_height;
+	car_information_out.car_wheel_base = car_measure_information_in.car_wheel_base;
+	car_information_out.car_wheel_width = car_measure_information_in.car_wheel_width;
+}
+void Common_data::transform_data(Car_measure_information& car_measure_information_out, const Car_information& car_informatio_in)
+{
+	car_measure_information_out.car_center_x = 0;
+	car_measure_information_out.car_center_y = 0;
+	car_measure_information_out.car_angle = 0;
+	car_measure_information_out.car_length = car_informatio_in.car_length;
+	car_measure_information_out.car_width = car_informatio_in.car_width;
+	car_measure_information_out.car_height = car_informatio_in.car_height;
+	car_measure_information_out.car_wheel_base = car_informatio_in.car_wheel_base;
+	car_measure_information_out.car_wheel_width = car_informatio_in.car_wheel_width;
+	car_measure_information_out.car_front_theta = 0;
+	car_measure_information_out.correctness = false;
+
+
+}
+
+void Common_data::transform_data(Car_wheel_information& car_wheel_information_out, const Car_measure_information& car_measure_information_in)
+{
+	car_wheel_information_out.car_center_x = car_measure_information_in.car_center_x;
+	car_wheel_information_out.car_center_y = car_measure_information_in.car_center_y;
 	car_wheel_information_out.car_angle = car_measure_information_in.car_angle;
 
-	car_wheel_information_out.wheel_base = car_measure_information_in.wheel_base;
-	car_wheel_information_out.wheel_width = car_measure_information_in.wheel_width;
-	car_wheel_information_out.front_theta = car_measure_information_in.front_theta;
+	car_wheel_information_out.car_wheel_base = car_measure_information_in.car_wheel_base;
+	car_wheel_information_out.car_wheel_width = car_measure_information_in.car_wheel_width;
+	car_wheel_information_out.car_front_theta = car_measure_information_in.car_front_theta;
 	car_wheel_information_out.correctness = car_measure_information_in.correctness;
 }
-void Common_data::copy_data(Car_wheel_information& car_wheel_information_in, Car_measure_information& car_measure_information_out)
+void Common_data::transform_data(Car_measure_information& car_measure_information_out, const Car_wheel_information& car_wheel_information_in)
 {
-	car_measure_information_out.center_x = car_wheel_information_in.center_x;
-	car_measure_information_out.center_y = car_wheel_information_in.center_y;
+	car_measure_information_out.car_center_x = car_wheel_information_in.car_center_x;
+	car_measure_information_out.car_center_y = car_wheel_information_in.car_center_y;
 	car_measure_information_out.car_angle = car_wheel_information_in.car_angle;
 	car_measure_information_out.car_length = 0;
 	car_measure_information_out.car_width = 0;
 	car_measure_information_out.car_height = 0;
-	car_measure_information_out.wheel_base = car_wheel_information_in.wheel_base;
-	car_measure_information_out.wheel_width = car_wheel_information_in.wheel_width;
-	car_measure_information_out.front_theta = car_wheel_information_in.front_theta;
+	car_measure_information_out.car_wheel_base = car_wheel_information_in.car_wheel_base;
+	car_measure_information_out.car_wheel_width = car_wheel_information_in.car_wheel_width;
+	car_measure_information_out.car_front_theta = car_wheel_information_in.car_front_theta;
 	car_measure_information_out.correctness = car_wheel_information_in.correctness;
-}
+}
+
+
+void Common_data::scaling(Car_measure_information& car_measure_information, float rate)
+{
+	car_measure_information.car_center_x *= rate;
+	car_measure_information.car_center_y *= rate;
+	car_measure_information.car_length *= rate;
+	car_measure_information.car_width *= rate;
+	car_measure_information.car_height *= rate;
+	car_measure_information.car_wheel_base *= rate;
+	car_measure_information.car_wheel_width *= rate;
+}
+void Common_data::scaling(Car_information& car_information, float rate)
+{
+	car_information.car_length *= rate;
+	car_information.car_width *= rate;
+	car_information.car_height *= rate;
+	car_information.car_wheel_base *= rate;
+	car_information.car_wheel_width *= rate;
+
+}
+void Common_data::scaling(Parkspace_information& parkspace_information, float rate)
+{
+	parkspace_information.parkingspace_width *= rate;
+	parkspace_information.parkingspace_height *= rate;
+	scaling(parkspace_information.car_information, rate);
+}
+
+bool Common_data::approximate_rate(float a, float b, float rate)
+{
+	if ( a >= b*(1-rate) && a< b*(1+rate))
+	{
+	    return true;
+	}
+	else
+	{
+	    return false;
+	}
+}
+
+bool Common_data::approximate_difference(float a, float b, float difference)
+{
+	if ( a >= (b-difference) && a< (b+difference) )
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+
+

tool/common_data.h

@@ -4,10 +4,12 @@
 
 #ifndef NNXX_TESTS_COMMON_DATA_H
 #define NNXX_TESTS_COMMON_DATA_H
+
 #include <chrono>
 #include <cmath>
 #include <string>
 #include <string.h>
+#include "../message/message_base.pb.h"
 
 class Common_data
 {
@@ -17,55 +19,73 @@ public:
 	//vlp16雷达扫描周期100ms, (频率10hz), 一般设置大一些
 #define VLP16_SCAN_CYCLE_MS 		110
 
+
+//车位表
+#define PARKSPACE_ID_BASE							0
+#define PASSAGEWAY_ID_BASE							1100
+
+//唯一码的默认长度 32byte
+#define COMMAND_KEY_DEFAULT_LENGTH					32
+
+
+
+	
 	//整车的测量信息
 	struct Car_measure_information
 	{
-		float center_x = 0;				//整车的中心点x值, 四轮的中心
-		float center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
+		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
 		float car_angle = 0;			//整车的车身旋转角,
 		float car_length = 0;			//整车的长度, 用于规避碰撞
 		float car_width = 0;			//整车的宽度, 用于规避碰撞
 		float car_height = 0;			//整车的高度, 用于规避碰撞
-		float wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
-		float wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
-		float front_theta = 0;			//整车的前轮的旋转角
+		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float car_front_theta = 0;			//整车的前轮的旋转角
 
 		bool correctness = false;			//整车的校准标记位
 
 	};
-
+	
+	
 	//四轮的测量信息
 	struct Car_wheel_information
 	{
-		float center_x = 0;				//整车的中心点x值, 四轮的中心
-		float center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
+		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
 		float car_angle = 0;			//整车的车身旋转角,
 
-		float wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
-		float wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
-		float front_theta = 0;			//整车的前轮的旋转角
+		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float car_front_theta = 0;			//整车的前轮的旋转角
 
 		bool correctness = false;			//整车的校准标记位
-		public:
+
+		float uniform_car_x = 0;
+		float uniform_car_y = 0;
+
+		int range_status = 0;
+
+	public:
 		Car_wheel_information& operator+=(const Car_wheel_information& other)
 		{
-			this->center_x += other.center_x;
-			this->center_y += other.center_y;
+			this->car_center_x += other.car_center_x;
+			this->car_center_y += other.car_center_y;
 			this->car_angle += other.car_angle;
-			this->wheel_base += other.wheel_base;
-			this->wheel_width += other.wheel_width;
-			this->front_theta += other.front_theta;
+			this->car_wheel_base += other.car_wheel_base;
+			this->car_wheel_width += other.car_wheel_width;
+			this->car_front_theta += other.car_front_theta;
 			this->correctness &= other.correctness;
 			return *this;
 		}
 		Car_wheel_information& operator-=(const Car_wheel_information& other)
 		{
-			this->center_x -= other.center_x;
-			this->center_y -= other.center_y;
+			this->car_center_x -= other.car_center_x;
+			this->car_center_y -= other.car_center_y;
 			this->car_angle -= other.car_angle;
-			this->wheel_base -= other.wheel_base;
-			this->wheel_width -= other.wheel_width;
-			this->front_theta -= other.front_theta;
+			this->car_wheel_base -= other.car_wheel_base;
+			this->car_wheel_width -= other.car_wheel_width;
+			this->car_front_theta -= other.car_front_theta;
 			this->correctness &= other.correctness;
 			return *this;
 		}
@@ -73,12 +93,12 @@ public:
 		{
 			if(scalar==0)
 				return *this;
-			this->center_x /= scalar;
-			this->center_y /= scalar;
+			this->car_center_x /= scalar;
+			this->car_center_y /= scalar;
 			this->car_angle /= scalar;
-			this->wheel_base /= scalar;
-			this->wheel_width /= scalar;
-			this->front_theta /= scalar;
+			this->car_wheel_base /= scalar;
+			this->car_wheel_width /= scalar;
+			this->car_front_theta /= scalar;
 			return *this;
 		}
 		// 定义评分规则, 
@@ -87,12 +107,12 @@ public:
 		{
 			float weights[] = {1.0f, 1.0f, 0.5f, 0.1f, 0.05f, 0.05f};
 			float final_score = 0.0f;
-			final_score += fabs(weights[0] * this->center_x);
-			final_score += fabs(weights[1] * this->center_y);
+			final_score += fabs(weights[0] * this->car_center_x);
+			final_score += fabs(weights[1] * this->car_center_y);
 			final_score += fabs(weights[2] * this->car_angle);
-			final_score += fabs(weights[3] * this->wheel_base);
-			final_score += fabs(weights[4] * this->wheel_width);
-			final_score += fabs(weights[5] * this->front_theta);
+			final_score += fabs(weights[3] * this->car_wheel_base);
+			final_score += fabs(weights[4] * this->car_wheel_width);
+			final_score += fabs(weights[5] * this->car_front_theta);
 
 			return final_score;
 		}
@@ -100,9 +120,25 @@ public:
 		std::string to_string()
         {
             char buf[512]={0};
-            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", center_x, center_y, car_angle, wheel_base, wheel_width, front_theta);
+            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", car_center_x, car_center_y, car_angle, car_wheel_base, car_wheel_width, car_front_theta);
             return std::string(buf);
         }
+
+		// 旋转正位(theta至90度)后车辆中心
+		void theta_uniform(float turnplate_cx, float turnplate_cy, float &x, float &y)
+		{
+			float d_theta = (90.0f - car_angle) * M_PI / 180.0f;
+			float car_x = car_center_x - turnplate_cx;
+			float car_y = car_center_y - turnplate_cy;
+			x = cos(d_theta) * car_x - sin(d_theta) * car_y + turnplate_cx;
+			y = sin(d_theta) * car_x + cos(d_theta) * car_y + turnplate_cy;
+		}
+
+		// 更新旋转正位后自身缓存的车辆中心
+		void theta_uniform(float turnplate_cx, float turnplate_cy)
+		{
+			theta_uniform(turnplate_cx, turnplate_cy, uniform_car_x, uniform_car_y);
+		}
 	};
 
 	// 带时间戳的四轮测量信息
@@ -141,11 +177,160 @@ public:
 		}
 	};
 
-	static void copy_data(Car_measure_information& car_measure_information_in, Car_wheel_information& car_wheel_information_out);
-	static void copy_data(Car_wheel_information& car_wheel_information_in, Car_measure_information& car_measure_information_out);
 
 
+	struct Car_information
+	{
+		std::string                license;              //车辆凭证号
+		float                      car_length=0;           //车长
+		float                      car_width=0;            //车宽
+		float                      car_height=0;           //车高
+		float                      car_wheel_base = 0;	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
+		float                      car_wheel_width = 0;	//整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
+	};
+
+
+//车位状态枚举
+	enum Parkspace_status
+	{
+		PARKSPACE_STATUS_UNKNOW		= 0,
+		PARKSPACE_EMPTY     		= 1,         //空闲，可分配
+		PARKSPACE_OCCUPIED    		= 2,         //被占用，不可分配
+		PARKSPACE_RESERVED  		= 3,         //被预约，预约车辆可分配, 暂时不用
+		PARKSPACE_LOCKED 			= 4,         //临时锁定，不可分配
+		PARKSPACE_ERROR 			= 5,         //车位机械结构或硬件故障
+	};
+
+	enum Direction
+	{
+		DIRECTION_UNKNOW = 0,
+		DIRECTION_FORWARD = 1,
+		DIRECTION_BACKWARD = 2,
+	};
+
+//车位分配路线（根据中跑车的路线来定）
+	enum Parkspace_path
+	{
+		UNKNOW_PATH = 0,
+		OPTIMAL_PATH = 1,
+		LEFT_PATH = 2,
+		RIGHT_PATH = 3,
+		TEMPORARY_CACHE_PATH = 4,
+
+	};
+	//车位类型
+	enum Parkspace_type
+	{
+		UNKNOW_PARKSPACE_TYPE = 0,
+		MIN_PARKINGSPACE = 1,//小车位
+		MID_PARKINGSPACE = 2,//中车位
+		BIG_PARKINGSPACE = 3,//大车位
+	};
+
+	//汽车类型
+	enum Car_type
+	{
+		UNKNOW_CAR_TYPE = 0,
+		MIN_CAR = 1,//小车
+		MID_CAR = 2,//中车
+		BIG_CAR = 3,//大车
+	};
+
+	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
+	struct Parkspace_information
+	{
+		int              	parkingspace_index_id=0;         //车位编号
+		Parkspace_type     	parkingspace_type=UNKNOW_PARKSPACE_TYPE;               //车位类型
 
+		int              	parkingspace_unit_id=0;            //区块编号
+		int              	parkingspace_floor_id=0;                //楼层
+		int              	parkingspace_room_id=0;                //同层编号
+		Direction    		parkingspace_direction=DIRECTION_UNKNOW;            //前后
+
+		float              	parkingspace_width=0;                //车位宽
+		float              	parkingspace_height=0;               //车位高
+		Parkspace_status   	parkingspace_status=PARKSPACE_EMPTY;     //车位当前状态
+
+		Car_information    	car_information;              //当前车位存入车辆的凭证号
+		std::string        	car_entry_time;          //入场时间
+		std::string        	car_leave_time;          //离场时间
+
+		Parkspace_path     	parkspace_path = UNKNOW_PATH;            // 车位分配路线
+		float              	path_estimate_time = 0;        //车位分配路线 time(s)
+		Parkspace_status   	parkspace_status_target=PARKSPACE_EMPTY;     //车位目标状态
+	};
+
+	static void copy_data(Car_measure_information& car_measure_information_out, const message::Locate_information& locate_information_in);
+	static void copy_data(message::Locate_information& locate_information_out, const Car_measure_information& car_measure_information_in);
+	static void copy_data(Car_information& car_information_out, const message::Car_info& car_info_in);
+	static void copy_data(message::Car_info& car_info_out, const Car_information& car_information_in);
+	static void copy_data(Parkspace_information& parkspace_information_out, const message::Parkspace_info& parkspace_info_in);
+	static void copy_data(message::Parkspace_info& parkspace_info_out, const Parkspace_information& parkspace_information_in);
+
+
+	static void transform_data(Car_information& car_information_out, const Car_measure_information& car_measure_information_in);
+	static void transform_data(Car_measure_information& car_measure_information_out, const Car_information& car_informatio_in);
+	static void transform_data(Car_wheel_information& car_wheel_information_out, const Car_measure_information& car_measure_information_in);
+	static void transform_data(Car_measure_information& car_measure_information_out, const Car_wheel_information& car_wheel_information_in);
+
+	static void scaling(Car_measure_information& car_measure_information, float rate);
+	static void scaling(Car_information& car_information, float rate);
+	static void scaling(Parkspace_information& parkspace_information, float rate);
+
+
+	static bool approximate_rate(float a, float b, float rate);
+	static bool approximate_difference(float a, float b, float difference);
+
+
+
+
+
+
+
+
+
+	//调度模块
+	//调度流程类型
+	enum Dispatch_process_type
+	{
+		DISPATCH_PROCESS_TYPE_UNKNOW 			= 0,    //未知
+		DISPATCH_PROCESS_STORE  				= 101,    //存车
+		DISPATCH_PROCESS_PICKUP					= 102,	//取车
+	};
+
+
+	//调度动作方向   0=未知，1=存车，2=取车
+	enum Dispatch_motion_direction
+	{
+		DISPATCH_MOTION_DIRECTION_UNKNOWN					= 0,
+		DISPATCH_MOTION_DIRECTION_STORE              		= 1,    //
+		DISPATCH_MOTION_DIRECTION_PICKUP					= 2,    //
+	};
+
+	//出入口方向   0=未知，1=入口，2=出口
+	enum Passageway_direction
+	{
+		PASSAGEWAY_DIRECTION_UNKNOWN             			= 0,    //
+		PASSAGEWAY_DIRECTION_INLET               			= 1,    //
+		PASSAGEWAY_DIRECTION_OUTLET							= 2,    //
+	};
+
+
+	//楼上车位方向   0=未知，1=朝南，2=朝北
+	enum Parkingspace_direction
+	{
+		PARKINGSPACE_DIRECTION_UNKNOWN					= 0,    //
+		PARKINGSPACE_DIRECTION_SOUTH               		= 1,    //
+		PARKINGSPACE_DIRECTION_NORTH               		= 2,    //
+	};
+
+	//防撞雷达标志位  0=未知，1=位置正常，2=位置异常
+	enum Anticollision_lidar_flag
+	{
+		ANTICOLLISION_LIDAR_UNKNOWN               	= 0,    //
+		ANTICOLLISION_LIDAR_NORMAL               	= 1,    //
+		ANTICOLLISION_LIDAR_ERROR               	= 2,    //
+	};
 };
 
 

tool/point2D_tool.cpp

@@ -82,7 +82,7 @@ bool Point2D_tool::limit_with_point2D_box(float x, float y, Point2D_tool::Point2
 	}
 	return true;
 }
-
+#ifdef OPEN_PCL_FLAG
 //平面坐标的转换, 可以进行旋转和平移, 不能缩放
 bool Point2D_tool::transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
 										 Point2D_tool::Point2D_transform* p_point2D_transform)
@@ -137,3 +137,4 @@ bool Point2D_tool::transform_with_translation_rotation(pcl::PointCloud<pcl::Poin
 	}
 	return true;
 }
+#endif

tool/point2D_tool.h

@@ -5,10 +5,18 @@
 #ifndef POINT2D_TOOL_H
 #define POINT2D_TOOL_H
 
+
+
+//#define OPEN_PCL_FLAG
+#ifdef OPEN_PCL_FLAG
 #include <pcl/point_types.h>
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/conversions.h>
 #include <pcl/common/common.h>
+#endif
+
+#include <stddef.h>
+#include <math.h>
 
 class Point2D_tool
 {
@@ -67,6 +75,7 @@ public:
 	//判断平面坐标点是否在限制范围
 	static bool limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box);
 
+#ifdef OPEN_PCL_FLAG
 	//平面坐标的转换, 可以进行旋转和平移, 不能缩放
 	static bool transform_with_translation_rotation(float* p_x_in, float* p_y_in, float* p_x_out, float* p_y_out,
 													Point2D_tool::Point2D_transform* p_point2D_transform);
@@ -75,6 +84,8 @@ public:
 	static bool transform_with_translation_rotation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
 													pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out,
 													Point2D_tool::Point2D_transform* p_point2D_transform);
+
+#endif
 };
 
 

tool/point3D_tool.cpp

@@ -0,0 +1,5 @@
+//
+// Created by huli on 2021/3/24.
+//
+
+#include "point3D_tool.h"

tool/point3D_tool.h

@@ -0,0 +1,33 @@
+//
+// Created by huli on 2021/3/24.
+//
+
+#ifndef NNXX_TESTS_POINT3D_TOOL_H
+#define NNXX_TESTS_POINT3D_TOOL_H
+
+
+class Point3D_tool
+{
+public:
+	//坐标
+	struct Point3D
+	{
+		float x=0;				//x轴
+		float y=0;				//y轴
+		float z=0;				//z轴
+	};
+
+	//平面坐标的限定范围
+	struct Point3D_box
+	{
+		float x_min=0;				//x轴最小值
+		float x_max=0;				//x轴最大值
+		float y_min=0;				//y轴最小值
+		float y_max=0;				//y轴最大值
+		float z_min=0;				//z轴最小值
+		float z_max=0;				//z轴最大值
+	};
+};
+
+
+#endif //NNXX_TESTS_POINT3D_TOOL_H

tool/proto_tool.h

@@ -3,8 +3,8 @@
 
 
 
-#ifndef __POINT_TOOL_H
-#define __POINT_TOOL_H
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
 
 #include "../tool/singleton.h"
 #include <istream>
@@ -34,7 +34,7 @@ public:
 
 
 
-#endif //__POINT_TOOL_H
+#endif //__PROTO_TOOL_H
 
 
 

tool/rotate_center_caliber.cpp

@@ -0,0 +1,280 @@
+//
+// Created by zx on 2021/8/27.
+//
+
+#include "rotate_center_caliber.h"
+#include <pcl/registration/ndt.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+// extern pcl::visualization::PCLVisualizer viewer;
+// extern std::mutex mut;
+
+void rotate_center_caliber::rotate_center_caliber_init()
+{
+}
+
+void rotate_center_caliber::rotate_center_caliber_uninit()
+{
+    delete mp_matcher;
+    mp_matcher = nullptr;
+}
+
+void rotate_center_caliber::set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model)
+{
+    m_model_cloud=cloud_model;
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(*cloud_model, centroid);
+    m_center.x=centroid[0];
+    m_center.y=centroid[1];
+    m_center.z=centroid[2];
+
+    //平移点云
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -centroid[0],-centroid[1],-centroid[2];
+    pcl::transformPointCloud(*cloud_model, *transformed_cloud, transform_2);
+
+    printf("model center: %.5f,%.5f,%.5f\n",centroid[0],centroid[1],centroid[2]);
+
+
+    mp_matcher=new detect_ceres3d(transformed_cloud);
+}
+
+bool rotate_center_caliber::push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+{
+
+    //平移点云
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr offset_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -m_center.x,-m_center.y,-m_center.z;
+    pcl::transformPointCloud(*cloud, *offset_cloud, transform_2);
+
+    // mut.lock();
+    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); // green
+    // viewer.removePointCloud("cloud_rotation");
+    // viewer.addPointCloud(cloud, single_color,"cloud_rotation");
+    // mut.unlock();
+
+
+    if(mp_matcher== nullptr)
+        return false;
+
+    static detect_ceres3d::Detect_result last_result;
+    std::string error;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    detect_ceres3d::Detect_result result=last_result;
+    if(mp_matcher->detect(offset_cloud,result,transform_cloud,error))
+    {
+        if(fabs(result.theta-last_result.theta)>1.0/180.*M_PI)
+        {
+            printf(" detect success  r : %.3f   t:%.5f,%.5f\n", result.theta * 180. / M_PI, result.cx, result.cy);
+
+            Eigen::Affine3f transform_affine = Eigen::Affine3f::Identity();
+            pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+            transform_affine.translation() << m_center.x, m_center.y, m_center.z;
+            pcl::transformPointCloud(*transform_cloud, *new_cloud, transform_affine);
+
+            // mut.lock();
+            // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 0, 0); // green
+            // viewer.removePointCloud("ndt_cloud");
+            // viewer.addPointCloud(new_cloud, single_color, "ndt_cloud");
+            // mut.unlock();
+
+            match_transform solve_data;
+            solve_data.theta = result.theta;
+            solve_data.dx = result.cx;
+            solve_data.dy = result.cy;
+
+            m_match_datas.push_back(solve_data);
+            last_result = result;
+        }
+        else
+        {
+            // std::cout << " small dtheta: " << (result.theta - last_result.theta) * 180.0 / M_PI << std::endl;
+        }
+    }else{
+        std::cout << "?????" << std::endl;
+    }
+
+    /*Eigen::Matrix<float,6,1>  temp;
+    temp.setZero();
+    static Eigen::Matrix<float, 6,1> last_transform=temp;
+    printf("init r:%.3f,%.3f,%.3f, t :%.5f,%.5f,%.5f\n",
+            last_transform(2,0)*180.0/M_PI,last_transform(1,0)*180.0/M_PI,last_transform(0,0)*180.0/M_PI,
+           last_transform(3,0),last_transform(4,0),last_transform(5,0));
+    Eigen::Matrix4f matrix=match(m_model_cloud,cloud,last_transform);
+
+    //std::cout<<"  match :"<<matrix<<std::endl;
+
+    Eigen::Matrix3f roation=matrix.topLeftCorner(3,3);
+    Eigen::Vector3f euler_angles = roation.eulerAngles(0, 1, 2);
+
+    //std::cout<<euler_angles<<std::endl;
+    printf("r:%.3f,%.3f,%.3f  t:%.5f,%.5f\n",euler_angles[2]*180.0/M_PI,euler_angles[1]*180.0/M_PI,
+           euler_angles[0]*180.0/M_PI,matrix(0,3),matrix(1,3));
+
+    match_transform solve_data;
+    solve_data.theta=euler_angles[2];
+    solve_data.dx=matrix(0,3);
+    solve_data.dy=matrix(1,3);
+
+    m_match_datas.push_back(solve_data);
+
+    last_transform[0]=euler_angles[0];
+    last_transform[1]=euler_angles[1];
+    last_transform[2]=euler_angles[2];
+
+    last_transform[3]=matrix(0,3);
+    last_transform[4]=matrix(1,3);
+    last_transform[5]=matrix(2,3);
+*/
+
+    if(solve())
+    {
+        /*printf(" solve center = %.5f,%.5f\n",
+                m_rotation_center_x,m_rotation_center_y);*/
+
+        // updata_points();
+    }
+
+}
+
+
+Eigen::Matrix4f rotate_center_caliber::match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Eigen::Matrix<float, 6,1> init_transform)
+{
+    //初始化正态分布变换（NDT）
+    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+    //设置依赖尺度NDT参数
+    //为终止条件设置最小转换差异
+    ndt.setTransformationEpsilon(0.0001);
+    //为More-Thuente线搜索设置最大步长
+    ndt.setStepSize(0.1);
+    //设置NDT网格结构的分辨率（VoxelGridCovariance）
+    ndt.setResolution(0.3);
+    //设置匹配迭代的最大次数
+    ndt.setMaximumIterations(100);
+    // 设置要配准的点云
+    ndt.setInputCloud(cloud);
+    //设置点云配准目标
+    ndt.setInputTarget(cloud_center);
+    //设置使用机器人测距法得到的初始对准估计结果
+
+    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(init_transform(0), Eigen::Vector3f::UnitX()));
+    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(init_transform(1), Eigen::Vector3f::UnitY()));
+    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(init_transform(2), Eigen::Vector3f::UnitZ()));
+    Eigen::AngleAxisf rotation ;
+    rotation = yawAngle*pitchAngle*rollAngle;
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+
+    transform_2.translation() << init_transform(3),init_transform(4),init_transform(5);	// 三个数分别对应X轴、Y轴、Z轴方向上的平移
+    transform_2.rotate(rotation);	//同理，UnitX(),绕X轴；UnitY(
+
+    Eigen::Matrix4f init_guess = transform_2.matrix();
+    //计算需要的刚体变换以便将输入的点云匹配到目标点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    ndt.align(*output_cloud, init_guess);
+
+    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
+              << " score: " << ndt.getFitnessScore() << std::endl;
+
+
+
+    //使用创建的变换对未过滤的输入点云进行变换
+    return ndt.getFinalTransformation();
+}
+
+bool rotate_center_caliber::solve()
+{
+    int size=m_match_datas.size();
+
+    if(size<=1)
+    {
+        LOG(INFO) <<"match_data size:"<< m_match_datas.size();
+        return false;
+    }
+
+    Eigen::MatrixXf A;
+    A.resize(2*size,2);
+    Eigen::MatrixXf B;
+    B.resize(2*size,1);
+
+    for(int i=0;i<size;i++)
+    {
+        match_transform data=m_match_datas[i];
+        float da=-data.theta;
+        float dx=-data.dx;
+        float dy=-data.dy;
+
+        float RA=1-cos(da);
+        float RB=sin(da);
+        float RC=-sin(da);
+        float RD=1-cos(da);
+
+        A(i*2+0,0)=RA;
+        A(i*2+0,1)=RB;
+        A(i*2+1,0)=RC;
+        A(i*2+1,1)=RD;
+
+        B(i*2+0,0)=dx;
+        B(i*2+1,0)=dy;
+
+    }
+    //std::cout<<"A:"<<A<<std::endl;
+    Eigen::MatrixXf result= (A.transpose()*A).inverse()*(A.transpose())*B;
+    //std::cout<<"result M:"<<result.transpose()<<std::endl;
+
+    m_rotation_center_x=result(0,0)+m_center.x;
+    m_rotation_center_y=result(1,0)+m_center.y;
+    return true;
+
+}
+void rotate_center_caliber::updata_points()
+{
+    if(m_match_datas.size()<1)
+        return ;
+    static int id = 0;
+    if(id==0)
+    {
+        // viewer.addLine(pcl::PointXYZ(m_center.x,m_center.y,0),
+        //         pcl::PointXYZ(m_rotation_center_x,m_rotation_center_y,0),0,1,1,"org");
+    }
+    double max_d=-1,min_d=1e9;
+    for (int i = 0; i < m_match_datas.size(); ++i)
+    {
+
+        float x = -m_match_datas[i].dx + m_center.x;
+        float y = -m_match_datas[i].dy + m_center.y;
+
+        char buf[32] = {0};
+        sprintf(buf, "line_%d", i);
+        // mut.lock();
+        // if (i < id)
+        // {
+        //     viewer.removeShape(buf);
+        // }
+
+        // viewer.addLine(pcl::PointXYZ(x, y, 0),
+        //                pcl::PointXYZ(m_rotation_center_x, m_rotation_center_y, 0),
+        //                0,
+        //                0,
+        //                1,
+        //                buf);
+
+        // mut.unlock();
+
+        double distance=sqrt(pow(m_rotation_center_x-x,2)+pow(m_rotation_center_y-y,2));
+        if(distance<min_d)
+            min_d=distance;
+        if(distance>max_d)
+            max_d=distance;
+
+    }
+    id = m_match_datas.size();
+    if((max_d-min_d)>0.01)
+    {
+        LOG(ERROR)<<" distances verify failed ";
+        return ;
+    }
+    LOG(INFO)<<" distances verify :"<<max_d-min_d;
+}

tool/rotate_center_caliber.h

@@ -0,0 +1,62 @@
+//
+// Created by zx on 2021/8/27.
+//
+
+#ifndef ROTATE_CANTER_CALIBER_HH
+#define ROTATE_CANTER_CALIBER_HH
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include "../velodyne_lidar/match3d/detect_ceres3d.h"
+#include <vector>
+
+#include "../tool/singleton.h"
+
+class rotate_center_caliber:public Singleton<rotate_center_caliber>
+{
+    typedef struct
+    {
+        float theta;
+        float dx;
+        float dy;
+    }match_transform;
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<rotate_center_caliber>;
+ public:
+
+  // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	rotate_center_caliber(const rotate_center_caliber&)=delete;
+	rotate_center_caliber& operator =(const rotate_center_caliber&)= delete;
+ 	~rotate_center_caliber()=default;
+
+   void rotate_center_caliber_init();
+   void rotate_center_caliber_uninit();
+
+   void set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model);
+
+   bool push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+
+   static Eigen::Matrix4f match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Matrix<float, 6, 1> init_transform);
+
+protected:
+   bool solve();
+   void updata_points();
+
+private:
+   // 父类的构造函数必须保护，子类的构造函数必须私有。
+	rotate_center_caliber()=default;
+
+ protected:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr             m_model_cloud;
+    std::vector<match_transform>                    m_match_datas;
+
+    detect_ceres3d*                                 mp_matcher;
+
+    //待解算的参数， 原点中心坐标和旋转中心坐标
+    pcl::PointXYZ                                   m_center;
+    float       m_rotation_center_x;
+    float       m_rotation_center_y;
+};
+
+
+#endif //ROTATE_CANTER_CALIBER_HH

tool/time_tool.cpp

@@ -0,0 +1,133 @@
+//
+// Created by wk on 2020/9/25.
+//
+
+#include "time_tool.h"
+Time_tool::Time_tool()
+{
+}
+Time_tool::~Time_tool()
+{
+	Time_tool_uninit();
+}
+void Time_tool::set_points_digits(int num)
+{
+	std::cout.precision(num);
+	std::cout.setf(std::ios::fixed);
+}
+std::chrono::system_clock::time_point Time_tool::get_system_point()
+{
+	return std::chrono::system_clock::now();
+}
+tm Time_tool::get_current_time_struct()
+{
+	auto now = std::chrono::system_clock::now();
+	time_t tt = std::chrono::system_clock::to_time_t(now);
+	tm time_tm=*localtime(&tt);
+	return time_tm;
+}
+std::string Time_tool::get_current_time_seconds()
+{
+	auto time_tm = get_current_time_struct();
+	char strTime[100] = "";
+	sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d", time_tm.tm_year + 1900,
+			time_tm.tm_mon + 1, time_tm.tm_mday, time_tm.tm_hour,
+			time_tm.tm_min, time_tm.tm_sec);
+	std::string str=strTime;
+	return str;
+}
+std::string Time_tool::get_current_time_millisecond()
+{
+
+	auto now = std::chrono::system_clock::now();
+	//通过不同精度获取相差的毫秒数
+	uint64_t dis_millseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count()
+							   - std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() * 1000;
+	std::string strTime=get_current_time_seconds()+" "+std::to_string((int)dis_millseconds);
+	return strTime;
+}
+std::string Time_tool::get_current_time_microsecond()
+{
+
+	auto now = std::chrono::system_clock::now();
+	//通过不同精度获取相差的微秒
+	uint64_t dis_microseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count()
+							   - std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() * 1000;
+	std::string strTime=get_current_time_millisecond()+" "+std::to_string((int)dis_microseconds);
+	return strTime;
+}
+void Time_tool::time_start(int key)
+{
+	timetool_map[key].t_time_start=get_system_point();//保存开始的时间 //单位为微秒
+}
+
+void Time_tool::time_end(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		timetool_map[key].t_time_end = get_system_point();//保存结束的时间
+		timetool_map[key].t_time_difference = timetool_map[key].t_time_end - timetool_map[key].t_time_start;//保存时差
+	}
+	else
+	{
+	    std::cout << "计时器：" << key<<"还未开始"<<std::endl;
+	}
+
+}
+void Time_tool::cout_time_seconds(int key)
+{
+
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000000000;
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_millisecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000000;
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 毫秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_microsecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		double dieoutTime=(double)timetool_map[key].t_time_difference.count()/1000;
+
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<dieoutTime<<" 微秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::cout_time_nanosecond(int key)
+{
+	if ( timetool_map.find(key)!=timetool_map.end() )
+	{
+		std::cout << "计时器:"<<key<<" 计时的时间为：" <<timetool_map[key].t_time_difference.count()<<" 纳秒" << std::endl;
+	}
+	else
+	{
+		std::cout<<"没有此计时器:"<<key<<std::endl;
+	}
+}
+void Time_tool::clear_timer()
+{
+	Time_tool_uninit();
+}
+void Time_tool::Time_tool_uninit()
+{
+	timetool_map.clear();
+}

tool/time_tool.h

@@ -0,0 +1,81 @@
+//
+// Created by wk on 2020/9/25.
+//
+
+#ifndef PKGNAME_TIME_TOOL_H
+#define PKGNAME_TIME_TOOL_H
+
+#include <thread>
+#include <map>
+#include <iostream>
+#include<time.h>
+#include <queue>
+#include "./singleton.h"
+
+class Time_tool:public Singleton<Time_tool>
+{
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+   friend class Singleton<Time_tool>;
+struct time_data
+{
+	std::chrono::system_clock::time_point		t_time_start;//计时开始
+	std::chrono::system_clock::time_point 		t_time_end;//计时结束
+	std::chrono::system_clock::duration			t_time_difference;//时差
+};
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+   Time_tool();
+public:
+    //必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Time_tool(const Time_tool& other) = delete;
+    Time_tool& operator =(const Time_tool& other) = delete;
+    ~Time_tool();
+public://API functions
+	void Time_tool_uninit();
+
+    //获取当前系统时间点
+	std::chrono::system_clock::time_point get_system_point();
+
+	//设置输出格式  保留小数点后几位  不设置默认为系统输出
+	void set_points_digits(int);
+
+	//获取当前时间 精确到秒
+	 std::string get_current_time_seconds();
+
+	//获取当前时间 精确到毫秒
+	std::string get_current_time_millisecond();
+
+	//获取当前时间 精确到微妙
+	std::string get_current_time_microsecond();
+
+	//获取当前时间结构体
+	tm get_current_time_struct();
+
+	//计时开始
+	void time_start(int key=1);
+
+	//指定计时器计时结束
+	void time_end(int key=1);
+
+	//打印计时时差 精确到秒
+	void cout_time_seconds(int key=1);
+
+	//打印计时时差 精确到毫秒
+	void cout_time_millisecond(int key=1);
+
+	//打印计时时差 精确到微秒
+	void cout_time_microsecond(int key=1);
+
+	//打印计时时差 精确到纳秒
+	void cout_time_nanosecond(int key=1);
+
+	//清除计时器
+	void clear_timer();
+public://get or set member variable
+protected://member variable
+
+    
+private:
+	std::map<int,time_data> 			timetool_map;
+};
+#endif //PKGNAME_TIME_TOOL_H

velodyne_lidar/car_pose_detector.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-09-16 15:18:34
- * @LastEditTime: 2021-09-22 17:26:17
+ * @LastEditTime: 2021-09-24 17:55:03
  * @LastEditors: yct
  */
 

velodyne_lidar/ground_region.cpp

@@ -228,8 +228,6 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
         return false;
     }
 
-    if (cloud_filtered->size() == 0)
-        return false;
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
     sor.setInputCloud(cloud_filtered);                 //设置待滤波的点云
     sor.setMeanK(5);                                   //设置在进行统计时考虑的临近点个数
@@ -329,9 +327,11 @@ bool computer_var(std::vector<double> data, double &var)
 
 Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result)
 {
+    // 1.点云合法性检验
     if (cloud->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");
 
+    // 2.点云预处理
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -342,15 +342,18 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_filtered->push_back(pt);
         }
     }
-    //离群点过滤
+        //离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_filtered);
     sor.setMeanK(15);            //K近邻搜索点个数
     sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
-    sor.filter(*cloud_filtered);      //保存滤波结果到cloud_filter
+    sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
 
-    //下采样
+    if (cloud_filtered->size() == 0)
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
+
+        //下采样
     pcl::ApproximateVoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
     vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
     vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
@@ -359,19 +362,21 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     if (cloud_filtered->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
 
-    // 更新过滤点
+        // 更新过滤点
     m_filtered_cloud_mutex.lock();
     mp_cloud_filtered->clear();
     mp_cloud_filtered->operator+=(*cloud_filtered);
     m_filtered_cloud_mutex.unlock();
 
-    // z detect
+    // 3.位姿优化，获取中心xy与角度
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
     double x, y, theta, width, z_value=0.2;
     if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, x, y, theta, width, z_value, false))
     {
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find chassis z value failed.");
     }
+    std::cout << "cx: " << x+m_region.plc_offsetx() << ", cy: " << y+m_region.plc_offsety() << ", theta: " << theta*180.0/M_PI << std::endl;
+
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_bowl = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
 
@@ -393,6 +398,9 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
     // vox.filter(*cloud_z_solver);         //执行滤波处理，存储输出
 
+    if (cloud_filtered->size() == 0)
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
+
     x = 0.0;
     width = 1.0;
     // Error_manager ec = t_solver.solve(cloud_z_solver, x, mid_z, width, height);
@@ -405,11 +413,12 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     pass.setFilterLimits(m_region.minz(), mid_z + height / 2.0);
     pass.setFilterLimitsNegative(false);
     pass.filter(*cloud_z_solver);
-    for (double i = -3.0; i < 3.0; i+=0.02)
-    {
-        cloud_z_solver->push_back(pcl::PointXYZ(-width/2.0, i, 0));
-        cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0));
-    }
+    // // 车宽方向画线
+    // for (double i = -3.0; i < 3.0; i+=0.02)
+    // {
+    //     cloud_z_solver->push_back(pcl::PointXYZ(-width/2.0, i, 0));
+    //     cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0));
+    // }
     // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << std::endl;
     std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_block = std::chrono::duration_cast<std::chrono::duration<double>>(t3 - t2);
@@ -476,7 +485,12 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     if (results.size() == 0)
     {
-        std::cout << "\n-------- no result: " << std::endl;
+        // changed by yct, save 3d wheel detect result.
+        static int save_debug = 0;
+        if (save_debug++ == 2)
+            m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
+
+        // std::cout << "\n-------- no result: " << std::endl;
         return Error_manager(FAILED, NORMAL, "no car detected");
     }
     ///  to be
@@ -502,18 +516,42 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //        center_z, last_result.theta, last_result.front_theta, last_result.wheel_base, last_result.width,
     //        min_mean_loss);
     // std::cout << "\n-------- final z: " << chassis_z << std::endl;
-    // std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
-    //           << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl;
+    std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
+              << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;
 
     // last_result.cx -= x;
     // last_result.cy -= y;
     // last_result.theta -= theta;
 
-    // // changed by yct, save 3d wheel detect result.
-    // m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
     return SUCCESS;
 }
 
+int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx, double plate_cy, double theta)
+{
+    if(cloud->size() <= 0)
+        return Range_status::Range_correct;
+    int res = Range_status::Range_correct;
+    double theta_rad = theta * M_PI / 180.0;
+    pcl::PointCloud<pcl::PointXYZ> t_cloud;
+    for (size_t i = 0; i < cloud->size(); i++)
+    {
+        Eigen::Vector2d t_point(cloud->points[i].x - plate_cx, cloud->points[i].y - plate_cy);
+        pcl::PointXYZ t_pcl_point;
+        t_pcl_point.x = (t_point.x() * cos(theta_rad) - t_point.y() * sin(theta_rad)) + plate_cx;
+        t_pcl_point.y = (t_point.x() * sin(theta_rad) + t_point.y() * cos(theta_rad)) + plate_cy;
+        t_pcl_point.z = cloud->points[i].z;
+        t_cloud.push_back(t_pcl_point);
+    }
+    pcl::PointXYZ min_p, max_p;
+    pcl::getMinMax3D(t_cloud, min_p, max_p);
+    if(min_p.x < m_region.border_minx())
+        res |= Range_status::Range_left;
+    if(max_p.x > m_region.border_maxx())
+        res |= Range_status::Range_right;
+
+    return res;
+}
+
 //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
 Error_manager Ground_region::get_current_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time)
 {
@@ -525,6 +563,7 @@ Error_manager Ground_region::get_current_wheel_information(Common_data::Car_whee
 	//获取指令时间之后的信息, 如果没有就会报错, 不会等待
 	if( m_detect_update_time > command_time )
 	{
+        std::lock_guard<std::mutex> lck(m_car_result_mutex);
 		*p_car_wheel_information = m_car_wheel_information;
 		if(m_car_wheel_information.correctness)
 		    return Error_code::SUCCESS;
@@ -551,7 +590,8 @@ Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_i
 //                <<std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_cloud_collection_time).count()/1000.0;
 	if( m_detect_update_time > command_time - std::chrono::milliseconds(GROUND_REGION_DETECT_CYCLE_MS))
 	{
-		*p_car_wheel_information = m_car_wheel_information;
+        std::lock_guard<std::mutex> lck(m_car_result_mutex);
+        *p_car_wheel_information = m_car_wheel_information;
         if(m_car_wheel_information.correctness)
             return Error_code::SUCCESS;
         else
@@ -600,19 +640,30 @@ void Ground_region::thread_measure_func()
             detect_wheel_ceres3d::Detect_result t_result;
 
             Error_manager ec = detect(mp_cloud_collection, t_result);
-            m_detect_update_time = std::chrono::system_clock::now();
-
-            m_car_wheel_information.center_x = t_result.cx;
-            m_car_wheel_information.center_y = t_result.cy;
-            m_car_wheel_information.car_angle = t_result.theta;
-            m_car_wheel_information.wheel_base = t_result.wheel_base;
-            m_car_wheel_information.wheel_width = t_result.width;
-            m_car_wheel_information.front_theta = t_result.front_theta;
+            
+            std::lock_guard<std::mutex> lck(m_car_result_mutex);
             if (ec == SUCCESS)
             {
                 m_car_wheel_information.correctness = true;
+                m_car_wheel_information.car_center_x = t_result.cx;
+                m_car_wheel_information.car_center_y = t_result.cy;
+                m_car_wheel_information.car_angle = t_result.theta;
+                m_car_wheel_information.car_wheel_base = t_result.wheel_base;
+                m_car_wheel_information.car_wheel_width = t_result.width;
+                m_car_wheel_information.car_front_theta = t_result.front_theta;
+                m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy());
+                // 添加plc偏移
+                m_car_wheel_information.car_center_x += m_region.plc_offsetx();
+                m_car_wheel_information.car_center_y += m_region.plc_offsety();
+                // 超界校验
+                {
+                    std::lock_guard<std::mutex> lck(m_filtered_cloud_mutex);
+                    int res = outOfRangeDetection(mp_cloud_filtered, m_region.turnplate_cx(), m_region.turnplate_cy(), 90.0 - t_result.theta);
+                    m_car_wheel_information.range_status = res;
+                }
+
                 Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
-				t_wheel_info_stamped.wheel_data = m_car_wheel_information;
+                t_wheel_info_stamped.wheel_data = m_car_wheel_information;
 				t_wheel_info_stamped.measure_time = m_detect_update_time;
 				Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
             }
@@ -621,6 +672,7 @@ void Ground_region::thread_measure_func()
                 m_car_wheel_information.correctness = false;
                 // LOG(ERROR) << ec.to_string();
             }
+            m_detect_update_time = std::chrono::system_clock::now();
         }
         m_region_status = E_READY;
     }

velodyne_lidar/ground_region.h

@@ -45,6 +45,13 @@ public:
 
       E_FAULT = 10, //故障
    };
+   enum Range_status
+   {
+      Range_correct = 0x0000, // 未超界
+      Range_left = 0x0001,    // 左超界
+      Range_right = 0x0002,   // 右超界
+   };
+
 #define GROUND_REGION_DETECT_CYCLE_MS 150
 
    Ground_region();
@@ -137,6 +144,8 @@ private:
 
    bool isRect(std::vector<cv::Point2f> &points);
 
+   // 超界判断，旋转单位(deg)，默认无旋转
+   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);
 
 private:
    velodyne::Region m_region; // 区域配置
@@ -155,10 +164,11 @@ private:
    pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_filtered;      // 区域切除后的点
 
    std::mutex	 								      m_detect_z_cloud_mutex;       // 点云更新互斥锁, 内存由上级管理
-   pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_detect_z;      // 区域切除后的点
+   pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_detect_z;      // 底盘切除后的点
 
    Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
 	std::chrono::system_clock::time_point		m_detect_update_time;			//定位结果的更新时间.
+   std::mutex m_car_result_mutex; // 测量结果互斥锁
 };
 
 #endif //LIDARMEASURE__GROUD_REGION_HPP_

velodyne_lidar/match3d/detect_ceres3d.cpp

@@ -0,0 +1,236 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_ceres3d.h"
+#include "interpolated_grid.hpp"
+#include <pcl/common/transforms.h>
+
+class CostFunctor3d
+{
+ private:
+    const InterpolatedProbabilityGrid m_interpolated_grid;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //左前点云
+
+ public:
+    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                  const HybridGrid& interpolated_grid)
+                  :m_interpolated_grid(interpolated_grid),m_cloud(cloud)
+    {
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const
+    {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //
+        int right_rear_num = m_cloud->size();
+        for (int i = 0; i < m_cloud->size(); ++i)
+        {
+            //先平移后旋转
+            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)+cx),
+                                               (T(m_cloud->points[i].y)+cy));
+            //绕原点旋转
+            const Eigen::Matrix<T, 2, 1> rotate_point = rotation_matrix * point;
+
+            residual[i] = T(1.0) -
+                    m_interpolated_grid.GetInterpolatedValue(rotate_point[0],rotate_point[1],
+                                                                  T(m_cloud->points[i].z));
+        }
+        return true;
+    }
+
+};
+
+
+detect_ceres3d::detect_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr model_grid_cloud)
+{
+
+
+
+    float del_x=0.05;
+    float del_y=0.03;
+    float del_z=0.05;
+    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
+    delta(0,0)=del_x*del_x;
+    delta(1,1)=del_y*del_y;
+    delta(2,2)=del_z*del_z;
+    Eigen::Matrix3f delta_inv=delta.inverse();
+
+
+    Eigen::Vector3f d(0,0,0.02);
+    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
+    float cut_p=exp(ce(0,0));
+
+    float resolution=0.02;
+    mp_model_grid=new HybridGrid(resolution);
+
+
+    for(int i=0;i<model_grid_cloud->size();++i)
+    {
+        pcl::PointXYZ pt=model_grid_cloud->points[i];
+        Eigen::Vector3f u(pt.x,pt.y,pt.z);
+
+        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_model_grid->resolution())
+        {
+            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_model_grid->resolution())
+            {
+                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_model_grid->resolution())
+                {
+                    Eigen::Vector3f p(x,y,z);
+                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
+                    float prob=exp(B(0,0));
+                    if(prob>cut_p)
+                        prob=cut_p;
+                    Eigen::Array3i index=mp_model_grid->GetCellIndex(p);
+                    const Eigen::Array3i shifted_index = index+ (mp_model_grid->grid_size() >> 1);
+                    if ((shifted_index.cast<unsigned int>() >= mp_model_grid->grid_size()).any())
+                    {
+                    	std::cout<<index.transpose()<<","<<u.transpose()<<std::endl;
+                    }
+                    if(mp_model_grid->GetProbability(index)<prob)
+                        mp_model_grid->SetProbability(index,prob);
+
+                }
+            }
+        }
+
+    }
+
+    save_model("./");
+
+}
+
+#include "point_tool.h"
+void detect_ceres3d::save_model(std::string dir)
+{
+
+    InterpolatedProbabilityGrid inter_grid(*mp_model_grid);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for(int x=0;x<mp_model_grid->grid_size();++x)
+    {
+        for(int y=0;y<mp_model_grid->grid_size();++y)
+        {
+            for(int z=0;z<mp_model_grid->grid_size();++z)
+            {
+                Eigen::Array3i index(x-mp_model_grid->grid_size()/2,
+                        y-mp_model_grid->grid_size()/2,z-mp_model_grid->grid_size()/2);
+                Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);
+
+                float prob=inter_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
+                if(prob>0.01)
+                {
+                    Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);
+                    int rgb=int((prob-0.01)*255);
+                    pcl::PointXYZRGB point;
+                    point.x=pt[0];
+                    point.y=pt[1];
+                    point.z=pt[2];
+                    point.r=rgb;
+                    point.g=0;
+                    point.b=255-rgb;
+                    cloud_grid->push_back(point);
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"  save model :"<<dir<<std::endl;
+    save_cloud_txt(cloud_grid,dir+"/model.txt");
+
+}
+
+detect_ceres3d::~detect_ceres3d()
+{
+    delete mp_model_grid;
+    mp_model_grid= nullptr;
+}
+
+
+
+bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+        Detect_result &detect_result, std::string &error_info)
+{
+
+
+    error_info = "";
+
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+
+
+    double variable[] = {cx, cy, init_theta};
+    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    CostFunctor3d *cost_func = new CostFunctor3d(cloud,*mp_model_grid);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 3>(
+            cost_func,cloud->size());
+    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps=false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    //options.logging_type = ceres::LoggingType::SILENT;
+    options.max_num_iterations=500;
+    options.num_threads=2;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    double loss=summary.final_cost/(cloud->size());
+
+    if(loss>0.05)
+    {
+        char buf[64]={0};
+        sprintf(buf," loss : %.5f  > 0.05",loss);
+        error_info=buf;
+        return false;
+    }
+
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=variable[2];
+
+    return true;
+
+}
+
+bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Detect_result &detect_result,
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,std::string &error_info)
+{
+
+    if(detect(cloud,detect_result,error_info))
+    {
+
+        Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+        pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud0(new pcl::PointCloud<pcl::PointXYZ>());
+        transform_2.translation() << detect_result.cx, detect_result.cy, 0;
+        pcl::transformPointCloud(*cloud, *transform_cloud0, transform_2);
+
+        transform_2=Eigen::Affine3f::Identity();
+        transform_2.rotate(Eigen::AngleAxisf(detect_result.theta, Eigen::Vector3f::UnitZ()));
+        pcl::transformPointCloud(*transform_cloud0, *transformed_cloud, transform_2);
+
+
+        return true;
+    }
+    std::cout << "error:    " << error_info << std::endl;
+    return false;
+}

velodyne_lidar/match3d/detect_ceres3d.h

@@ -0,0 +1,75 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef DETECT_CERES_3D_HH
+#define DETECT_CERES_3D_HH
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+#include <string>
+#include <chrono>
+#include "boost/format.hpp"
+#include "hybrid_grid.hpp"
+
+class detect_ceres3d {
+public:
+    struct Loss_result
+    {
+        public: 
+        double lf_loss;
+        double rf_loss;
+        double lb_loss;
+        double rb_loss;
+        double total_avg_loss;
+        Loss_result& operator=(const Loss_result& loss_result)
+        {
+            this->lf_loss = loss_result.lf_loss;
+            this->rf_loss = loss_result.rf_loss;
+            this->lb_loss = loss_result.lb_loss;
+            this->rb_loss = loss_result.rb_loss;
+            this->total_avg_loss = loss_result.total_avg_loss;
+            return *this;
+        }
+    };
+    struct Detect_result
+    {
+     public:
+        double cx;
+        double cy;
+        double theta;
+        bool success;
+        Loss_result loss;
+        Detect_result& operator=(const Detect_result& detect_result)
+        {
+            this->cx = detect_result.cx;
+            this->cy = detect_result.cy;
+            this->theta = detect_result.theta;
+            this->success = detect_result.success;
+            this->loss=detect_result.loss;
+            return *this;
+        }
+    };
+    detect_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr model_grid_cloud);
+    ~detect_ceres3d();
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Detect_result &detect_result, std::string &error_info);
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Detect_result &detect_result,
+                pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,std::string &error_info);
+
+protected:
+    void save_model(std::string file);
+
+ private:
+    HybridGrid*                                  mp_model_grid;
+
+
+
+};
+
+
+#endif //DETECT_CERES_3D_HH

velodyne_lidar/velodyne_config.pb.cc

@@ -249,6 +249,12 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, maxz_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, region_id_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, lidar_exts_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, turnplate_cx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, turnplate_cy_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, border_minx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, border_maxx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_offsetx_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_offsety_),
   0,
   1,
   2,
@@ -257,13 +263,19 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   5,
   6,
   ~0u,
+  7,
+  8,
+  9,
+  10,
+  11,
+  12,
 };
 static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
   { 0, 12, sizeof(::velodyne::velodyneManagerParams)},
   { 19, 35, sizeof(::velodyne::velodyneLidarParams)},
   { 46, 57, sizeof(::velodyne::CalibParameter)},
   { 63, 70, sizeof(::velodyne::lidarExtrinsic)},
-  { 72, 85, sizeof(::velodyne::Region)},
+  { 72, 91, sizeof(::velodyne::Region)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -314,14 +326,18 @@ void AddDescriptorsImpl() {
       "\002:\0010\022\014\n\001p\030\002 \001(\002:\0010\022\014\n\001y\030\003 \001(\002:\0010\022\r\n\002cx\030\004"
       " \001(\002:\0010\022\r\n\002cy\030\005 \001(\002:\0010\022\r\n\002cz\030\006 \001(\002:\0010\"K\n"
       "\016lidarExtrinsic\022\020\n\010lidar_id\030\001 \002(\005\022\'\n\005cal"
-      "ib\030\002 \001(\0132\030.velodyne.CalibParameter\"\235\001\n\006R"
+      "ib\030\002 \001(\0132\030.velodyne.CalibParameter\"\235\002\n\006R"
       "egion\022\014\n\004minx\030\001 \002(\002\022\014\n\004maxx\030\002 \002(\002\022\014\n\004min"
       "y\030\003 \002(\002\022\014\n\004maxy\030\004 \002(\002\022\014\n\004minz\030\005 \002(\002\022\014\n\004m"
       "axz\030\006 \002(\002\022\021\n\tregion_id\030\007 \002(\005\022,\n\nlidar_ex"
-      "ts\030\010 \003(\0132\030.velodyne.lidarExtrinsic"
+      "ts\030\010 \003(\0132\030.velodyne.lidarExtrinsic\022\024\n\014tu"
+      "rnplate_cx\030\t \002(\002\022\024\n\014turnplate_cy\030\n \002(\002\022\023"
+      "\n\013border_minx\030\013 \002(\002\022\023\n\013border_maxx\030\014 \002(\002"
+      "\022\023\n\013plc_offsetx\030\r \002(\002\022\023\n\013plc_offsety\030\016 \002"
+      "(\002"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 914);
+      descriptor, 1042);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "velodyne_config.proto", &protobuf_RegisterTypes);
 }
@@ -2408,6 +2424,12 @@ const int Region::kMinzFieldNumber;
 const int Region::kMaxzFieldNumber;
 const int Region::kRegionIdFieldNumber;
 const int Region::kLidarExtsFieldNumber;
+const int Region::kTurnplateCxFieldNumber;
+const int Region::kTurnplateCyFieldNumber;
+const int Region::kBorderMinxFieldNumber;
+const int Region::kBorderMaxxFieldNumber;
+const int Region::kPlcOffsetxFieldNumber;
+const int Region::kPlcOffsetyFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 Region::Region()
@@ -2426,16 +2448,16 @@ Region::Region(const Region& from)
       lidar_exts_(from.lidar_exts_) {
   _internal_metadata_.MergeFrom(from._internal_metadata_);
   ::memcpy(&minx_, &from.minx_,
-    static_cast<size_t>(reinterpret_cast<char*>(&region_id_) -
-    reinterpret_cast<char*>(&minx_)) + sizeof(region_id_));
+    static_cast<size_t>(reinterpret_cast<char*>(&plc_offsety_) -
+    reinterpret_cast<char*>(&minx_)) + sizeof(plc_offsety_));
   // @@protoc_insertion_point(copy_constructor:velodyne.Region)
 }
 
 void Region::SharedCtor() {
   _cached_size_ = 0;
   ::memset(&minx_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&region_id_) -
-      reinterpret_cast<char*>(&minx_)) + sizeof(region_id_));
+      reinterpret_cast<char*>(&plc_offsety_) -
+      reinterpret_cast<char*>(&minx_)) + sizeof(plc_offsety_));
 }
 
 Region::~Region() {
@@ -2477,10 +2499,15 @@ void Region::Clear() {
 
   lidar_exts_.Clear();
   cached_has_bits = _has_bits_[0];
-  if (cached_has_bits & 127u) {
+  if (cached_has_bits & 255u) {
     ::memset(&minx_, 0, static_cast<size_t>(
-        reinterpret_cast<char*>(&region_id_) -
-        reinterpret_cast<char*>(&minx_)) + sizeof(region_id_));
+        reinterpret_cast<char*>(&turnplate_cx_) -
+        reinterpret_cast<char*>(&minx_)) + sizeof(turnplate_cx_));
+  }
+  if (cached_has_bits & 7936u) {
+    ::memset(&turnplate_cy_, 0, static_cast<size_t>(
+        reinterpret_cast<char*>(&plc_offsety_) -
+        reinterpret_cast<char*>(&turnplate_cy_)) + sizeof(plc_offsety_));
   }
   _has_bits_.Clear();
   _internal_metadata_.Clear();
@@ -2605,6 +2632,90 @@ bool Region::MergePartialFromCodedStream(
         break;
       }
 
+      // required float turnplate_cx = 9;
+      case 9: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(77u /* 77 & 0xFF */)) {
+          set_has_turnplate_cx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &turnplate_cx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float turnplate_cy = 10;
+      case 10: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(85u /* 85 & 0xFF */)) {
+          set_has_turnplate_cy();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &turnplate_cy_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float border_minx = 11;
+      case 11: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(93u /* 93 & 0xFF */)) {
+          set_has_border_minx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &border_minx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float border_maxx = 12;
+      case 12: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(101u /* 101 & 0xFF */)) {
+          set_has_border_maxx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &border_maxx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float plc_offsetx = 13;
+      case 13: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(109u /* 109 & 0xFF */)) {
+          set_has_plc_offsetx();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &plc_offsetx_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float plc_offsety = 14;
+      case 14: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(117u /* 117 & 0xFF */)) {
+          set_has_plc_offsety();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &plc_offsety_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -2674,6 +2785,36 @@ void Region::SerializeWithCachedSizes(
       8, this->lidar_exts(static_cast<int>(i)), output);
   }
 
+  // required float turnplate_cx = 9;
+  if (cached_has_bits & 0x00000080u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(9, this->turnplate_cx(), output);
+  }
+
+  // required float turnplate_cy = 10;
+  if (cached_has_bits & 0x00000100u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(10, this->turnplate_cy(), output);
+  }
+
+  // required float border_minx = 11;
+  if (cached_has_bits & 0x00000200u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(11, this->border_minx(), output);
+  }
+
+  // required float border_maxx = 12;
+  if (cached_has_bits & 0x00000400u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(12, this->border_maxx(), output);
+  }
+
+  // required float plc_offsetx = 13;
+  if (cached_has_bits & 0x00000800u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(13, this->plc_offsetx(), output);
+  }
+
+  // required float plc_offsety = 14;
+  if (cached_has_bits & 0x00001000u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->plc_offsety(), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -2732,6 +2873,36 @@ void Region::SerializeWithCachedSizes(
         8, this->lidar_exts(static_cast<int>(i)), deterministic, target);
   }
 
+  // required float turnplate_cx = 9;
+  if (cached_has_bits & 0x00000080u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(9, this->turnplate_cx(), target);
+  }
+
+  // required float turnplate_cy = 10;
+  if (cached_has_bits & 0x00000100u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(10, this->turnplate_cy(), target);
+  }
+
+  // required float border_minx = 11;
+  if (cached_has_bits & 0x00000200u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(11, this->border_minx(), target);
+  }
+
+  // required float border_maxx = 12;
+  if (cached_has_bits & 0x00000400u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(12, this->border_maxx(), target);
+  }
+
+  // required float plc_offsetx = 13;
+  if (cached_has_bits & 0x00000800u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(13, this->plc_offsetx(), target);
+  }
+
+  // required float plc_offsety = 14;
+  if (cached_has_bits & 0x00001000u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->plc_offsety(), target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -2781,6 +2952,36 @@ size_t Region::RequiredFieldsByteSizeFallback() const {
         this->region_id());
   }
 
+  if (has_turnplate_cx()) {
+    // required float turnplate_cx = 9;
+    total_size += 1 + 4;
+  }
+
+  if (has_turnplate_cy()) {
+    // required float turnplate_cy = 10;
+    total_size += 1 + 4;
+  }
+
+  if (has_border_minx()) {
+    // required float border_minx = 11;
+    total_size += 1 + 4;
+  }
+
+  if (has_border_maxx()) {
+    // required float border_maxx = 12;
+    total_size += 1 + 4;
+  }
+
+  if (has_plc_offsetx()) {
+    // required float plc_offsetx = 13;
+    total_size += 1 + 4;
+  }
+
+  if (has_plc_offsety()) {
+    // required float plc_offsety = 14;
+    total_size += 1 + 4;
+  }
+
   return total_size;
 }
 size_t Region::ByteSizeLong() const {
@@ -2792,7 +2993,7 @@ size_t Region::ByteSizeLong() const {
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
         _internal_metadata_.unknown_fields());
   }
-  if (((_has_bits_[0] & 0x0000007f) ^ 0x0000007f) == 0) {  // All required fields are present.
+  if (((_has_bits_[0] & 0x00001fff) ^ 0x00001fff) == 0) {  // All required fields are present.
     // required float minx = 1;
     total_size += 1 + 4;
 
@@ -2816,6 +3017,24 @@ size_t Region::ByteSizeLong() const {
       ::google::protobuf::internal::WireFormatLite::Int32Size(
         this->region_id());
 
+    // required float turnplate_cx = 9;
+    total_size += 1 + 4;
+
+    // required float turnplate_cy = 10;
+    total_size += 1 + 4;
+
+    // required float border_minx = 11;
+    total_size += 1 + 4;
+
+    // required float border_maxx = 12;
+    total_size += 1 + 4;
+
+    // required float plc_offsetx = 13;
+    total_size += 1 + 4;
+
+    // required float plc_offsety = 14;
+    total_size += 1 + 4;
+
   } else {
     total_size += RequiredFieldsByteSizeFallback();
   }
@@ -2861,7 +3080,7 @@ void Region::MergeFrom(const Region& from) {
 
   lidar_exts_.MergeFrom(from.lidar_exts_);
   cached_has_bits = from._has_bits_[0];
-  if (cached_has_bits & 127u) {
+  if (cached_has_bits & 255u) {
     if (cached_has_bits & 0x00000001u) {
       minx_ = from.minx_;
     }
@@ -2883,6 +3102,27 @@ void Region::MergeFrom(const Region& from) {
     if (cached_has_bits & 0x00000040u) {
       region_id_ = from.region_id_;
     }
+    if (cached_has_bits & 0x00000080u) {
+      turnplate_cx_ = from.turnplate_cx_;
+    }
+    _has_bits_[0] |= cached_has_bits;
+  }
+  if (cached_has_bits & 7936u) {
+    if (cached_has_bits & 0x00000100u) {
+      turnplate_cy_ = from.turnplate_cy_;
+    }
+    if (cached_has_bits & 0x00000200u) {
+      border_minx_ = from.border_minx_;
+    }
+    if (cached_has_bits & 0x00000400u) {
+      border_maxx_ = from.border_maxx_;
+    }
+    if (cached_has_bits & 0x00000800u) {
+      plc_offsetx_ = from.plc_offsetx_;
+    }
+    if (cached_has_bits & 0x00001000u) {
+      plc_offsety_ = from.plc_offsety_;
+    }
     _has_bits_[0] |= cached_has_bits;
   }
 }
@@ -2902,7 +3142,7 @@ void Region::CopyFrom(const Region& from) {
 }
 
 bool Region::IsInitialized() const {
-  if ((_has_bits_[0] & 0x0000007f) != 0x0000007f) return false;
+  if ((_has_bits_[0] & 0x00001fff) != 0x00001fff) return false;
   if (!::google::protobuf::internal::AllAreInitialized(this->lidar_exts())) return false;
   return true;
 }
@@ -2921,6 +3161,12 @@ void Region::InternalSwap(Region* other) {
   swap(minz_, other->minz_);
   swap(maxz_, other->maxz_);
   swap(region_id_, other->region_id_);
+  swap(turnplate_cx_, other->turnplate_cx_);
+  swap(turnplate_cy_, other->turnplate_cy_);
+  swap(border_minx_, other->border_minx_);
+  swap(border_maxx_, other->border_maxx_);
+  swap(plc_offsetx_, other->plc_offsetx_);
+  swap(plc_offsety_, other->plc_offsety_);
   swap(_has_bits_[0], other->_has_bits_[0]);
   _internal_metadata_.Swap(&other->_internal_metadata_);
   swap(_cached_size_, other->_cached_size_);

velodyne_lidar/velodyne_config.pb.h

@@ -961,6 +961,48 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   ::google::protobuf::int32 region_id() const;
   void set_region_id(::google::protobuf::int32 value);
 
+  // required float turnplate_cx = 9;
+  bool has_turnplate_cx() const;
+  void clear_turnplate_cx();
+  static const int kTurnplateCxFieldNumber = 9;
+  float turnplate_cx() const;
+  void set_turnplate_cx(float value);
+
+  // required float turnplate_cy = 10;
+  bool has_turnplate_cy() const;
+  void clear_turnplate_cy();
+  static const int kTurnplateCyFieldNumber = 10;
+  float turnplate_cy() const;
+  void set_turnplate_cy(float value);
+
+  // required float border_minx = 11;
+  bool has_border_minx() const;
+  void clear_border_minx();
+  static const int kBorderMinxFieldNumber = 11;
+  float border_minx() const;
+  void set_border_minx(float value);
+
+  // required float border_maxx = 12;
+  bool has_border_maxx() const;
+  void clear_border_maxx();
+  static const int kBorderMaxxFieldNumber = 12;
+  float border_maxx() const;
+  void set_border_maxx(float value);
+
+  // required float plc_offsetx = 13;
+  bool has_plc_offsetx() const;
+  void clear_plc_offsetx();
+  static const int kPlcOffsetxFieldNumber = 13;
+  float plc_offsetx() const;
+  void set_plc_offsetx(float value);
+
+  // required float plc_offsety = 14;
+  bool has_plc_offsety() const;
+  void clear_plc_offsety();
+  static const int kPlcOffsetyFieldNumber = 14;
+  float plc_offsety() const;
+  void set_plc_offsety(float value);
+
   // @@protoc_insertion_point(class_scope:velodyne.Region)
  private:
   void set_has_minx();
@@ -977,6 +1019,18 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   void clear_has_maxz();
   void set_has_region_id();
   void clear_has_region_id();
+  void set_has_turnplate_cx();
+  void clear_has_turnplate_cx();
+  void set_has_turnplate_cy();
+  void clear_has_turnplate_cy();
+  void set_has_border_minx();
+  void clear_has_border_minx();
+  void set_has_border_maxx();
+  void clear_has_border_maxx();
+  void set_has_plc_offsetx();
+  void clear_has_plc_offsetx();
+  void set_has_plc_offsety();
+  void clear_has_plc_offsety();
 
   // helper for ByteSizeLong()
   size_t RequiredFieldsByteSizeFallback() const;
@@ -992,6 +1046,12 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   float minz_;
   float maxz_;
   ::google::protobuf::int32 region_id_;
+  float turnplate_cx_;
+  float turnplate_cy_;
+  float border_minx_;
+  float border_maxx_;
+  float plc_offsetx_;
+  float plc_offsety_;
   friend struct ::protobuf_velodyne_5fconfig_2eproto::TableStruct;
   friend void ::protobuf_velodyne_5fconfig_2eproto::InitDefaultsRegionImpl();
 };
@@ -2189,6 +2249,150 @@ Region::lidar_exts() const {
   return lidar_exts_;
 }
 
+// required float turnplate_cx = 9;
+inline bool Region::has_turnplate_cx() const {
+  return (_has_bits_[0] & 0x00000080u) != 0;
+}
+inline void Region::set_has_turnplate_cx() {
+  _has_bits_[0] |= 0x00000080u;
+}
+inline void Region::clear_has_turnplate_cx() {
+  _has_bits_[0] &= ~0x00000080u;
+}
+inline void Region::clear_turnplate_cx() {
+  turnplate_cx_ = 0;
+  clear_has_turnplate_cx();
+}
+inline float Region::turnplate_cx() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.turnplate_cx)
+  return turnplate_cx_;
+}
+inline void Region::set_turnplate_cx(float value) {
+  set_has_turnplate_cx();
+  turnplate_cx_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.turnplate_cx)
+}
+
+// required float turnplate_cy = 10;
+inline bool Region::has_turnplate_cy() const {
+  return (_has_bits_[0] & 0x00000100u) != 0;
+}
+inline void Region::set_has_turnplate_cy() {
+  _has_bits_[0] |= 0x00000100u;
+}
+inline void Region::clear_has_turnplate_cy() {
+  _has_bits_[0] &= ~0x00000100u;
+}
+inline void Region::clear_turnplate_cy() {
+  turnplate_cy_ = 0;
+  clear_has_turnplate_cy();
+}
+inline float Region::turnplate_cy() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.turnplate_cy)
+  return turnplate_cy_;
+}
+inline void Region::set_turnplate_cy(float value) {
+  set_has_turnplate_cy();
+  turnplate_cy_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.turnplate_cy)
+}
+
+// required float border_minx = 11;
+inline bool Region::has_border_minx() const {
+  return (_has_bits_[0] & 0x00000200u) != 0;
+}
+inline void Region::set_has_border_minx() {
+  _has_bits_[0] |= 0x00000200u;
+}
+inline void Region::clear_has_border_minx() {
+  _has_bits_[0] &= ~0x00000200u;
+}
+inline void Region::clear_border_minx() {
+  border_minx_ = 0;
+  clear_has_border_minx();
+}
+inline float Region::border_minx() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.border_minx)
+  return border_minx_;
+}
+inline void Region::set_border_minx(float value) {
+  set_has_border_minx();
+  border_minx_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.border_minx)
+}
+
+// required float border_maxx = 12;
+inline bool Region::has_border_maxx() const {
+  return (_has_bits_[0] & 0x00000400u) != 0;
+}
+inline void Region::set_has_border_maxx() {
+  _has_bits_[0] |= 0x00000400u;
+}
+inline void Region::clear_has_border_maxx() {
+  _has_bits_[0] &= ~0x00000400u;
+}
+inline void Region::clear_border_maxx() {
+  border_maxx_ = 0;
+  clear_has_border_maxx();
+}
+inline float Region::border_maxx() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.border_maxx)
+  return border_maxx_;
+}
+inline void Region::set_border_maxx(float value) {
+  set_has_border_maxx();
+  border_maxx_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.border_maxx)
+}
+
+// required float plc_offsetx = 13;
+inline bool Region::has_plc_offsetx() const {
+  return (_has_bits_[0] & 0x00000800u) != 0;
+}
+inline void Region::set_has_plc_offsetx() {
+  _has_bits_[0] |= 0x00000800u;
+}
+inline void Region::clear_has_plc_offsetx() {
+  _has_bits_[0] &= ~0x00000800u;
+}
+inline void Region::clear_plc_offsetx() {
+  plc_offsetx_ = 0;
+  clear_has_plc_offsetx();
+}
+inline float Region::plc_offsetx() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.plc_offsetx)
+  return plc_offsetx_;
+}
+inline void Region::set_plc_offsetx(float value) {
+  set_has_plc_offsetx();
+  plc_offsetx_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.plc_offsetx)
+}
+
+// required float plc_offsety = 14;
+inline bool Region::has_plc_offsety() const {
+  return (_has_bits_[0] & 0x00001000u) != 0;
+}
+inline void Region::set_has_plc_offsety() {
+  _has_bits_[0] |= 0x00001000u;
+}
+inline void Region::clear_has_plc_offsety() {
+  _has_bits_[0] &= ~0x00001000u;
+}
+inline void Region::clear_plc_offsety() {
+  plc_offsety_ = 0;
+  clear_has_plc_offsety();
+}
+inline float Region::plc_offsety() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.plc_offsety)
+  return plc_offsety_;
+}
+inline void Region::set_plc_offsety(float value) {
+  set_has_plc_offsety();
+  plc_offsety_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.plc_offsety)
+}
+
 #ifdef __GNUC__
   #pragma GCC diagnostic pop
 #endif  // __GNUC__

velodyne_lidar/velodyne_config.proto

@@ -53,4 +53,10 @@ message Region
     required float maxz=6;
     required int32 region_id=7;
     repeated lidarExtrinsic lidar_exts=8;
+    required float turnplate_cx=9;
+    required float turnplate_cy=10;
+    required float border_minx=11; // 最小边界x，左超界提示
+    required float border_maxx=12; // 最大边界x，右超界提示
+    required float plc_offsetx=13; // plc偏移x
+    required float plc_offsety=14; // plc偏移y
 }

velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

@@ -139,9 +139,12 @@ Error_manager Velodyne_lidar_device::uninit()
 		delete mp_capture_thread;
 		mp_capture_thread = NULL;
 	}
+	// LOG(WARNING) << "000";
 
 	// //终止队列，防止 wait_and_pop 阻塞线程。
 	m_communication_data_queue.termination_queue();
+	m_communication_data_queue.clear();
+	// LOG(WARNING) << "111";
 	if (mp_parse_thread)
 	{
 		m_parse_condition.kill_all();
@@ -149,7 +152,6 @@ Error_manager Velodyne_lidar_device::uninit()
 		delete mp_parse_thread;
 		mp_parse_thread = NULL;
 	}
-
 	//关闭线程
 	//回收线程的资源
 	if (mp_execute_thread)
@@ -159,9 +161,7 @@ Error_manager Velodyne_lidar_device::uninit()
 		delete mp_execute_thread;
 		mp_execute_thread = NULL;
 	}
-
 	m_input_socket.uninit();
-
 	//清空队列
 	m_communication_data_queue.clear_and_delete();
 
@@ -538,7 +538,7 @@ void Velodyne_lidar_device::capture_thread_fun()
 
 	while (m_capture_condition.is_alive())
 	{
-		m_capture_condition.wait_for_ex(std::chrono::microseconds(10));
+		m_capture_condition.wait_for_ex(std::chrono::microseconds(100));
 		if (m_capture_condition.is_alive())
 		{
 			// socket状态正常才更新状态
@@ -549,6 +549,7 @@ void Velodyne_lidar_device::capture_thread_fun()
 				Binary_buf *tp_binary_buf = new Binary_buf(reinterpret_cast<char *>(m_buf), PACKET_SIZE);
 				//将数据添加到队列中, 然后内存权限转交给队列.
 				m_communication_data_queue.push(tp_binary_buf);
+				// LOG(WARNING) << "queue count"<<m_communication_data_queue.size();
 				m_capture_time = std::chrono::steady_clock::now();
 				m_velodyne_socket_status = E_READY;
 			}
@@ -573,15 +574,19 @@ void Velodyne_lidar_device::parse_thread_fun()
 	while (m_parse_condition.is_alive())
 	{
 		// m_parse_condition.wait();
-		m_parse_condition.wait_for_ex(std::chrono::microseconds(10));
+		m_parse_condition.wait_for_ex(std::chrono::microseconds(100));
 		if (m_parse_condition.is_alive())
 		{
+			// LOG(INFO) << "00000";
 			if (m_velodyne_protocol_status == E_READY)
 				m_velodyne_protocol_status = E_BUSY;
 			if (m_protocol.isInitialized())
 			{
 				// LOG(INFO) << "11111";
-				Binary_buf *buf = *m_communication_data_queue.wait_and_pop();
+				std::shared_ptr<Binary_buf*> tp_buf = m_communication_data_queue.wait_and_pop();
+				if(tp_buf == nullptr)
+					continue;
+				Binary_buf *buf = *tp_buf;
 				// LOG(INFO) << "22222";
 				if (buf != nullptr)
 				{
@@ -609,6 +614,9 @@ void Velodyne_lidar_device::parse_thread_fun()
 						}
 						m_parse_ring_time = std::chrono::steady_clock::now();
 					}
+					// LOG(INFO) << "33333";
+					delete buf;
+					buf = nullptr;
 				}
 				m_velodyne_protocol_status = E_READY;
 			}
@@ -617,6 +625,7 @@ void Velodyne_lidar_device::parse_thread_fun()
 				m_velodyne_protocol_status = E_FAULT;
 			}
 		}
+		// LOG(INFO) << "44444";
 	}
 	LOG(INFO) << " Velodyne_lidar_device::parse_thread_fun() end " << this;
 	return;

velodyne_lidar/velodyne_manager.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-07-23 16:39:04
- * @LastEditTime: 2021-09-22 16:53:09
+ * @LastEditTime: 2021-09-26 11:49:46
  * @LastEditors: yct
  */
 #include "velodyne_manager.h"
@@ -83,7 +83,7 @@ Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(velodyne::ve
 		}
 	}
 	// 检查是否存在该区域
-	if(m_ground_region_map.find(m_terminal_id) == m_ground_region_map.end())
+	if(velodyne_parameters.distribution_mode() && m_ground_region_map.find(m_terminal_id) == m_ground_region_map.end())
 	{
 		LOG(ERROR) << "param error, cannot find region "<<m_terminal_id;
 		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
@@ -414,6 +414,7 @@ void Velodyne_manager::collect_cloud_thread_fun()
 							}
 							else
 							{
+								// LOG(WARNING) << "map count: "<< m_region_laser_to_cloud_collection_map.size();
 								m_region_laser_to_cloud_collection_map[t_region_lidar_key]->setCloud(t_cloud);
 								// LOG(WARNING) << "编号" << iter->first << "雷达在区域" << iter_region->first << "中点云 " << t_cloud->size();
 							}

wanji_lidar/region_detect.cpp

@@ -411,10 +411,10 @@ Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud
 	}
 	// find bounding rectangle of all wheel points
 	cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
-	car_wheel_information.center_x = wheel_box.center.x;
-	car_wheel_information.center_y = wheel_box.center.y;
+	car_wheel_information.car_center_x = wheel_box.center.x;
+	car_wheel_information.car_center_y = wheel_box.center.y;
 	// add center point to the input cloud for further check
-	p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_x, car_wheel_information.center_y, 0.0));
+	p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, car_wheel_information.car_center_y, 0.0));
 
 
 
@@ -443,7 +443,7 @@ Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud
 	// the line formula is: nx * x + ny * y + (-(nx*cx+ny*cy)) = 0
 	Eigen::Vector2f normal, center_point;
 	normal << vec.x / sqrt(vec.x * vec.x + vec.y * vec.y), vec.y / sqrt(vec.x * vec.x + vec.y * vec.y);
-	center_point << car_wheel_information.center_x, car_wheel_information.center_y;
+	center_point << car_wheel_information.car_center_x, car_wheel_information.car_center_y;
 	float line_param_c = -1 * center_point.transpose() * normal;
 	// get rotation matrix, get the angle towards normal vector, rather than x axis
 	// R = [ cos -sin]
@@ -489,13 +489,13 @@ Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud
 	{
 		y_values0 /= count0;
 		y_values1 /= count1;
-		car_wheel_information.wheel_base = fabs(y_values1 - y_values0);
-		car_wheel_information.wheel_width = fabs(min_x - max_x);
+		car_wheel_information.car_wheel_base = fabs(y_values1 - y_values0);
+		car_wheel_information.car_wheel_width = fabs(min_x - max_x);
 //        LOG(INFO) << "--------detector find width, wheelbase: " << width << ", " << wheelbase << ", y means: [" << y_values0 << ", " << y_values1 << "] -----";
-		p_cloud_in->points.push_back(pcl::PointXYZ(min_x, car_wheel_information.center_y, 0.0));
-		p_cloud_in->points.push_back(pcl::PointXYZ(max_x, car_wheel_information.center_y, 0.0));
-		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_x, y_values0, 0.0));
-		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.center_x, y_values1, 0.0));
+		p_cloud_in->points.push_back(pcl::PointXYZ(min_x, car_wheel_information.car_center_y, 0.0));
+		p_cloud_in->points.push_back(pcl::PointXYZ(max_x, car_wheel_information.car_center_y, 0.0));
+		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, y_values0, 0.0));
+		p_cloud_in->points.push_back(pcl::PointXYZ(car_wheel_information.car_center_x, y_values1, 0.0));
 	}
 	else
 	{
@@ -503,8 +503,8 @@ Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud
 		// wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
 		// LOG(INFO) << "--------detector find border: [" << wheel_box.size.width << ", " << wheel_box.size.height << "] -----";
 		cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points);
-		car_wheel_information.wheel_base = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
-		car_wheel_information.wheel_width = std::min(wheel_box.size.width, wheel_box.size.height);
+		car_wheel_information.car_wheel_base = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
+		car_wheel_information.car_wheel_width = std::min(wheel_box.size.width, wheel_box.size.height);
 	}
 
 	return Error_manager(Error_code::SUCCESS);
@@ -542,12 +542,12 @@ Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCl
 	else
 	{
 		std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
-		car_wheel_information.center_x = detect_result.cx;
-		car_wheel_information.center_y = detect_result.cy;
+		car_wheel_information.car_center_x = detect_result.cx;
+		car_wheel_information.car_center_y = detect_result.cy;
 		car_wheel_information.car_angle = detect_result.theta;
-		car_wheel_information.wheel_base = detect_result.wheel_base;
-		car_wheel_information.wheel_width = detect_result.width;
-		car_wheel_information.front_theta = detect_result.front_theta;
+		car_wheel_information.car_wheel_base = detect_result.wheel_base;
+		car_wheel_information.car_wheel_width = detect_result.width;
+		car_wheel_information.car_front_theta = detect_result.front_theta;
 		car_wheel_information.correctness = detect_result.success;
 	}