add mix lidar func, but pcl 1-10

may incompatible with deploy env

main.cpp

@@ -93,7 +93,7 @@ int main(int argc,char* argv[])
 	Error_manager ec;
 
 	// 初始化
-	if(WJ_VELO == 0)
+	if(WJ_VELO == 0 || WJ_VELO == 2)
 	{
 		Wanji_manager::get_instance_references().wanji_manager_init(t_terminal_id);
 		std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status().to_string() << std::endl;
@@ -103,7 +103,7 @@ int main(int argc,char* argv[])
 			return -1;
 		}
 	}
-	else if(WJ_VELO == 1)
+	if(WJ_VELO == 1 || WJ_VELO == 2)
 	{
 		ec = Velodyne_manager::get_instance_references().velodyne_manager_init(t_terminal_id);
 		std::cout << "veodyne_manager = " << Velodyne_manager::get_instance_references().check_status().to_string() << std::endl;
@@ -164,9 +164,9 @@ int main(int argc,char* argv[])
 	// 反初始化
 	System_communication::get_instance_references().communication_uninit();
 	System_executor::get_instance_references().system_executor_uninit();
-	if(WJ_VELO == 0)
+	if(WJ_VELO == 0 || WJ_VELO == 2)
 		Wanji_manager::get_instance_references().wanji_manager_uninit();
-	else if(WJ_VELO == 1)
+	if(WJ_VELO == 1 || WJ_VELO == 2)
 		Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
 
 	return 0;

system/system_executor.cpp

@@ -3,13 +3,6 @@
 //
 
 #include "system_executor.h"
-#include "../tool/common_data.h"
-#include "../tool/measure_filter.h"
-#include "../message/measure_message.pb.h"
-#include "../system/system_communication.h"
-
-#include "../wanji_lidar/wanji_manager.h"
-#include "../velodyne_lidar/velodyne_manager.h"
 
 // std::ofstream g_debug_file; // 用于测试滤波功能
 
@@ -226,6 +219,99 @@ Error_manager System_executor::check_status()
 	}
 }
 
+// 更新消息中关于定位结果与超界信息的内容
+bool System_executor::update_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size)
+{
+	Error_manager t_error;
+	//无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
+	message::Locate_information t_locate_information;
+	//= t_multi_status_msg.add_locate_information_realtime();
+	t_locate_information.set_locate_x(measure_info.car_center_x);
+	t_locate_information.set_locate_y(measure_info.car_center_y);
+	t_locate_information.set_locate_angle(measure_info.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(measure_info.car_wheel_base);
+	t_locate_information.set_locate_wheel_width(measure_info.car_wheel_width);
+	t_locate_information.set_locate_front_theta(measure_info.car_front_theta);
+	t_locate_information.set_locate_correct(measure_info.correctness);
+	t_locate_information.set_uniformed_car_x(measure_info.uniform_car_x);
+	t_locate_information.set_uniformed_car_y(measure_info.uniform_car_y);
+	msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
+	// 当前超界提示仅保留一项
+	// 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
+	msg.set_border_status(measure_info.range_status);
+	if (!measure_info.correctness)
+	{
+		if (cloud_size > 0)
+			msg.set_ground_status(message::Ground_statu::Noise);
+		else
+			msg.set_ground_status(message::Ground_statu::Nothing);
+	}
+	else if (measure_info.range_status == int(Ground_region::Range_status::Range_correct))
+	{
+		msg.set_ground_status(message::Ground_statu::Car_correct);
+	}
+	else
+	{
+		msg.set_ground_status(message::Ground_statu::Car_border_reached);
+		// 更新待提示错误信息
+		std::string t_error_str;
+		if (measure_info.range_status & Ground_region::Range_status::Range_front != 0)
+		{
+			t_error_str.append("前超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_back != 0)
+		{
+			t_error_str.append("后超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_left != 0)
+		{
+			t_error_str.append("左超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_right != 0)
+		{
+			t_error_str.append("右超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_bottom != 0)
+		{
+			t_error_str.append("底盘超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_top != 0)
+		{
+			t_error_str.append("顶超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_car_width != 0)
+		{
+			t_error_str.append("车宽超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
+		{
+			t_error_str.append("轴距超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
+		{
+			t_error_str.append("车辆角度左超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_angle_clock != 0)
+		{
+			t_error_str.append("车辆角度右超界 ");
+		}
+		if (measure_info.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
+		{
+			t_error_str.append("车辆前轮角超界 ");
+		}
+
+		t_error.set_error_description(t_error_str);
+	}
+
+	msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
+	msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
+	msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
+	return true;
+}
+
 //定时发送状态信息
 Error_manager System_executor::encapsulate_send_status()
 {
@@ -250,7 +336,7 @@ Error_manager System_executor::encapsulate_send_status()
 	// 创建各区域状态消息,
 	// 注意！！！目前公共消息名字依旧使用wj，不做修改
 	// manager
-	if (WJ_VELO == 1)
+#if WJ_VELO == 1
 	{
 		Velodyne_manager::Velodyne_manager_status t_velodyne_manager_status = Velodyne_manager::get_instance_references().get_status();
 		t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_velodyne_manager_status);
@@ -307,92 +393,7 @@ 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);
-			// 当前超界提示仅保留一项
-			// 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
-			t_multi_status_msg.set_border_status(t_car_wheel_information.range_status);
-			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 == int(Ground_region::Range_status::Range_correct))
-			{
-				t_multi_status_msg.set_ground_status(message::Ground_statu::Car_correct);
-			}
-			else
-			{
-				t_multi_status_msg.set_ground_status(message::Ground_statu::Car_border_reached);
-				// 更新待提示错误信息
-				std::string t_error_str;
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_front != 0)
-				{
-					t_error_str.append("前超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_back != 0)
-				{
-					t_error_str.append("后超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
-				{
-					t_error_str.append("左超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
-				{
-					t_error_str.append("右超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_bottom != 0)
-				{
-					t_error_str.append("底盘超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_top != 0)
-				{
-					t_error_str.append("顶超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_width != 0)
-				{
-					t_error_str.append("车宽超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
-				{
-					t_error_str.append("轴距超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
-				{
-					t_error_str.append("车辆角度左超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_clock != 0)
-				{
-					t_error_str.append("车辆角度右超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
-				{
-					t_error_str.append("车辆前轮角超界 ");
-				}
-
-				t_error.set_error_description(t_error_str);
-			}
-
-			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());
+			update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
 
 			std::string t_msg = t_multi_status_msg.SerializeAsString();
 			System_communication::get_instance_references().encapsulate_msg(t_msg);
@@ -401,8 +402,7 @@ Error_manager System_executor::encapsulate_send_status()
 						  << std::endl;
 		}
 	}
-
-	else if (WJ_VELO == 0)
+#elif WJ_VELO == 0
 	{
 		// wj_support 20220718
 		Wanji_manager::Wanji_manager_status t_wj_manager_status = Wanji_manager::get_instance_references().get_status();
@@ -460,103 +460,157 @@ 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);
-			// 当前超界提示仅保留一项
-			// 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
-			t_multi_status_msg.set_border_status(t_car_wheel_information.range_status);
-			if (!t_car_wheel_information.correctness)
+			update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
+
+			std::string t_msg = t_multi_status_msg.SerializeAsString();
+			System_communication::get_instance_references().encapsulate_msg(t_msg);
+			if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
+				LOG(INFO) << t_multi_status_msg.DebugString();
+		}
+	}
+	//	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = "  << std::endl;
+	//	std::cout << t_ground_status_msg.DebugString() << std::endl;
+
+	//	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl;
+#else
+{
+	// mix vlp16 and wj716
+	// 以多线雷达为主
+	Velodyne_manager::Velodyne_manager_status t_velodyne_manager_status = Velodyne_manager::get_instance_references().get_status();
+	t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_velodyne_manager_status);
+
+	// vlp16 lidar
+	std::map<int, Velodyne_lidar_device *> t_velodyne_lidar_device_map = Velodyne_manager::get_instance_references().get_velodyne_lidar_device_map();
+	std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status> t_velodyne_lidar_status_map;
+	for (auto iter = t_velodyne_lidar_device_map.begin(); iter != t_velodyne_lidar_device_map.end(); ++iter)
+	{
+		t_velodyne_lidar_status_map.emplace(std::pair<int, Velodyne_lidar_device::Velodyne_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
+	}
+
+	// wj lidar
+	std::map<int, Wanji_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
+	std::map<int, Wanji_lidar_device::Wanji_lidar_device_status> t_wj_lidar_status_map;
+	for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
+	{
+		t_wj_lidar_status_map.emplace(std::pair<int, Wanji_lidar_device::Wanji_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
+	}
+
+	// wj region
+	std::map<int, Region_worker *> t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
+
+	// vlp16 region
+	std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
+	int region_index = 0;
+	for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
+	{
+		// 以t_ground_status_msg为模板创建各区域心跳消息
+		message::Ground_status_msg t_multi_status_msg;
+		t_multi_status_msg.CopyFrom(t_ground_status_msg);
+		t_multi_status_msg.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id());
+		t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status()));
+		// 查找多线区域内对应多线激光雷达
+		velodyne::Region t_param = iter->second->get_param();
+		for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
+		{
+			std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status>::iterator t_status_iter = t_velodyne_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
+			if (t_status_iter == t_velodyne_lidar_status_map.end())
 			{
-				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);
+				LOG(WARNING) << "vlp16 lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
 			}
-			else if (t_car_wheel_information.range_status == int(Ground_region::Range_status::Range_correct))
+			else
 			{
-				t_multi_status_msg.set_ground_status(message::Ground_statu::Car_correct);
+				t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
 			}
-			else
+		}
+
+		//velodyne雷达的自动定位信息
+		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());
+
+		// 获取区域点云填入信息
+		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)
+		{
+			std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
+			save_cloud_txt(t_region_cloud, t_filename);
+			LOG(INFO) << "vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
+		}
+		// for (size_t j = 0; j < t_region_cloud->size(); j++)
+		// {
+		// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
+		// 	tp_cloud->set_x(t_region_cloud->points[j].x);
+		// 	tp_cloud->set_y(t_region_cloud->points[j].y);
+		// 	tp_cloud->set_z(t_region_cloud->points[j].z);
+		// }
+
+		// 检查vlp16区域与wj区域对应关系，wj存在对应区域则填充设备状态，并融合测量结果
+		auto t_wj_region_iter = t_region_worker_map.find(iter->first);
+		if(t_wj_region_iter != t_region_worker_map.end())
+		{
+			wj::Region t_param = t_wj_region_iter->second->get_param();
+			// 
+			for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
 			{
-				t_multi_status_msg.set_ground_status(message::Ground_statu::Car_border_reached);
-				// 更新待提示错误信息
-				std::string t_error_str;
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_front != 0)
-				{
-					t_error_str.append("前超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_back != 0)
-				{
-					t_error_str.append("后超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
-				{
-					t_error_str.append("左超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
-				{
-					t_error_str.append("右超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_bottom != 0)
-				{
-					t_error_str.append("底盘超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_top != 0)
-				{
-					t_error_str.append("顶超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_width != 0)
-				{
-					t_error_str.append("车宽超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
-				{
-					t_error_str.append("轴距超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
-				{
-					t_error_str.append("车辆角度左超界 ");
-				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_clock != 0)
+				std::map<int, Wanji_lidar_device::Wanji_lidar_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
+				if (t_status_iter == t_wj_lidar_status_map.end())
 				{
-					t_error_str.append("车辆角度右超界 ");
+					LOG(WARNING) << "wj lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
 				}
-				if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
+				else
 				{
-					t_error_str.append("车辆前轮角超界 ");
+					t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
 				}
+			}
+			//wj雷达的自动定位信息
+			Common_data::Car_wheel_information t_wj_car_wheel_information;
+			t_error = t_wj_region_iter->second->get_last_wheel_information(&t_wj_car_wheel_information, std::chrono::system_clock::now());
 
-				t_error.set_error_description(t_error_str);
+			// 获取区域点云填入信息
+			pcl::PointCloud<pcl::PointXYZ>::Ptr t_wj_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+			t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud);
+			if (cloud_count == 5)
+			{
+				std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt";
+				save_cloud_txt(t_wj_region_cloud, t_filename);
+				LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename;
 			}
+			// for (size_t j = 0; j < t_wj_region_cloud->size(); j++)
+			// {
+			// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
+			// 	tp_cloud->set_x(t_wj_region_cloud->points[j].x);
+			// 	tp_cloud->set_y(t_wj_region_cloud->points[j].y);
+			// 	tp_cloud->set_z(t_wj_region_cloud->points[j].z);
+			// }
 
-			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());
+			// 融合测量结果，验证中心点差异x<0.03, y<0.03, 角度差异ang<0.6, 轴距差异wb<0.055
+			double dx = t_car_wheel_information.car_center_x - t_wj_car_wheel_information.car_center_x;
+			double dy = t_car_wheel_information.car_center_y - t_wj_car_wheel_information.car_center_y;
+			double dang = t_car_wheel_information.car_angle - t_wj_car_wheel_information.car_angle;
+			double dwb = t_car_wheel_information.car_wheel_base - t_wj_car_wheel_information.car_wheel_base;
+			if(fabs(dx) < 0.03 && fabs(dy) < 0.03 && fabs(dang) < 0.6 && fabs(dwb) < 0.055)
+			{
+				t_car_wheel_information.car_wheel_base = t_car_wheel_information.car_wheel_base*0.2 + t_car_wheel_information.car_wheel_base*0.8;
+			}else
+			{
+				LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<
+					"detect mismatch, region "<<iter->first<<", dx dy dang dwb: "<<dx<<", "<<dy<<", "<<dang<<", "<< dwb;
+			}
 
-			std::string t_msg = t_multi_status_msg.SerializeAsString();
-			System_communication::get_instance_references().encapsulate_msg(t_msg);
-			if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
-				LOG(INFO) << t_multi_status_msg.DebugString();
+		}else{
+			LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"region mismatch, vlp16 "<<iter->first<<", wj doesn't have corresponding region id";
 		}
-	}
-	//	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = "  << std::endl;
-	//	std::cout << t_ground_status_msg.DebugString() << std::endl;
 
-	//	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl;
+		update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
+
+		std::string t_msg = t_multi_status_msg.SerializeAsString();
+		System_communication::get_instance_references().encapsulate_msg(t_msg);
+		if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
+			std::cout << t_multi_status_msg.DebugString() << std::endl
+						<< std::endl;
+	}
+}
+#endif
 
 	return Error_code::SUCCESS;
 }

system/system_executor.h

@@ -10,9 +10,17 @@
 #include "../error_code/error_code.h"
 #include "../communication/communication_message.h"
 
-#define DISP_TERM_ID -1
-// 0wj  1velo
-#define WJ_VELO 0
+#include "../tool/common_data.h"
+#include "../tool/measure_filter.h"
+#include "../message/measure_message.pb.h"
+#include "../system/system_communication.h"
+
+#include "../wanji_lidar/wanji_manager.h"
+#include "../velodyne_lidar/velodyne_manager.h"
+
+#define DISP_TERM_ID 5
+// 0wj  1velo	2both
+#define WJ_VELO 2
 
 class System_executor:public Singleton<System_executor>
 {
@@ -60,6 +68,9 @@ public://API functions
 
 	//判断是否为待机，如果已经准备好，则可以执行任务。
 	bool is_ready();
+
+	// 更新消息中关于定位结果与超界信息的内容
+	bool update_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size);
 public://get or set member variable
 	System_executor_status get_system_executor_status();
 	int get_terminal_id();