20200914, 万集雷达

setting/limit.prototxt

@@ -0,0 +1,79 @@
+center_min_x:0
+center_max_x:18800
+center_min_y:260
+center_max_y:900
+
+corner_min_y:-2300
+corner_max_y:3450
+
+height:2000
+
+theta_range
+{
+	min_theta:80
+	max_theta:100
+}
+
+theta_range
+{
+	min_theta:260
+	max_theta:280
+}
+
+#1号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:2265
+	railing_width:30
+}
+#2号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-2006
+	railing_width:30
+}
+#3号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-5350
+	railing_width:30
+}
+#4号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-8724
+	railing_width:30
+}
+#5号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-12215
+	railing_width:30
+}
+#6号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-15680
+	railing_width:30
+}
+#7号栏杆
+railing_parameter
+{
+	pa:1
+	pb:0
+	pc:-19000
+	railing_width:30
+}
+

setting/terminal.prototxt

@@ -0,0 +1,333 @@
+save_root_path:"/home/zx/data/scans"
+#1号终端
+terminor_parameters
+{
+	id:0
+	area_3d
+	{
+		
+		min_x:-1100
+		max_x:2400
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+	
+}
+#2号终端
+terminor_parameters
+{
+	id:1
+	area_3d
+	{
+		
+		min_x:2100
+		max_x:5600
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#3号终端
+terminor_parameters
+{
+	id:2
+	area_3d
+	{
+		
+		min_x:5300
+		max_x:8800
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+	
+}
+#4号终端
+terminor_parameters
+{
+	id:3
+	area_3d
+	{
+		
+		min_x:8500
+		max_x:12000
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#5号终端
+terminor_parameters
+{
+	id:4
+	area_3d
+	{
+		
+		min_x:11700
+		max_x:15200
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:1800
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}
+#6号终端
+terminor_parameters
+{
+	id:5
+	area_3d
+	{
+		
+		min_x:15600
+		max_x:18800
+		min_y:-2218
+		max_y:3270
+		min_z:-20
+		max_z:2000
+	}
+	laser_parameter
+	{
+		id:0
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:1
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:2
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:3
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:4
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:5
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:6
+		frame_count:1000
+	}
+	laser_parameter
+	{
+		id:7
+		frame_count:1000
+	}
+}

setting/wanji_lidar.prototxt

@@ -0,0 +1,9 @@
+
+
+
+	net_config
+    {
+        ip_address:"192.168.0.208"
+        port:2110
+    }
+

setting/wanji_manager.prototxt

@@ -0,0 +1,33 @@
+
+
+wj_lidar
+{
+    net_config
+    {
+        ip_address:"192.168.0.208"
+        port:2110
+    }
+    transform
+    {
+
+    }
+    scan_limit
+    {
+
+    }
+}
+
+regions
+{
+     minx:-10000
+     maxx:-10000
+     miny:10000
+     maxy:10000
+}
+
+fence_data_path:"../data"
+fence_log_path:"../log"
+
+
+
+

setting/wj_manager_settings.prototxt

@@ -0,0 +1,253 @@
+fence_data_path:"/home/zx/data/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
+
+
+#2
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.202"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0123952
+		m01:-0.9999232
+		m02:1.851
+		m10:-0.9999232
+		m11:0.0123952
+		m12:3.48100000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#3
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.203"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0108588
+		m01:0.9999409
+		m02:5.229
+		m10:0.9999409
+		m11:-0.0108588
+		m12:-2.3500000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#4
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.204"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0549574
+		m01:-0.9984888
+		m02:8.717
+		m10:-0.9984888
+		m11:-0.0549574
+		m12:3.54400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#5
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.205"
+        port:2110
+    }
+    transform
+    {
+    	m00:-0.0196446
+		m01:0.9998069
+		m02:12.136
+		m10:0.9998069
+		m11:0.0196446
+		m12:-2.3190000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#6
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.206"
+        port:2110
+    }
+    transform
+    {
+        m00:0.085926
+		m01:-0.9963016
+		m02:15.675
+		m10:-0.9963016
+		m11:-0.0859263
+		m12:3.48400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#7
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.207"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0609897
+		m01:0.9981384
+		m02:19.069
+		m10:0.9981384
+		m11:0.0609897
+		m12:-2.3750000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#1
+regions
+{
+	minx:-0.5
+	maxx:2.4
+	miny:-2.43
+	maxy:3.43
+}
+
+#2
+regions
+{
+	minx:2.5
+	maxx:5.3
+	miny:-2.4
+	maxy:3.43
+}
+
+#3
+regions
+{
+	minx:5.4
+	maxx:8.7
+	miny:-2.40
+	maxy:3.43
+}
+#4
+regions
+{
+    minx:8.7
+    maxx:12
+    miny:-2.40
+    maxy:3.45
+}
+#5
+regions
+{
+    minx:12.1
+    maxx:15.4
+    miny:-2.40
+    maxy:3.45
+}
+#6
+regions
+{
+    minx:15.8
+    maxx:18.8
+    miny:-2.40
+    maxy:3.45
+}
+

tool/common_data.cpp

@@ -0,0 +1,41 @@
+//
+// 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)
+{
+	car_wheel_information_out.center_x = car_measure_information_in.center_x;
+	car_wheel_information_out.center_y = car_measure_information_in.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.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)
+{
+	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_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.correctness = car_wheel_information_in.correctness;
+}

tool/common_data.h

@@ -0,0 +1,57 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#ifndef NNXX_TESTS_COMMON_DATA_H
+#define NNXX_TESTS_COMMON_DATA_H
+
+
+class Common_data
+{
+public:
+	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
+#define WANJI_716_SCAN_CYCLE_MS 		70
+
+
+	//整车的测量信息
+	struct Car_measure_information
+	{
+		float center_x = 0;				//整车的中心点x值, 四轮的中心
+		float 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;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+
+	};
+
+	//四轮的测量信息
+	struct Car_wheel_information
+	{
+		float center_x = 0;				//整车的中心点x值, 四轮的中心
+		float center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_angle = 0;			//整车的车身旋转角,
+
+		float wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float front_theta = 0;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+
+	};
+
+
+	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);
+
+
+
+};
+
+
+#endif //NNXX_TESTS_COMMON_DATA_H

tool/point2D_tool.cpp

@@ -0,0 +1,139 @@
+//
+// Created by huli on 2020/9/1.
+//
+
+#include "point2D_tool.h"
+
+//极坐标 -> 平面坐标
+bool Point2D_tool::Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D)
+{
+	if ( p_polar_coordinates == NULL || p_point2D == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_point2D->x = p_polar_coordinates->distance * cos(p_polar_coordinates->angle);
+		p_point2D->y = p_polar_coordinates->distance * sin(p_polar_coordinates->angle);
+		return true;
+	}
+	return true;
+}
+//平面坐标 -> 极坐标
+bool Point2D_tool::Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates)
+{
+	if ( p_polar_coordinates == NULL || p_point2D == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_polar_coordinates->distance = sqrt(p_point2D->x * p_point2D->x + p_point2D->y * p_point2D->y);
+		p_polar_coordinates->angle = atan(p_point2D->y / p_point2D->x);
+		return true;
+	}
+	return true;
+}
+
+//判断极坐标点是否在限制范围
+bool Point2D_tool::limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box)
+{
+	if ( p_polar_coordinates_box == NULL )
+	{
+		return false;
+	}
+	else
+	{
+		if ( angle >= p_polar_coordinates_box->angle_min &&
+			 angle <= p_polar_coordinates_box->angle_max &&
+			 distance >= p_polar_coordinates_box->distance_min &&
+			 distance <= p_polar_coordinates_box->distance_max )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+//判断平面坐标点是否在限制范围
+bool Point2D_tool::limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box)
+{
+	if ( p_point2D_box == NULL )
+	{
+		return false;
+	}
+	else
+	{
+		if ( x >= p_point2D_box->x_min &&
+			 x <= p_point2D_box->x_max &&
+			 y >= p_point2D_box->y_min &&
+			 y <= p_point2D_box->y_max )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+//平面坐标的转换, 可以进行旋转和平移, 不能缩放
+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)
+{
+	if ( p_x_in == NULL || p_x_in == NULL || p_x_out == NULL || p_y_out == NULL || p_point2D_transform == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		*p_x_out = *p_x_in * p_point2D_transform->m00 + *p_y_in * p_point2D_transform->m01 + p_point2D_transform->m02;
+		*p_y_out = *p_x_in * p_point2D_transform->m10 + *p_y_in * p_point2D_transform->m11 + p_point2D_transform->m12;
+		return true;
+	}
+	return true;
+}
+bool Point2D_tool::transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
+												Point2D_tool::Point2D_transform* p_point2D_transform)
+{
+	if (p_point3D_in == NULL || p_point3D_out == NULL || p_point2D_transform == NULL)
+	{
+		return false;
+	}
+	else
+	{
+		p_point3D_out->x = p_point3D_in->x * p_point2D_transform->m00 + p_point3D_in->y * p_point2D_transform->m01 +
+						   p_point2D_transform->m02;
+		p_point3D_out->y = p_point3D_in->x * p_point2D_transform->m10 + p_point3D_in->y * p_point2D_transform->m11 +
+						   p_point2D_transform->m12;
+		return true;
+	}
+	return true;
+}
+bool Point2D_tool::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)
+{
+	if ( p_cloud_in.get() == NULL || p_cloud_out.get() == NULL || p_point2D_transform == NULL)
+	{
+	    return false;
+	}
+	else
+	{
+		pcl::PointXYZ point;
+		for (int i = 0; i < p_cloud_in->size(); ++i)
+		{
+			point.x = p_cloud_in->points[i].x * p_point2D_transform->m00 + p_cloud_in->points[i].y * p_point2D_transform->m01 + p_point2D_transform->m02;
+			point.y = p_cloud_in->points[i].x * p_point2D_transform->m10 + p_cloud_in->points[i].y * p_point2D_transform->m11 + p_point2D_transform->m12;
+			p_cloud_out->push_back(point);
+		}
+		return true;
+	}
+	return true;
+}

tool/point2D_tool.h

@@ -0,0 +1,81 @@
+//
+// Created by huli on 2020/9/1.
+//
+
+#ifndef NNXX_TESTS_POINT2D_H
+#define NNXX_TESTS_POINT2D_H
+
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/common/common.h>
+
+class Point2D_tool
+{
+public:
+	//极坐标
+	struct Polar_coordinates
+	{
+		float angle=0;				//弧度
+		float distance=0;				//距离
+	};
+
+	//极坐标的限定范围
+	struct Polar_coordinates_box
+	{
+		float angle_min=0;			//角度最小值
+		float angle_max=0;			//角度最大值
+		float distance_min=0;			//距离最小值
+		float distance_max=0;			//距离最大值
+	};
+
+	//平面坐标
+	struct Point2D
+	{
+		float x=0;				//x轴
+		float y=0;				//y轴
+	};
+
+	//平面坐标的限定范围
+	struct Point2D_box
+	{
+		float x_min=0;				//x轴最小值
+		float x_max=0;				//x轴最大值
+		float y_min=0;				//y轴最小值
+		float y_max=0;				//y轴最大值
+	};
+
+	//平面坐标的转换矩阵, 可以进行旋转和平移, 不能缩放
+	struct Point2D_transform
+	{
+		float m00=1;
+		float m01=0;
+		float m02=0;
+		float m10=0;
+		float m11=1;
+		float m12=0;
+	};
+
+	//极坐标 -> 平面坐标
+	static bool Polar_coordinates_to_point2D(Point2D_tool::Polar_coordinates* p_polar_coordinates, Point2D_tool::Point2D* p_point2D);
+	//平面坐标 -> 极坐标
+	static bool Point2D_to_polar_coordinates(Point2D_tool::Point2D* p_point2D, Point2D_tool::Polar_coordinates* p_polar_coordinates);
+
+
+	//判断极坐标点是否在限制范围
+	static bool limit_with_polar_coordinates_box(float distance, float angle, Point2D_tool::Polar_coordinates_box* p_polar_coordinates_box);
+	//判断平面坐标点是否在限制范围
+	static bool limit_with_point2D_box(float x, float y, Point2D_tool::Point2D_box* p_point2D_box);
+
+	//平面坐标的转换, 可以进行旋转和平移, 不能缩放
+	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);
+	static bool transform_with_translation_rotation(pcl::PointXYZ* p_point3D_in, pcl::PointXYZ* p_point3D_out,
+													Point2D_tool::Point2D_transform* p_point2D_transform);
+	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 //NNXX_TESTS_POINT2D_H

wanji_lidar/async_client.cpp

@@ -0,0 +1,374 @@
+#include "async_client.h"
+
+// 构造
+Async_Client::Async_Client() : m_socket(m_io_service)
+{
+	m_communication_status = E_UNKNOWN;	//通信状态
+	m_is_reconnection_flag = false; 		// 断线是否重连的标志位, true:断线自动重连, false:断线不重连, 保持断线状态.
+
+	mp_thread_receive = NULL;     	// 接受线程, 内存由本模块管理
+	mp_thread_check_connect = NULL;      	// 检查线程, 内存由本模块管理
+	m_check_connect_wait_time_ms = 0;	// 检查连接线程 的等待时间
+
+	m_data_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
+
+	memset(m_data_buf, 0, DATA_LENGTH_MAX);
+	mp_communication_data_queue = NULL;
+}
+
+/**
+ * 析构函数
+ * */
+Async_Client::~Async_Client()
+{
+	uninit();
+}
+
+//初始化, 默认重连
+Error_manager Async_Client::init(std::string ip, int port, Thread_safe_queue<Binary_buf*>* p_communication_data_queue,
+								  bool is_reconnection_flag )
+{
+	if ( p_communication_data_queue == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+
+	m_is_reconnection_flag = is_reconnection_flag;
+
+	m_check_connect_wait_time_ms = 0;	// 检查连接线程 的等待时间, 第一次直接通过
+
+	boost::asio::ip::tcp::endpoint t_ep(boost::asio::ip::address_v4::from_string(ip), port);
+	m_ep = t_ep;
+	mp_communication_data_queue = p_communication_data_queue;
+
+	//接受线程, 默认等待
+	m_condition_receive.reset(false, false, false);
+	mp_thread_receive = new std::thread(&Async_Client::thread_receive, this);
+	//检查连接线程, 默认等待, 内部的wait_for会让其通过
+	m_condition_check_connect.reset(false, false, false);
+	mp_thread_check_connect = new std::thread(&Async_Client::thread_check_connect, this);
+
+	return Error_code::SUCCESS;
+}
+//反初始化
+Error_manager Async_Client::uninit()
+{
+	//注注注注注意了, 这里一定要在关闭通信线程之前, 关闭socket, 
+	//因为异步操作自带一个线程, 这个线程可能会卡住我们的线程...
+	//关闭通信
+	m_socket.close();
+	
+	//杀死2个线程，强制退出
+	if (mp_thread_receive)
+	{
+		m_condition_receive.kill_all();
+	}
+	if (mp_thread_check_connect)
+	{
+		m_condition_check_connect.kill_all();
+	}
+
+	//回收2个线程的资源
+	if (mp_thread_receive)
+	{
+		mp_thread_receive->join();
+		delete mp_thread_receive;
+		mp_thread_receive = NULL;
+	}
+	if (mp_thread_check_connect)
+	{
+		mp_thread_check_connect->join();
+		delete mp_thread_check_connect;
+		mp_thread_check_connect = NULL;
+	}
+
+	
+	m_io_service.stop();
+
+
+	mp_communication_data_queue = NULL;
+
+	if ( m_communication_status != E_FAULT  )
+	{
+		m_communication_status = E_UNKNOWN;
+	}
+	return Error_code::SUCCESS;
+}
+
+//检查状态
+Error_manager Async_Client::check_status()
+{
+	if ( m_communication_status == E_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if(m_communication_status == E_UNKNOWN)
+	{
+		return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_UNINITIALIZED, Error_level::MINOR_ERROR,
+							 " Async_Client::check_status() error ");
+	}
+	else if(m_communication_status == E_DISCONNECT)
+	{
+		return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_DISCONNECT, Error_level::MINOR_ERROR,
+							 " Async_Client::check_status() error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_FAULT, Error_level::MINOR_ERROR,
+							 " Async_Client::check_status() error ");
+	}
+}
+
+//判断是否正常
+bool Async_Client::is_ready()
+{
+	return (m_communication_status == E_READY);
+}
+//获取状态
+Async_Client::Communication_status Async_Client::get_status()
+{
+	return m_communication_status;
+}
+
+
+
+//线程接受函数，包括连接,重连和接受数据
+void Async_Client::thread_receive()
+{
+	LOG(INFO) << " ---Async_Client::thread_receive start ---"<< this;
+
+	//接受雷达消息，包括连接,重连和接受数据
+	while (m_condition_receive.is_alive())
+	{
+		m_condition_receive.wait();
+		if (m_condition_receive.is_alive())
+		{
+			std::this_thread::yield();
+
+			// 连接成功后开启异步读
+			client_async_read();
+
+		}
+	}
+	LOG(INFO) << " Async_Client::thread_receive end :"<<this;
+	return;
+}
+
+//线程检查连接函数，进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+void Async_Client::thread_check_connect()
+{
+	LOG(INFO) << " ---Async_Client::thread_check_connect start ---"<< this;
+
+	//检查连接, 连接, 重连, 判断时间
+	while (m_condition_check_connect.is_alive())
+	{
+		m_condition_check_connect.wait_for_millisecond(m_check_connect_wait_time_ms);
+		if (m_condition_check_connect.is_alive())
+		{
+			std::this_thread::yield();
+
+			switch ( (Communication_status)m_communication_status )
+			{
+				case E_UNKNOWN:
+				{
+					//连接
+					socket_connect();
+					break;
+				}
+				case E_DISCONNECT:
+				{
+					//重连
+					socket_reconnect();
+					break;
+				}
+				case E_READY:
+				{
+					//检查
+					socket_check();
+					break;
+				}
+				default:
+				{
+
+					break;
+				}
+			}
+
+		}
+	}
+	LOG(INFO) << " Async_Client::thread_check_connect end :"<<this;
+	return;
+}
+
+
+
+// 开启连接
+void Async_Client::socket_connect()
+{
+	m_io_service.reset();
+	//开启异步连接, 使用m_ep里面的ip和port进行tcp异步连接
+	// 只有在run()之后, 才会真正的执行连接函数
+	m_socket.async_connect(m_ep, boost::bind(&Async_Client::handle_connect, this, boost::asio::placeholders::error));
+	//注注注注注注意了:async_connect是阻塞函数, 如果连接不上就会一直卡在这, 而不是通过之后, 回调返回错误码.
+	//这个函数大约1分钟之后就会通过, 并调用里面的回调函数,返回我们错误码.
+	//如果硬件恢复连接, 那么这个函数会立即连上, 
+	//如果我们中途退出, 一定要 m_socket.close()退出异步通信他自己后台的线程, 防止这里卡死, 影响我们的线程关闭........
+	m_io_service.run();
+
+	return;
+}
+//关闭连接
+void Async_Client::socket_close()
+{
+	m_socket.close();
+	return;
+}
+// 重新连接
+void Async_Client::socket_reconnect()
+{
+	if (m_is_reconnection_flag)
+	{
+		//关闭原有的连接
+		socket_close();
+		// 开启连接
+		socket_connect();
+	}
+	//否则就在这无限等待下去
+	return;
+}
+
+// 检查连接
+void Async_Client::socket_check()
+{
+	// 判断距上次读取到数据超时时间，过长则主动断开并重连
+	int duration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - m_data_updata_time).count();
+	if (duration > READ_TIMEOUT_MILLISECONDS)
+	{
+		LOG(WARNING) << "connection check thread found read timeout, close socket and try to reinitialize/reconnect.";
+
+		// 通信超时后, 修改状态为断连,
+		m_communication_status = E_DISCONNECT;
+		//检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+		m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+		//关闭接受线程
+		m_condition_receive.notify_all(false);
+	}
+	//否则不做处理
+	return;
+}
+
+// 异步写入
+void Async_Client::client_async_write(char *buf, int len)
+{
+	m_io_service.run();
+	boost::asio::async_write(m_socket,
+							 boost::asio::buffer(buf, len),
+							 boost::bind(&Async_Client::handle_write, this,
+										 boost::asio::placeholders::error));
+	m_io_service.run();
+	return;
+}
+
+// 异步读取
+void Async_Client::client_async_read()
+{
+	//异步读取, 在run()之后, 会自动接受数据, 并存入mp_data_buf里面,
+	// 然后调用回调函数handle_read(), bytes_transferred是返回的数据有效长度
+
+	m_io_service.reset();
+	m_socket.async_read_some(boost::asio::buffer(m_data_buf, DATA_LENGTH_MAX),
+							 boost::bind(&Async_Client::handle_read, this,
+										 boost::asio::placeholders::error,
+										 boost::asio::placeholders::bytes_transferred));
+	//注:async_read_some, 如果断连, 那么就会回调handle_read, 并填充错误码 boost::asio::placeholders::error
+	m_io_service.run();
+
+	return;
+}
+
+
+//异步连接回调, 失败后重连
+void Async_Client::handle_connect(const boost::system::error_code &error)
+{
+	if (!error)
+	{
+		LOG(INFO) << "Async_Client::handle_connect Connection Successful! " << m_ep.address().to_string() << ":" << m_ep.port();
+		// 连接成功后修改状态为正常待机,
+		m_communication_status = E_READY;
+		//检查连接等待时间变为 CHECK_WAIT_TIME_MS,
+		m_check_connect_wait_time_ms = CHECK_WAIT_TIME_MS;
+		//唤醒接受线程
+		m_condition_receive.notify_all(true);
+
+
+	}
+	else
+	{
+		LOG(WARNING) << "connect failed, " << boost::system::system_error(error).what() << ", waiting for reconnection " << m_ep.address().to_string() << ":" << m_ep.port();
+		// 连接失败后修改状态为断连,
+		m_communication_status = E_DISCONNECT;
+		//检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+		m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+		//关闭接受线程
+		m_condition_receive.notify_all(false);
+	}
+
+	return;
+}
+
+
+//异步读取回调, 失败后重连. bytes_transferred是返回的数据有效长度
+void Async_Client::handle_read(const boost::system::error_code &error,
+							   size_t bytes_transferred)
+{
+	if (!error)
+	{
+		Binary_buf * tp_binary_buf = new Binary_buf(m_data_buf, bytes_transferred);
+		if ( mp_communication_data_queue != NULL )
+		{
+			//将数据添加到队列中, 然后内存权限转交给队列.
+			mp_communication_data_queue->push(tp_binary_buf);
+		}
+
+		//更新时间
+		m_data_updata_time = std::chrono::system_clock::now();
+
+	}
+	else
+	{
+		LOG(WARNING) << "handle_read, " << boost::system::system_error(error).what() << ", waiting for reconnection " << m_ep.address().to_string() << ":" << m_ep.port();
+
+		// 读取失败后修改状态为断连,
+		m_communication_status = E_DISCONNECT;
+		//检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+		m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+		//关闭接受线程
+		m_condition_receive.notify_all(false);
+	}
+	return;
+}
+
+
+//异步写入回调, 失败后重连
+void Async_Client::handle_write(const boost::system::error_code &error)
+{
+	if (!error)
+	{
+		//发送成功, 暂时不做处理
+//		LOG(INFO) << "handle write no error";
+	}
+	else
+	{
+		LOG(WARNING) << "handle_write, " << boost::system::system_error(error).what() << ", waiting for reconnection " << m_ep.address().to_string() << ":" << m_ep.port();
+		// 读取失败后修改状态为断连,
+		m_communication_status = E_DISCONNECT;
+		//检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+		m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+		//关闭接受线程
+		m_condition_receive.notify_all(false);
+	}
+	return;
+}
+
+

wanji_lidar/async_client.h

@@ -0,0 +1,180 @@
+
+/*
+ * 	//启动异步服务
+ *
+ * 必须使用如下三步
+	m_io_service.reset();
+ 	//异步事件函数
+ 	m_io_service.run();
+
+
+	//run运行之后, 代码会卡在run()这里
+	//socket.async_connect() socket.async_read_some() boost::asio::async_write()等函数内部才会真正的执行,
+	// 执行完成后就会自动调用里面的回调函数,
+ //里面的事件函数执行完之后, 才会结束run(), 此时才会执行run()后面的代码
+
+	//异步通信的机制就是, 如上3个函数,在被调用的时候会先跳过,
+	//并执行后面的其他代码, 只有在run之后, 才会启动如上3个函数,
+	//由m_io_service.run() m_io_service.stop() m_io_service.reset() m_socket.close() 来控制
+	//功能有点像线程的条件变量.
+
+ //注, run()函数只能启动前面的异步事件,
+ 在run之后, 如果还有异步事件, 那么需要reset和run再次使用.
+
+ //注注注注注意了
+	//m_io_service.run(), m_io_service.stop() , m_io_service.reset()只是控制异步服务的事件函数
+	//m_socket.close() 控制通信
+
+ * */
+
+
+
+#ifndef ASYNC_CLIENT_H
+#define ASYNC_CLIENT_H
+#include <iostream>
+#include <stdio.h>
+#include <time.h>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <chrono>
+#include <mutex>
+#include <thread>
+#include <atomic>
+#include <string>
+#include <unistd.h>
+#include "glog/logging.h"
+
+#include "../error_code/error_code.h"
+#include "../tool/thread_condition.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_queue.h"
+#include "../tool/common_data.h"
+
+
+/**
+ * 异步tcp通信客户端
+ * */
+class Async_Client
+{
+
+public:
+#define DATA_LENGTH_MAX 50000			//接受消息的缓存长度50000byte, 实际通信数据不能超过它, 如果超过了, 则需要增大
+#define RECONNECTION_WAIT_TIME_MS 1000	//每隔1000ms重连一次
+#define CHECK_WAIT_TIME_MS WANJI_716_SCAN_CYCLE_MS			//每隔66ms检查一次通信, 万集雷达15HZ, 必须要大于一个通信周期, 防止重连之后没有及时刷新消息就又超时断连了
+#define READ_TIMEOUT_MILLISECONDS 5000	//通信数据更新 的超时时间5000ms, 如果数据一直不更新那么就断开然后重连
+	//注:隐藏bug:如果通信正常,但是服务端
+
+	typedef void (*fundata_t)(const char *data, const int len, const void *p);
+	typedef void (*Callback_function)(const char *data, const int len, const void *p);
+
+	//通信状态
+	enum Communication_status
+	{//default  = 0
+	    E_UNKNOWN              	= 0,    //未知
+	    E_READY	             	= 1,    //正常待机
+		E_DISCONNECT			= 2,	//断连
+
+		E_FAULT					=10,	//故障
+	};
+public:
+	// 构造
+	Async_Client();
+	// 析构
+	~Async_Client();
+
+	//初始化, 默认重连
+	Error_manager init(std::string ip, int port, Thread_safe_queue<Binary_buf*>* p_communication_data_queue,
+					   bool is_reconnection_flag = true);
+	//反初始化
+	Error_manager uninit();
+
+	//检查状态
+	Error_manager check_status();
+
+	//判断是否正常
+	bool is_ready();
+	//获取状态
+	Communication_status get_status();
+
+protected:
+	//线程接受函数，自动接受数据到 mp_data_buf, 然后触发回调函数,
+	void thread_receive();
+	//线程检查连接函数，进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+	void thread_check_connect();
+
+	// 开启连接
+	void socket_connect();
+	// 关闭连接
+	void socket_close();
+	// 重新连接
+	void socket_reconnect();
+	// 检查连接
+	void socket_check();
+	// 异步写入
+	void client_async_write(char *buf, int len);
+	// 异步读取
+	void client_async_read();
+
+	// 异步连接回调
+	void handle_connect(const boost::system::error_code &error);
+	//异步读取回调, 失败后重连. bytes_transferred是返回的数据有效长度
+	void handle_read(const boost::system::error_code &error, size_t bytes_transferred);
+	// 异步写回调
+	void handle_write(const boost::system::error_code &error);
+
+protected:
+	//状态
+	std::atomic<Communication_status>			m_communication_status;	//通信状态
+	std::atomic<bool> 							m_is_reconnection_flag; 		// 断线是否重连的标志位, true:断线自动重连, false:断线不重连, 保持断线状态.
+
+	//通信
+	//把m_io_service作为参数, 构造m_socket, 那么m_io_service.run()可以异步启动m_socket的通信
+	boost::asio::io_service 					m_io_service;      	// 异步控制服务, 负责异步操作, run启动异步服务, stop停止异步服务
+	boost::asio::ip::tcp::socket 				m_socket; 			// socket句柄, 负责tcp通信, 主要是connect,read,write,close
+	//m_socket.async_connect() 传入m_ep, 可以tcp连接 指定的ip和port
+	boost::asio::ip::tcp::endpoint 				m_ep; 				// 连接参数, 主要是ip和port
+
+	//数据
+	char										m_data_buf[DATA_LENGTH_MAX];			// 通信消息接受缓存, 默认50000byte
+	std::chrono::system_clock::time_point 		m_data_updata_time; 			// 数据更新时间
+	Thread_safe_queue<Binary_buf*>*				mp_communication_data_queue;	//通信数据队列, 内存由上级管理
+
+
+	//接受数据的线程, 自动接受数据到 mp_data_buf, 然后存入队列
+	std::thread *								mp_thread_receive;     	// 接受线程, 内存由本模块管理
+	Thread_condition							m_condition_receive;	//接受线程的条件变量
+	// 检查连接线程, 进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+	std::thread *								mp_thread_check_connect;      	// 检查连接线程, 内存由本模块管理
+	Thread_condition							m_condition_check_connect;		//检查连接线程的条件变量
+	int 										m_check_connect_wait_time_ms;	// 检查连接线程 的等待时间
+
+};
+
+#endif // ASYNC_CLIENT_H
+
+
+/*
+ * boost::asio::io_service使用时的注意事项：
+
+①请让boost::asio::io_service和boost::asio::io_service::work搭配使用。
+
+②想让event按照进入(strand)时的顺序被执行，需要boost::asio::io_service要和boost::asio::io_service::strand搭配使用。
+
+③一般情况下，io_service的成员函数的使用顺序：
+
+boost::asio::io_service构造，
+boost::asio::io_service::run()，
+boost::asio::io_service::stop()，
+boost::asio::io_service::reset()，
+boost::asio::io_service::run()，
+......
+boost::asio::io_service析构，
+④不论有没有使用io_service::work，run()都会执行完io_service里面的event，(若没有用work，run就会退出)。
+⑤一个新创建的io_service不需要执行reset()函数。
+⑥在调用stop()后，在调用run()之前，请先调用reset()函数。
+⑦函数stop()和reset()并不能清除掉io_service里面尚未执行的event。
+我个人认为，也只有析构掉io_service，才能清空它里面的那些尚未执行的event了。(可以用智能指针)。
+
+⑧函数stop(),stopped(),reset(),很简单，请单步调试，以明白它在函数里做了什么。
+ */

wanji_lidar/detect_wheel_ceres.cpp

@@ -0,0 +1,829 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_wheel_ceres.h"
+#include <ceres/cubic_interpolation.h>
+#include <pcl/common/transforms.h>
+
+
+constexpr float kMinProbability = 0.0f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+class GridArrayAdapter {
+public:
+    enum { DATA_DIMENSION = 1 };
+
+    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
+
+    void GetValue(const int row, const int column, double* const value) const {
+        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
+            column >= NumCols() - kPadding) {
+            *value = kMaxCorrespondenceCost;
+        } else {
+            *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
+        }
+    }
+
+    int NumRows() const {
+        return grid_.rows + 2 * kPadding;
+    }
+
+    int NumCols() const {
+        return grid_.cols + 2 * kPadding;
+    }
+private:
+    const cv::Mat& grid_;
+};
+
+class CostFunctor {
+private:
+    cv::Mat  m_map;
+    double m_scale;
+    mutable double m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr;
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+
+public:
+    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front, pcl::PointCloud<pcl::PointXYZ> right_front,
+                pcl::PointCloud<pcl::PointXYZ> left_rear, pcl::PointCloud<pcl::PointXYZ> right_rear,
+                cv::Mat &map, double scale)
+    {
+        m_map = map;
+        m_scale = scale;
+        m_left_front_cloud = left_front;
+        m_right_front_cloud = right_front;
+        m_left_rear_cloud = left_rear;
+        m_right_rear_cloud = right_rear;
+        m_costs_lf = 0.0;
+        m_costs_rf = 0.0;
+        m_costs_lr = 0.0;
+        m_costs_rr = 0.0;
+    }
+    template <typename T>
+    bool operator()(const T* const variable, T* residual) const {
+        // 每一轮重新初始化残差值
+        T costs[4]={T(0),T(0),T(0),T(0)};
+
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+        T length = variable[3];
+        T width = variable[4];
+        T theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<T> rotation_front(theta_front);
+        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        const GridArrayAdapter adapter(m_map);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        double rows = m_map.rows;
+        double cols = m_map.cols;
+        //左前轮
+        int left_front_num = m_left_front_cloud.size();
+        for (int i = 0; i < m_left_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
+                                               (T(m_left_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[i]);
+            costs[0] += residual[i];
+
+        }
+        //右前轮
+        int right_front_num = m_right_front_cloud.size();
+        for (int i = 0; i < m_right_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
+                                               (T(m_right_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + i]);
+            costs[1] += residual[left_front_num+i];
+        }
+        //左后轮
+        int left_rear_num = m_left_rear_cloud.size();
+
+        for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
+                                               (T(m_left_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + i]);
+            costs[2] += residual[left_front_num + right_front_num + i];
+        }
+
+        //右后轮
+
+        for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
+                                               (T(m_right_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + left_rear_num + i]);
+            costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
+        }
+
+        char buf[30];
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[0]);
+        m_costs_lf = std::stod(buf) / left_front_num;
+
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[1]);
+        m_costs_rf = std::stod(buf) / right_front_num;
+
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[2]);
+        m_costs_lr = std::stod(buf) / left_rear_num;
+
+
+        memset(buf, 0, 30);
+        sprintf(buf, "%.7f", costs[3]);
+        m_costs_rr = std::stod(buf) / m_right_rear_cloud.size();
+        // m_costs_lf = costs[0];
+        return true;
+    }
+    void get_costs(double &lf, double &rf, double &lr, double &rr)
+    {
+        lf = m_costs_lf;
+        rf = m_costs_rf;
+        lr = m_costs_lr;
+        rr = m_costs_rr;
+    }
+};
+
+bool detect_wheel_ceres::update_mat(int rows, int cols)
+{
+    /////创建地图
+    int L=std::min(rows,cols);
+    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
+    //map=map*255;
+    cv::Point center(L/2,L/2);
+
+
+    float K=L*0.08;
+    // 从中心开始向外画矩形
+    // 内层高斯，外层二次函数
+    for(int n=0;n<L;n+=2)
+    {
+        cv::Size size(n+2,n+2);
+        cv::Rect rect(center-cv::Point(n/2,n/2),size);
+        double x = n / double(L);
+        double sigma = 0.06;
+        double miu = 0.0;
+        double scale = 0.02;
+        double translation = -0.05;
+        double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
+        double quadratic_value = x-0.08;
+        // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
+        // 对内外分别取色
+        cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));
+
+        // if(n<K*2){
+        //     cv::rectangle(map,rect,1.0*float(K*2-n)/float(L));
+        // }
+        // else{
+        //     cv::rectangle(map,rect,float(n-K*2)/float(L));
+        // }
+
+    }
+    cv::resize(map,map,cv::Size(cols,rows));
+    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
+}
+
+detect_wheel_ceres::detect_wheel_ceres()
+{
+    int cols=800;
+    int rows=200;
+    update_mat(rows, cols);
+}
+detect_wheel_ceres::~detect_wheel_ceres(){}
+
+
+
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result)
+{
+    //清理点云
+    m_left_front_cloud.clear();
+    m_right_front_cloud.clear();
+    m_left_rear_cloud.clear();
+    m_right_rear_cloud.clear();
+    //重新计算点云,按方位分割
+    //第一步,计算整体中心,主轴方向
+    pcl::PointCloud<pcl::PointXYZ> cloud_all;
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        cloud_all+=cloud_vec[i];
+    }
+
+    // std::cout<<"cloud size: "<<cloud_all.size()<<std::endl;
+
+    if(cloud_all.size()<20)
+        return false;
+
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(cloud_all, centroid);
+    double center_x=centroid[0];
+    double center_y=centroid[1];
+    //计算外接旋转矩形
+    std::vector<cv::Point2f> points_cv;
+    for(int i=0;i<cloud_all.size();++i)
+    {
+        points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
+    }
+    cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
+    //计算旋转矩形与X轴的夹角
+    cv::Point2f vec;
+    cv::Point2f vertice[4];
+    rotate_rect.points(vertice);
+
+    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+    // 寻找长边，倾角为长边与x轴夹角
+    if (len1 > len2)
+    {
+        vec.x = vertice[0].x - vertice[1].x;
+        vec.y = vertice[0].y - vertice[1].y;
+    }
+    else
+    {
+        vec.x = vertice[1].x - vertice[2].x;
+        vec.y = vertice[1].y - vertice[2].y;
+    }
+    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+    // printf("rect theta: %.3f\n",angle_x);
+    //第二步, 将没分点云旋转回去,计算点云重心所在象限
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
+        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
+        // 平移
+        traslation.translation() << -center_x, -center_y, 0.0;
+        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
+        pcl::transformPointCloud(cloud, translate_cloud, traslation);
+
+        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
+        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
+        rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));
+
+        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
+        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
+
+        //计算重心
+        Eigen::Vector4f centroid;
+        pcl::compute3DCentroid(transformed_cloud, centroid);
+        double x=centroid[0];
+        double y=centroid[1];
+        //计算象限
+        if(x>0&&y>0)
+        {
+            //第一象限
+            m_left_front_cloud=cloud;
+        }
+        if(x>0 && y<0)
+        {
+            //第四象限
+            m_right_front_cloud=cloud;
+        }
+        if(x<0 && y>0)
+        {
+            //第二象限
+            m_left_rear_cloud=cloud;
+        }
+        if(x<0 && y<0)
+        {
+            //第三象限
+            m_right_rear_cloud=cloud;
+
+        }
+
+    }
+
+    // // 仅优化一次，不调整图像比例与角度初值。
+    // detect_result.cx=center_x;
+    // detect_result.cy=center_y;
+    // detect_result.theta=-angle_x*M_PI/180.0;
+    // Loss_result one_shot_loss_result;
+    // return Solve(detect_result, one_shot_loss_result);
+
+    // 多次优化，获取最佳优化结果
+    detect_result.cx=center_x;
+    detect_result.cy=center_y;
+    // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
+    int calc_count=0;
+    // double final_theta = 0;
+    // 误差结构体，保存左前、右前、左后、右后、整体平均误差
+    Loss_result loss_result;
+    // 平均误差值，用于获取最小整体平均误差
+    double avg_loss = 100;
+    // 定义图像列数，控制图像大小
+    int map_cols = 800;
+    // 优化后图像行数，用于保存优化后结果图像
+    int optimized_map_rows = 200;
+    // 优化结果
+    bool solve_result = false;
+    double total_solve_time = 0;
+    bool stop_sign = false;
+    for (int j = 2; j < 4&&!stop_sign; j++)
+    {
+        // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
+        double map_rows = map_cols * 1.0 / j ;
+        update_mat(map_rows, map_cols);
+        Detect_result t_detect_result = detect_result;
+        // 寻找最小loss值对应的初始旋转角
+        for (int i = -5; i < 6&&!stop_sign; i+=2)
+        {
+            t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
+            // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
+            Loss_result t_loss_result;
+            t_loss_result.total_avg_loss = 1000;
+            //输出角度已变化，需恢复成弧度
+            if(calc_count > 0)
+            {
+                // t_detect_result.theta *= (-M_PI) / 180.0;
+                t_detect_result.front_theta *= (-M_PI) / 180.0;
+            }
+            auto t1=std::chrono::system_clock::now();
+            bool current_result = Solve(t_detect_result, t_loss_result);
+            auto t2=std::chrono::system_clock::now();
+            auto duration=t2-t1;
+            static double second=0.0;
+            second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
+            total_solve_time+=second;
+            // std::cout<<" time "<<second<<std::endl;
+            // std::cout<<"current_result: "<<current_result<<std::endl;
+            if(!current_result)
+                continue;
+            // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
+            if (avg_loss > t_loss_result.total_avg_loss)
+            {
+                avg_loss = t_loss_result.total_avg_loss;
+                // final_theta = -input_vars[2] * M_PI / 180.0;
+                detect_result = t_detect_result;
+                solve_result = current_result;
+                loss_result = t_loss_result;
+                optimized_map_rows = map_rows;
+                calc_count++;
+                /*// 新增，优化时间足够长则认为已找到正确结果
+                if(second > 0.02)
+                {
+                    stop_sign = true;
+                    break;
+                }*/
+            }
+        }
+    } 
+    // std::cout<<"solve time "<<total_solve_time<<std::endl;
+    // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
+
+//    if(solve_result)
+//        printf("final avg cost for four wheels: %7.6f %7.6f %7.6f %7.6f---%7.6f\n", loss_result.lf_loss, loss_result.rf_loss, loss_result.lb_loss, loss_result.rb_loss, loss_result.total_avg_loss);
+    
+    // // 生成并保存图片
+    // update_mat(optimized_map_rows, map_cols);
+    // Detect_result t_detect_result = detect_result;
+    // t_detect_result.theta *= (-M_PI) / 180.0;
+    // t_detect_result.front_theta *= (-M_PI) / 180.0;
+    // Loss_result t_loss_result;
+    // // LOG(INFO) <<"going to show img ";
+    // Solve(detect_result, t_loss_result, true);
+
+    return solve_result;
+
+}
+
+// 根据测量结果，生成四轮各中心十字星点云
+void detect_wheel_ceres::transform_src(Detect_result detect_result)
+{
+
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(-detect_result.theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, detect_result.width / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, detect_result.width / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
+
+
+    Eigen::Matrix<double, 2, 1> translate_car(detect_result.cx,detect_result.cy);
+
+    const Eigen::Matrix<double, 2, 1> lt_center = rotation_matrix * wheel_center_normal_left_front+translate_car;
+    const Eigen::Matrix<double, 2, 1> rt_center = rotation_matrix * wheel_center_normal_right_front+translate_car;
+    const Eigen::Matrix<double, 2, 1> lb_center = rotation_matrix * wheel_center_normal_left_rear+translate_car;
+    const Eigen::Matrix<double, 2, 1> rb_center = rotation_matrix * wheel_center_normal_right_rear+translate_car;
+
+    create_mark(lt_center(0,0),lt_center(1,0),-detect_result.theta-detect_result.front_theta,m_left_front_cloud_out);
+    create_mark(rt_center(0,0),rt_center(1,0),-detect_result.theta-detect_result.front_theta,m_right_front_cloud_out);
+    create_mark(lb_center(0,0),lb_center(1,0),-detect_result.theta,m_left_rear_cloud_out);
+    create_mark(rb_center(0,0),rb_center(1,0),-detect_result.theta,m_right_rear_cloud_out);
+}
+
+// 创建车轮中心十字星点云
+void detect_wheel_ceres::create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud_out)
+{
+    cloud_out.clear();
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+
+    //std::cout<<"1111111111"<<std::endl;
+    int width=30;
+    int height=10;
+    double step=0.015;
+
+    for(int i=0;i<width;++i)
+    {
+        pcl::PointXYZ point((i-width/2)*step,0,0);
+        cloud.push_back(point);
+    }
+    for(int i=0;i<height;++i)
+    {
+        pcl::PointXYZ point(0,(i-height/2)*step,0);
+        cloud.push_back(point);
+    }
+    //std::cout<<"22222222222"<<std::endl;
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    for(int i=0;i<cloud.size();++i) {
+
+        const Eigen::Matrix<double, 2, 1> point((double(cloud[i].x)),
+                                                (double(cloud[i].y)));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point;
+        pcl::PointXYZ point_out(tanslate_point(0,0)+x,tanslate_point(1,0)+y,0);
+        cloud_out.push_back(point_out);
+
+    }
+}
+
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
+{
+    double SCALE=300.0;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_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.max_num_iterations=60;
+    options.num_threads=1;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+
+    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
+           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
+
+    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=(-variable[2])*180.0/M_PI;
+    detect_result.wheel_base=variable[3];
+    detect_result.width=variable[4];
+    detect_result.front_theta=-(variable[5]*180.0/M_PI);
+
+    if(detect_result.theta>180.0)
+        detect_result.theta=detect_result.theta-180.0;
+    if(detect_result.theta<0)
+        detect_result.theta+=180.0;
+
+    //检验
+    if(loss>0.01)
+        return false;
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
+    {
+        return false;
+    }
+
+    detect_result.width+=0.15;//车宽+10cm
+    // printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+
+    // //added by yct
+    avg_loss = loss; // 将loss传出
+
+    Detect_result t_detect_result = detect_result;
+    t_detect_result.theta *= -M_PI/180.0;
+    t_detect_result.front_theta *= -M_PI/180.0;
+    transform_src(t_detect_result);
+
+    return true;
+
+}
+
+
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
+{
+    double SCALE=300.0;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+    // printf("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;
+    CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    cost_func,
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_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.max_num_iterations=60;
+    options.num_threads=3;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+    // debug_img(detect_result, loss_result, true);
+    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=(-variable[2])*180.0/M_PI;
+    detect_result.wheel_base=variable[3];
+    detect_result.width=variable[4];
+    detect_result.front_theta=-(variable[5]*180.0/M_PI);
+
+    if(detect_result.theta>180.0)
+        detect_result.theta=detect_result.theta-180.0;
+    if(detect_result.theta<0)
+        detect_result.theta+=180.0;
+
+    //检验
+    // printf("loss: %.5f\n", loss);
+    // printf("middle 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]);
+    if(loss>0.0115)
+    {
+//        LOG(WARNING) <<"总loss过大 "<<loss;
+        return false;
+    }
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.150 || detect_result.wheel_base < 2.200)
+    {
+//        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.15): "<<detect_result.width<<", "<<detect_result.wheel_base;
+        return false;
+    }
+
+    detect_result.width += 0.15; //车宽+10cm
+    // printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+
+    // //added by yct
+    if(detect_result.theta > 120 || detect_result.theta < 60)
+    {
+//        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
+        return false;
+    }
+    if(fabs(detect_result.front_theta)>35)
+    {
+//        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
+        return false;
+    }
+    // 将loss传出
+    if(cost_func!=nullptr)
+    {
+        
+        double costs_lf, costs_rf, costs_lr, costs_rr;
+        cost_func->get_costs(costs_lf, costs_rf, costs_lr, costs_rr);
+        // std::cout << "111"<< std::endl;
+        loss_result.lf_loss = costs_lf;
+        loss_result.rf_loss = costs_rf;
+        loss_result.lb_loss = costs_lr;
+        loss_result.rb_loss = costs_rr;
+        loss_result.total_avg_loss = loss;
+        // std::cout << "222"<< std::endl;
+        if (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
+        {
+            loss_result.lf_loss = loss;
+            loss_result.rf_loss = loss;
+            loss_result.lb_loss = loss;
+            loss_result.rb_loss = loss;
+        }
+        // 判断每个轮子平均loss是否满足条件
+        double single_wheel_loss_threshold = 0.109;
+        if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
+        {
+//            LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
+            return false;
+        }
+
+        // std::cout<<"save img: "<<save_img<<std::endl;
+//        debug_img(detect_result, loss_result, save_img);
+    }
+    else
+    {
+        return false;
+    }
+
+    Detect_result t_detect_result = detect_result;
+    t_detect_result.theta *= -M_PI/180.0;
+    t_detect_result.front_theta *= -M_PI/180.0;
+    t_detect_result.width -= 0.15;
+    transform_src(t_detect_result);
+
+    return true;
+}
+
+// 显示/保存图片供调试用
+void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, std::string out_img_path)
+{
+    double SCALE=300.0;
+    cv::Mat lf = m_map.clone();
+    cv::Mat rf = m_map.clone();
+    cv::Mat lb = m_map.clone();
+    cv::Mat rb = m_map.clone();
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(detect_result.theta);
+
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, (detect_result.width - 0.15) / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, (-detect_result.width + 0.15) / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, (detect_result.width - 0.15) / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, (-detect_result.width + 0.15) / 2.0);
+
+    //前轮旋转
+    Eigen::Rotation2D<double> rotation_front(detect_result.front_theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+    // 左前轮
+    for (size_t i = 0; i < m_left_front_cloud.size(); i++)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lf, cv::Point(c, r), 1, cv::Scalar(150));
+        // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
+        // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
+        // lf.at<uchar>(r, c) = 150;
+    }
+    //右前轮
+    for (int i = 0; i < m_right_front_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+    //左后轮
+    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    //右后轮
+    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    // cv::Mat rot90;// = (cv::Mat_<double>(2, 3)<<cos(90), -sin(90), 0, sin(90), cos(90), 0);
+    // rot90 = cv::getRotationMatrix2D(cv::Point2f(lf.cols/2, lf.rows/2), 90, 1);
+    // // std::cout<<rot90<<std::endl;
+    // cv::warpAffine(lf, lf, rot90, cv::Size(lf.cols, lf.rows));
+    // cv::flip()
+    // cv::namedWindow("left front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("left back", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right back", cv::WINDOW_FREERATIO);
+    cv::flip(lf, lf, -1);
+    cv::flip(rf, rf, -1);
+    cv::flip(lb, lb, -1);
+    cv::flip(rb, rb, -1);
+    cv::Mat lft = lf.t(), rft = rf.t(), lbt = lb.t(), rbt = rb.t();
+    // cv::imshow("left front", lf.t());
+    // cv::imshow("right front", rf.t());
+    // cv::imshow("left back", lb.t());
+    // cv::imshow("right back", rb.t());
+    // 写入各轮平均误差
+    cv::putText(lft, (boost::format("lf %.6f") % (loss_result.lf_loss)).str().c_str(),
+                cv::Point(lft.cols / 6, lft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rft, (boost::format("rf %.6f") % (loss_result.rf_loss)).str().c_str(),
+                cv::Point(rft.cols / 6, rft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(lbt, (boost::format("lb %.6f") % (loss_result.lb_loss)).str().c_str(),
+                cv::Point(lbt.cols / 6, lbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rbt, (boost::format("rb %.6f") % (loss_result.rb_loss)).str().c_str(),
+                cv::Point(rbt.cols / 6, rbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+
+    cv::Mat total_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, m_map.type());
+    lft.copyTo(total_img(cv::Rect(0, 0, m_map.rows, m_map.cols)));
+    rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
+    lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
+    rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
+    // cv::namedWindow("total img", CV_WINDOW_FREERATIO);
+    // cv::imshow("total img", total_img);
+    // cv::waitKey(20);
+    if(save_img)
+    {
+        cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
+        int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
+        std::string img_filename="";
+        if(out_img_path.empty()){
+            img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
+        }
+        else{
+            img_filename = out_img_path;
+        }
+        LOG(INFO) << "write to " << img_filename.c_str();
+        // cv::cvtColor(total_img*255, cvted_img, CV_8U);
+        cv::convertScaleAbs(total_img * 255, cvted_img);
+        cv::imwrite(img_filename, cvted_img);
+    }
+}

wanji_lidar/detect_wheel_ceres.h

@@ -0,0 +1,90 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
+#define CERES_TEST_DETECT_WHEEL_CERES_H
+
+#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"
+
+class detect_wheel_ceres {
+public:
+    struct Detect_result
+    {
+        public: 
+        double cx = 0;
+        double cy = 0;
+        double theta = 0;
+        double wheel_base = 0;
+        double width = 0;
+        double front_theta = 0;
+        bool success = false;
+        Detect_result& operator=(const Detect_result detect_result)
+        {
+            this->cx = detect_result.cx;
+            this->cy = detect_result.cy;
+            this->theta = detect_result.theta;
+            this->wheel_base = detect_result.wheel_base;
+            this->width = detect_result.width;
+            this->front_theta = detect_result.front_theta;
+            this->success = detect_result.success;
+            return *this;
+        }
+    };
+    struct Loss_result
+    {
+        public: 
+        double lf_loss = 0;
+        double rf_loss = 0;
+        double lb_loss = 0;
+        double rb_loss = 0;
+        double total_avg_loss = 0;
+        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;
+        }
+    };
+    detect_wheel_ceres();
+    ~detect_wheel_ceres();
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result);
+
+protected:
+    void transform_src(Detect_result detect_result);
+    bool update_mat(int rows, int cols);
+    bool Solve(Detect_result &detect_result, double &avg_loss);
+    bool Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img=false);
+    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");
+
+    void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
+public:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud_out;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud_out;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud_out;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud_out;     //右后点云
+
+protected:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+    cv::Mat                                 m_map;                  //保存一份地图用作匹配
+
+
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

wanji_lidar/region_detect.cpp

@@ -0,0 +1,551 @@
+//
+// Created by zx on 2019/12/6.
+//
+#include "region_detect.h"
+
+/**
+ * 有参构造函数
+ * */
+Region_detector::Region_detector()
+{
+
+}
+
+/**
+ * 析构函数
+ * */
+Region_detector::~Region_detector()
+{
+}
+
+Error_manager Region_detector::init(Point2D_tool::Point2D_box point2D_box)
+{
+	m_region_box = point2D_box;
+	return Error_code::SUCCESS;
+}
+
+
+/**
+ * 预处理，xy直通滤波与离群点过滤
+ * 返回 PARAMETER_ERROR，WJ_REGION_EMPTY_CLOUD 或 SUCCESS
+ * */
+Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+    // 1.参数合法性检验
+    if (p_cloud->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
+    // 2.直通滤波, 筛选xy
+    pcl::PassThrough<pcl::PointXYZ> pass;
+    pass.setInputCloud(p_cloud);
+    pass.setFilterFieldName("x");                                       //设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*p_cloud);                                            //输出到结果指针
+std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
+    if (p_cloud->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    pass.setInputCloud(p_cloud);
+    pass.setFilterFieldName("y");                                       //设置想在哪个坐标轴上操作
+    pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
+    pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
+    pass.filter(*p_cloud);                                            //输出到结果指针
+std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
+    if (p_cloud->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
+    // 3.离群点过滤
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+    sor.setInputCloud(p_cloud);
+    sor.setMeanK(15);            //K近邻搜索点个数
+    sor.setStddevMulThresh(3.0); //标准差倍数
+    sor.setNegative(false);      //保留未滤波点（内点）
+    sor.filter(*p_cloud);      //保存滤波结果到cloud_filter
+std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
+    if (p_cloud->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+    else
+        return Error_manager(Error_code::SUCCESS);
+}
+
+/**
+ * 返回二维点之间距离
+ * */
+double distance(cv::Point2f p1, cv::Point2f p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+/**
+ * 判断是否符合标准矩形，
+ * 传入矩形四个角点，以及是否打印
+ * 返回 WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
+ * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
+ * */
+Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool print)
+{
+    if (points.size() == 4)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        // 顶点
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i)
+        {
+            if (L[i] > max_l)
+            {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+        // 顶角大小
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        // 顶角相对于90度过大或小
+        if (fabs(cosa) >= 0.15 /* || std::min(l1, l2) > 2.0 || std::max(l1, l2) > 3.3*/)
+        {
+            if (print)
+            {
+                LOG(WARNING) << " angle cos >0.13 =" << cosa << "  i=" << max_index;
+                // LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            }
+            return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
+        }
+
+        float width = std::min(l1, l2);
+        float length = std::max(l1, l2);
+        // 车宽应位于[1.35, 2.0]，车长应位于[2.2, 3.0]
+        if (width < 1.350 || width > 2.000 || length > 3.000 || length < 2.200)
+        {
+            if (print)
+            {
+                LOG(WARNING) << "\t width<1350 || width >2100 || length >3300 ||length < 2100 "
+                             << "  length:" << length << "  width:" << width;
+            }
+            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
+        }
+
+        double d = distance(pc, points[3]);
+        cv::Point2f center1 = (ps + pt) * 0.5;
+        cv::Point2f center2 = (pc + points[3]) * 0.5;
+        // 对角线形变超过0.15倍，或中心点距离偏差0.15m
+        if (fabs(d - max_l) > max_l * 0.15 || distance(center1, center2) > 0.150)
+        {
+            if (print)
+            {
+                LOG(WARNING) << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
+                             << "  center distance=" << distance(center1, center2);
+            }
+            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SYMMETRY_ERROR);
+        }
+        if (print)
+        {
+            LOG(INFO) << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
+                         << "  center distance=" << distance(center1, center2);
+        }
+        return Error_manager(Error_code::SUCCESS);
+    }
+    // 以三个点进行验证
+    else if (points.size() == 3)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i)
+        {
+            if (L[i] > max_l)
+            {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        // 顶角应接近90度
+        if (fabs(cosa) >= 0.15)
+        {
+            if (print)
+            {
+                LOG(WARNING) << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
+                LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
+            }
+            return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
+        }
+
+        double length = std::max(l1, l2);
+        double width = std::min(l1, l2);
+        // 车宽应位于[1.4, 2.0]，车长应位于[2.2, 3.0]
+        if (length > 2.100 && length < 3.000 && width > 1.400 && width < 2.100)
+        {
+            //生成第四个点
+            cv::Point2f vec1 = ps - pc;
+            cv::Point2f vec2 = pt - pc;
+            cv::Point2f point4 = (vec1 + vec2) + pc;
+            points.push_back(point4);
+            if (print)
+            {
+                LOG(WARNING) << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << length
+                             << "  w=" << width;
+            }
+            return Error_manager(Error_code::SUCCESS);
+        }
+        else
+        {
+            if (print)
+            {
+                LOG(WARNING) << "3 wheels rectangle verify Failed  cos angle=" << cosa << "  L=" << length
+                             << "  w=" << width;
+            }
+            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
+        }
+    }
+    else
+    {
+        return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR);
+    }
+}
+
+/*
+ * 仅仅聚类
+ */
+Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+        std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
+{
+    // 1.判断参数合法性
+    if (cloud_in->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    // 2.聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cloud_in);
+
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
+    // 设置聚类的最小值 2cm (small values may cause objects to be divided
+    // in several clusters, whereas big values may join objects in a same cluster).
+    clustering.setClusterTolerance(0.5);
+    // 设置聚类的小点数和最大点云数
+    clustering.setMinClusterSize(10);
+    clustering.setMaxClusterSize(500);
+    clustering.setSearchMethod(kdtree);
+    clustering.setInputCloud(cloud_in);
+    std::vector<pcl::PointIndices> clusters;
+    clustering.extract(clusters);
+    if(clusters.size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    for (int i = 0; i < clusters.size(); ++i)
+    {
+        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>());
+    }
+
+    int j = 0;
+    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+        {
+            cloud_cluster->points.push_back(cloud_in->points[*pit]);
+            cloud_cluster->width = cloud_cluster->points.size();
+            cloud_cluster->height = 1;
+            cloud_cluster->is_dense = true;
+        }
+        seg_clouds[j] = *cloud_cluster;
+        j++;
+    }
+
+    if (print)
+    {
+        LOG(INFO) << " cluster classes:" << clusters.size();
+    }
+
+    return SUCCESS;
+}
+
+
+/**
+ * 聚类并通过矩形判断函数
+ * 传入原始点云，传出角点与聚类出的点云
+ * 返回 WJ_REGION_EMPTY_CLOUD, WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
+ * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
+ * */
+Error_manager Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f> &corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print)
+{
+    // 1.判断参数合法性
+    if (cloud_in->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    // 2.聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cloud_in);
+
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
+    // 设置聚类的最小值 2cm (small values may cause objects to be divided
+    // in several clusters, whereas big values may join objects in a same cluster).
+    clustering.setClusterTolerance(0.2);
+    // 设置聚类的小点数和最大点云数
+    clustering.setMinClusterSize(10);
+    clustering.setMaxClusterSize(500);
+    clustering.setSearchMethod(kdtree);
+    clustering.setInputCloud(cloud_in);
+    std::vector<pcl::PointIndices> clusters;
+    clustering.extract(clusters);
+    if(clusters.size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    for (int i = 0; i < clusters.size(); ++i)
+    {
+        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    }
+
+    int j = 0;
+    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+        {
+            cloud_cluster->points.push_back(cloud_in->points[*pit]);
+            cloud_cluster->width = cloud_cluster->points.size();
+            cloud_cluster->height = 1;
+            cloud_cluster->is_dense = true;
+        }
+        seg_clouds[j] = cloud_cluster;
+        j++;
+    }
+
+    if (print)
+    {
+        LOG(INFO) << " cluster classes:" << clusters.size();
+    }
+
+    // 3.分别计算每团点云几何中心，作为矩形角点
+    corner_points.clear();
+    for (int i = 0; i < clusters.size(); ++i)
+    {
+        if (seg_clouds.size() <= i || seg_clouds[i]->size() == 0)
+            continue;
+        float sumX = 0, sumY = 0;
+        for (int j = 0; j < seg_clouds[i]->size(); ++j)
+        {
+            sumX += seg_clouds[i]->points[j].x;
+            sumY += seg_clouds[i]->points[j].y;
+        }
+
+        float center_x = sumX / float(seg_clouds[i]->size());
+        float center_y = sumY / float(seg_clouds[i]->size());
+        corner_points.push_back(cv::Point2f(center_x, center_y));
+    }
+    /*char buf[255]={0};
+    for (int k = 0; k < corner_points.size(); ++k) {
+        sprintf(buf+strlen(buf), "point %d: (%.2f, %.2f)__", k, corner_points[k].x, corner_points[k].y);
+    }
+    LOG(WARNING) << buf;*/
+
+    cv::RotatedRect t_rect=cv::minAreaRect(corner_points);
+    //display(t_rect,cv::Scalar(255,0,0));
+    return isRect(corner_points, print);
+}
+
+
+
+
+// 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
+Error_manager Region_detector::detect(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+Common_data::Car_wheel_information& car_wheel_information, bool print)
+{
+	//整个函数都加锁, 这里面非法修改了输入点云, 是为了将筛选点云的结构传出, 然后点云存txt文件
+	std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
+	Error_manager result = Error_manager(Error_code::SUCCESS);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+	*p_cloud_filtered = *p_cloud_in;
+
+    // 1.预处理
+    result = preprocess(p_cloud_filtered);
+    if (result != SUCCESS)
+        return result;
+
+    // 2.聚类计算角点
+    std::vector<cv::Point2f> corner_points;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
+    result = clustering(p_cloud_filtered, corner_points, seg_clouds, print);
+    if (result != SUCCESS)
+		return result;
+	// 修改原始点云为预处理后点云，并加入角点供查验
+
+	p_cloud_in->clear();
+	p_cloud_in->operator+=(*p_cloud_filtered);
+	for (size_t i = 0; i < corner_points.size(); i++)
+	{
+		p_cloud_in->points.push_back(pcl::PointXYZ(corner_points[i].x, corner_points[i].y, 0.0));
+	}
+
+	// convert all points after preprocessed into 2d
+	std::vector<cv::Point2f> all_points;
+	// car center
+	for (int j = 0; j < p_cloud_filtered->size(); ++j)
+    {
+        all_points.push_back(cv::Point2f(p_cloud_filtered->points[j].x, p_cloud_filtered->points[j].y));
+    }
+    // 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;
+    // 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));
+
+
+
+    // 长边向量
+    cv::Point2f vec;
+    cv::Point2f vertice[4];
+    wheel_box.points(vertice);
+
+    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+    // 寻找长边，倾角为长边与x轴夹角
+    if (len1 > len2)
+    {
+        vec.x = vertice[0].x - vertice[1].x;
+        vec.y = vertice[0].y - vertice[1].y;
+    }
+    else
+    {
+        vec.x = vertice[1].x - vertice[2].x;
+        vec.y = vertice[1].y - vertice[2].y;
+    }
+    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+	car_wheel_information.car_angle = angle_x;
+
+    // get line formula, normal is (cos(theta), sin(theta)), towards the long side
+    // 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;
+    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]
+    //     [ sin  cos]
+    float rotate_angle = M_PI_2 - acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+    Eigen::Matrix2f rotation_matrix;
+    rotation_matrix << cos(rotate_angle), -sin(rotate_angle), sin(rotate_angle), cos(rotate_angle);
+
+    // find min x and max x, separate y values according to the line, calculate difference between mean of y values as wheelbase
+    float min_x = 20, max_x = 0;
+    float y_values0 = 0, y_values1 = 0;
+    int count0 = 0, count1 = 0;
+    for (size_t i = 0; i < seg_clouds.size(); i++)
+    {
+        if (seg_clouds[i]->size() <= 0)
+            continue;
+        for (size_t j = 0; j < seg_clouds[i]->size(); j++)
+        {
+            // origin point and the point rotated around the origin
+            Eigen::Vector2f vec, vec_rot;
+            vec << seg_clouds[i]->points[j].x, seg_clouds[i]->points[j].y;
+            vec_rot = rotation_matrix * (vec - center_point) + center_point;
+            // find min max x
+            if (vec_rot[0] < min_x)
+                min_x = vec_rot[0];
+            if (vec_rot[0] > max_x)
+                max_x = vec_rot[0];
+            // separate point as two clusters(front and back), calc y values respectively
+            if (normal.transpose() * vec + line_param_c > 0)
+            {
+                y_values0 += vec_rot[1];
+                count0 += 1;
+            }
+            else
+            {
+                y_values1 += vec_rot[1];
+                count1 += 1;
+            }
+        }
+    }
+    // check if front and back both has points
+    if (count0 > 0 && count1 > 0)
+    {
+        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);
+//        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));
+    }
+    else
+    {
+        // calculate wheelbase according to corner points
+        // 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);
+    }
+
+    return Error_manager(Error_code::SUCCESS);
+}
+
+
+
+//通过ceres检测中心,旋转,轴距,宽度,  新算法, 可以测量前轮旋转
+Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+										 Common_data::Car_wheel_information& car_wheel_information, bool print)
+{
+	Error_manager result = Error_manager(Error_code::SUCCESS);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+	{
+		std::unique_lock<std::mutex> t_lock(*p_cloud_mutex);
+		*p_cloud_filtered = *p_cloud_in;
+	}
+	// 1.预处理
+	result = preprocess(p_cloud_filtered);
+	if (result != SUCCESS)
+		return result;
+	std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
+	// 2.聚类
+	std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
+	result = clustering_only(p_cloud_filtered, seg_clouds, print);
+	if (result != SUCCESS)
+		return result;
+	std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
+	detect_wheel_ceres::Detect_result detect_result;
+	if(!m_detector_ceres.detect(seg_clouds, detect_result))
+		return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR);
+	else {
+		car_wheel_information.center_x = detect_result.cx;
+		car_wheel_information.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.correctness = detect_result.success;
+	}
+
+	return Error_manager(Error_code::SUCCESS);
+}

wanji_lidar/region_detect.h

@@ -0,0 +1,87 @@
+//
+// Created by zx on 2019/12/6.
+//
+#ifndef REGION_DETECT_H
+#define REGION_DETECT_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <mutex>
+#include <vector>
+#include <atomic>
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/Dense"
+
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/common/centroid.h>
+#include <pcl/common/common.h>
+
+#include <boost/thread.hpp>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+#include "glog/logging.h"
+#include "../error_code/error_code.h"
+#include "wj_lidar_conf.pb.h"
+
+//#include "../tool/StdCondition.h"
+#include "opencv2/opencv.hpp"
+#include "detect_wheel_ceres.h"
+#include "../tool/point2D_tool.h"
+#include "../tool/common_data.h"
+
+/**
+ * 万集区域检测算法类
+ * */
+class Region_detector
+{
+public:
+    // 有参构造函数
+    Region_detector();
+    // 析构函数
+    ~Region_detector();
+
+	Error_manager init(Point2D_tool::Point2D_box point2D_box);
+
+    // 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
+    Error_manager detect(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+    Common_data::Car_wheel_information& car_wheel_information, bool print=true);
+    //通过ceres检测中心,旋转,轴距,宽度,  新算法, 可以测量前轮旋转
+    Error_manager detect_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+    Common_data::Car_wheel_information& car_wheel_information, bool print=true);
+
+protected:
+    // 预处理，直通滤波限制点云范围
+    Error_manager preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
+    // 点云聚类，寻找四类点云并检验是否近似矩形
+    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points,
+            std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
+    //仅仅聚类
+    Error_manager clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                             std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print=false);
+    // 判断是否足够近似矩形
+    Error_manager isRect(std::vector<cv::Point2f>& points, bool print=false);
+
+protected:
+	Point2D_tool::Point2D_box		m_region_box;		// 区域范围参数
+
+    detect_wheel_ceres              m_detector_ceres;   //ceres优化对象
+
+};
+
+#endif //REGION_DETECT_H

wanji_lidar/region_worker.cpp

@@ -0,0 +1,149 @@
+//
+// Created by zx on 2019/12/9.
+//
+#include "region_worker.h"
+//#include "plc_data.h"
+
+Region_worker::Region_worker()
+{
+	m_region_worker_status = E_UNKNOW;
+
+	mp_cloud_collection_mutex = NULL;
+	mp_cloud_collection = NULL;
+	mp_auto_detect_thread = NULL;
+}
+
+/**
+ * 析构
+ * */
+Region_worker::~Region_worker()
+{
+
+}
+
+Error_manager Region_worker::init(Point2D_tool::Point2D_box point2D_box, std::mutex* p_cloud_collection_mutex,
+								  pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_collection)
+{
+	m_detector.init(point2D_box);
+
+	mp_cloud_collection_mutex = p_cloud_collection_mutex;
+	mp_cloud_collection = p_cloud_collection;
+
+	m_auto_detect_condition.reset(false, false, false);
+	mp_auto_detect_thread = new std::thread(&Region_worker::auto_detect_thread_fun, this);
+
+	m_region_worker_status = E_READY;
+
+	return Error_code::SUCCESS;
+}
+
+
+//唤醒自动预测的算法线程
+Error_manager Region_worker::wake_auto_detect()
+{
+	//唤醒一次
+	m_auto_detect_condition.notify_all(false, true);
+	return Error_code::SUCCESS;
+}
+
+// 获得中心点、角度等测量数据
+Error_manager Region_worker::get_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information)
+{
+	return m_detector.detect(p_cloud_mutex, p_cloud_in, car_wheel_information);
+}
+
+
+// 获得中心点、角度等测量数据,  新算法, 可以测量前轮旋转
+Error_manager Region_worker::get_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information)
+{
+    return m_detector.detect_ex(p_cloud_mutex, p_cloud_in, car_wheel_information);
+}
+
+
+
+
+
+cv::RotatedRect Region_worker::create_rotate_rect(float length,float width,float angle,float cx,float cy)
+{
+    const double C_PI=3.14159265;
+    float theta=C_PI*(angle/180.0);
+    float a00=cos(theta);
+    float a01=-sin(theta);
+    float a10=sin(theta);
+    float a11=cos(theta);
+    cv::Point2f point[4];
+    point[0].x=-length/2.0;
+    point[0].y=-width/2.0;
+
+    point[1].x=-length/2.0;
+    point[1].y=width/2.0;
+
+    point[2].x=length/2.0;
+    point[2].y=-width/2.0;
+
+    point[3].x=length/2.0;
+    point[3].y=width/2.0;
+
+    std::vector<cv::Point2f> point_vec;
+    for(int i=0;i<4;++i)
+    {
+        float x=point[i].x*a00+point[i].y*a01+cx;
+        float y=point[i].x*a10+point[i].y*a11+cy;
+        point_vec.push_back(cv::Point2f(x,y));
+    }
+    return cv::minAreaRect(point_vec);
+}
+
+
+//自动预测的线程函数
+void Region_worker::auto_detect_thread_fun()
+{
+	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	while (m_auto_detect_condition.is_alive())
+	{
+		//暂时固定为一个扫描周期, 就循环一次
+		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
+		m_auto_detect_condition.wait();
+		if ( m_auto_detect_condition.is_alive() )
+		{
+//			std::cout << " huli test :::: " << "  = " << "---------------------------------------------------------------------------------" << std::endl;
+//
+//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;
+//
+//			auto start   = std::chrono::system_clock::now();
+
+			t_error = get_wheel_result_ex(mp_cloud_collection_mutex, mp_cloud_collection, m_car_wheel_information);
+
+//			auto end   = std::chrono::system_clock::now();
+//			auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+//			std::cout <<  "花费了"
+//				 << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "毫秒" << std::endl;
+
+			if ( t_error == SUCCESS  )
+			{
+//				std::cout << " huli test :::: " << " x = " << m_car_wheel_information.center_x << std::endl;
+//				std::cout << " huli test :::: " << " y = " << m_car_wheel_information.center_y << std::endl;
+//				std::cout << " huli test :::: " << " c = " << m_car_wheel_information.car_angle << std::endl;
+//				std::cout << " huli test :::: " << " wheelbase = " << m_car_wheel_information.wheel_base << std::endl;
+//				std::cout << " huli test :::: " << " width = " << m_car_wheel_information.wheel_width << std::endl;
+//				std::cout << " huli test :::: " << " front_theta = " << m_car_wheel_information.front_theta << std::endl;
+//				std::cout << " huli test :::: " << " correctness = " << m_car_wheel_information.correctness << std::endl;
+				m_detect_updata_time = std::chrono::system_clock::now();
+			}
+			else
+			{
+//				std::cout << " huli test :::: " << " t_error = " << t_error << std::endl;
+			}
+
+
+//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;
+
+			//huli
+		}
+	}
+	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
+	return;
+}

wanji_lidar/region_worker.h

@@ -0,0 +1,109 @@
+//
+// Created by zx on 2019/12/9.
+//
+
+#ifndef REGION_WORKER_H
+#define REGION_WORKER_H
+
+#include "region_detect.h"
+//#include "../tool/StdCondition.h"
+#include <thread>
+#include <mutex>
+#include <iostream>
+#include <atomic>
+#include <chrono>
+#include "../error_code/error_code.h"
+#include "../verify/Verify_result.h"
+#include "../tool/thread_condition.h"
+#include "../tool/common_data.h"
+
+
+/**
+ * 区域功能类，负责自动检测区域状态并更新到plc
+ * */
+class Region_worker
+{
+public:
+#define REGION_WORKER_RESULT_DEFAULT 0x0000
+//#define REGION_WORKER_VERIFY_OK 0x0001
+//#define REGION_WORKER_EMPTY_SPACE 0x0020
+//#define REGION_WORKER_CLUSTER_SIZE_ERROR 0x0040
+//#define REGION_WORKER_OTHER_ERROR 0x0080
+//#define REGION_WORKER_NULL_POINTER 0x0100
+#define REGION_WORKER_EMPTY_SPACE 1
+#define REGION_WORKER_HAS_CAR 2
+#define REGION_WORKER_DETECT_ERROR 3
+
+enum Region_worker_status
+	{
+		E_UNKNOW               	= 0,    //未知
+		E_READY               	= 1,    //准备，待机
+		E_BUSY					= 2, 	//工作正忙
+
+		E_FAULT					= 10,	//故障
+	};
+public:
+
+	Region_worker();
+    ~Region_worker();
+
+	Error_manager init(Point2D_tool::Point2D_box point2D_box, std::mutex* p_cloud_collection_mutex,
+					   pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_collection);
+
+	//唤醒自动预测的算法线程
+	Error_manager wake_auto_detect();
+
+    // 获得中心点、角度等测量数据
+    Error_manager get_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+    Common_data::Car_wheel_information& car_wheel_information);
+
+    // 获得中心点、角度等测量数据,  新算法, 可以测量前轮旋转
+    Error_manager get_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+    Common_data::Car_wheel_information& car_wheel_information);
+
+protected:
+    static cv::RotatedRect create_rotate_rect(float length,float width,float angle,float x,float y);
+
+    //自动预测的线程函数
+    void auto_detect_thread_fun();
+
+protected:
+	//状态
+	Region_worker_status						m_region_worker_status;			//区域功能的状态
+
+    Region_detector 							m_detector;                             // 区域检测算法实例
+
+	//万集雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息.
+	std::mutex*	 								mp_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
+	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
+
+	Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
+	std::chrono::system_clock::time_point		m_detect_updata_time;			//定位结果的更新时间.
+
+	std::thread*        						mp_auto_detect_thread;   		//自动预测的线程指针，内存由本类管理
+	Thread_condition							m_auto_detect_condition;		//自动预测的条件变量
+
+
+
+
+
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;             // 自动区域检测用点云
+//    std::atomic<bool> mb_cloud_updated;                      // 点云更新指标
+////
+////    std::chrono::steady_clock::time_point m_update_plc_time; // 更新plc状态时刻
+////    int m_last_sent_code;                              // 上次写入plc的状态值
+////    int m_last_read_code;                              // 上次检查获得的状态值
+////    int m_last_border_status;				// 上次超界提示
+////    std::atomic<int> m_read_code_count;                      // 检查后重复获取相同状态值次数
+//
+//    Thread_condition m_cond_exit;     // 系统退出标志
+//    std::thread *m_detect_thread; // 实时检测线程
+//    std::mutex m_mutex;           // 点云互斥锁
+//
+//    Verify_result* mp_verify_handle; // 边缘检测
+};
+
+#endif //REGION_WORKER_H
+
+
+

wanji_lidar/wanji_lidar_device.cpp

@@ -0,0 +1,418 @@
+#include "wanji_lidar_device.h"
+
+
+
+// 万集雷达封装类构造函数
+Wanji_lidar_device::Wanji_lidar_device()
+{
+	m_wanji_lidar_device_status = E_UNKNOWN;
+	mp_wanji_lidar_scan_task = NULL;
+}
+
+// 万集雷达封装类析构函数
+Wanji_lidar_device::~Wanji_lidar_device()
+{
+	uninit();
+}
+
+// 初始化
+Error_manager Wanji_lidar_device::init(wj::wjLidarParams params)
+{
+	Error_manager t_error;
+	LOG(INFO) << " Wanji_lidar_device::init "<< this;
+
+	//唤醒队列
+	m_communication_data_queue.wake_queue();
+
+	//控制参数
+	Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;	//极坐标的限定范围
+	Point2D_tool::Point2D_box			t_point2D_box;				//平面坐标的限定范围
+	Point2D_tool::Point2D_transform 	t_point2D_transform;		//平面坐标的转换矩阵
+
+	t_polar_coordinates_box.angle_min = params.angle_min();
+	t_polar_coordinates_box.angle_max = params.angle_max();
+	t_polar_coordinates_box.distance_min = params.range_min();
+	t_polar_coordinates_box.distance_max = params.range_max();
+
+	t_point2D_box.x_min = params.scan_limit().minx();
+	t_point2D_box.x_max = params.scan_limit().maxx();
+	t_point2D_box.y_min = params.scan_limit().miny();
+	t_point2D_box.y_max = params.scan_limit().maxy();
+
+	t_point2D_transform.m00 = params.transform().m00();
+	t_point2D_transform.m01 = params.transform().m01();
+	t_point2D_transform.m02 = params.transform().m02();
+	t_point2D_transform.m10 = params.transform().m10();
+	t_point2D_transform.m11 = params.transform().m11();
+	t_point2D_transform.m12 = params.transform().m12();
+
+	//初始化通信协议
+	t_error = m_protocol.init(&m_communication_data_queue, t_polar_coordinates_box, t_point2D_box, t_point2D_transform);
+	if (t_error != SUCCESS)
+	{
+		return t_error;
+	}
+
+	//初始化雷达通信模块
+	std::string	t_ip = params.net_config().ip_address();
+	int t_port = params.net_config().port();
+
+	t_error = m_async_client.init("192.168.0.208", 2110, &m_communication_data_queue);
+	if ( t_error != SUCCESS )
+	{
+		return t_error;
+	}
+
+
+	//启动 内部线程。默认wait。
+	m_execute_condition.reset(false, false, false);
+	mp_execute_thread = new std::thread(&Wanji_lidar_device::execute_thread_fun, this);
+
+	m_wanji_lidar_device_status = E_READY;
+
+	return t_error;
+}
+
+//反初始化
+Error_manager Wanji_lidar_device::uninit()
+{
+//终止队列，防止 wait_and_pop 阻塞线程。
+	m_communication_data_queue.termination_queue();
+
+	//关闭线程
+	if (mp_execute_thread)
+	{
+		m_execute_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_execute_thread)
+	{
+		mp_execute_thread->join();
+		delete mp_execute_thread;
+		mp_execute_thread = NULL;
+	}
+
+	m_async_client.uninit();
+	m_protocol.uninit();
+
+//清空队列
+	m_communication_data_queue.clear_and_delete();
+
+	if ( m_wanji_lidar_device_status != E_FAULT  )
+	{
+		m_wanji_lidar_device_status = E_UNKNOWN;
+	}
+	return Error_code::SUCCESS;
+}
+
+
+//对外的接口函数，负责接受并处理任务单，
+Error_manager Wanji_lidar_device::execute_task(Task_Base* p_wanji_lidar_scan_task)
+{
+	LOG(INFO) << " ---Laser_manager::execute_task run---  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//检查指针
+	if (p_wanji_lidar_scan_task == NULL) {
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Wanji_lidar_device::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_wanji_lidar_scan_task->get_task_type() != WANJI_LIDAR_SCAN)
+	{
+		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 " task type error ！= WANJI_LIDAR_SCAN ");
+	}
+
+	//检查接收方的状态
+	t_error = check_status();
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+		//接受任务，并将任务的状态改为TASK_SIGNED已签收
+		mp_wanji_lidar_scan_task = (Wanji_lidar_scan_task *) p_wanji_lidar_scan_task;
+		mp_wanji_lidar_scan_task->set_task_statu(TASK_SIGNED);
+
+		//检查消息内容是否正确，
+		//检查三维点云指针
+		if (mp_wanji_lidar_scan_task->get_task_point_cloud().get() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_point_cloud is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else if ( mp_wanji_lidar_scan_task->get_task_cloud_lock() == NULL )
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_cloud_lock is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else
+		{
+			m_wanji_lidar_device_status = E_BUSY;
+			//启动雷达管理模块，的核心工作线程
+			m_execute_condition.notify_all(true);
+			//通知 thread_work 子线程启动。
+
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_wanji_lidar_scan_task->set_task_statu(TASK_WORKING);
+		}
+
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_wanji_lidar_scan_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_result);
+		}
+	}
+
+	return t_result;
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Wanji_lidar_device::check_status()
+{
+	updata_status();
+	if ( m_wanji_lidar_device_status == E_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if(m_wanji_lidar_device_status == E_BUSY)
+	{
+	    return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
+	    					"  Wanji_lidar_device::check_status() error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Wanji_lidar_device::check_status() error ");
+	}
+	return Error_code::SUCCESS;
+}
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+Error_manager Wanji_lidar_device::end_task()
+{
+	m_execute_condition.notify_all(false);
+
+	if ( m_wanji_lidar_device_status == E_BUSY)
+	{
+		m_wanji_lidar_device_status = E_READY;
+	}
+	//else 状态不变
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_wanji_lidar_scan_task !=NULL)
+	{
+		//判断任务单的错误等级，
+		if ( mp_wanji_lidar_scan_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			//强制改为TASK_OVER，不管它当前在做什么。
+			mp_wanji_lidar_scan_task->set_task_statu(TASK_OVER);
+		}
+		else
+		{
+			//强制改为 TASK_ERROR，不管它当前在做什么。
+			mp_wanji_lidar_scan_task->set_task_statu(TASK_ERROR);
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+//取消任务单，由发送方提前取消任务单
+Error_manager Wanji_lidar_device::cancel_task(Task_Base* p_wanji_lidar_scan_task)
+{
+	if ( p_wanji_lidar_scan_task == mp_wanji_lidar_scan_task )
+	{
+		m_execute_condition.notify_all(false);
+
+		if ( m_wanji_lidar_device_status == E_BUSY)
+		{
+			m_wanji_lidar_device_status = E_READY;
+		}
+		//else 状态不变
+
+		//强制改为 TASK_DEAD，不管它当前在做什么。
+		mp_wanji_lidar_scan_task->set_task_statu(TASK_DEAD);
+
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+	    return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+	    					" Wanji_lidar_device::cancel_task PARAMRTER ERROR ");
+	}
+
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Wanji_lidar_device::is_ready()
+{
+	updata_status();
+	return m_wanji_lidar_device_status == E_READY;
+}
+
+
+
+// 外部调用获取点云
+//input: p_mutex : 输入点云锁的指针
+//output: cloud : 输出点云智能指针, 本函数会将点云添加到cloud里面
+//input: command_time: 触发执行的时间点. 从command_time提前一个扫描周期, 然后取最新的点
+//input: timeout_ms:超时时间, 超时之后自动退出, 返回错误
+Error_manager Wanji_lidar_device::get_latest_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+												   std::chrono::system_clock::time_point command_time, int timeout_ms)
+{
+	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
+	//判断是否超时
+	while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
+	{
+		//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
+		//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
+		if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
+		{
+			std::unique_lock<std::mutex> lck(*p_mutex);
+			Error_manager code = m_protocol.get_scan_points(p_cloud);
+			return code;
+		}
+		//else 等待下一次数据更新
+
+		//等1ms
+		std::this_thread::sleep_for(std::chrono::milliseconds(1));
+	}
+	//超时退出就报错
+	return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+						 " Wanji_lidar_device::get_cloud error ");
+}
+
+//外部获取当前的点云, 就是往前一个周期内的, 如果没有就会报错, 不会等待
+Error_manager Wanji_lidar_device::get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													std::chrono::system_clock::time_point command_time)
+{
+	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
+	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
+	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
+	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
+	{
+		std::unique_lock<std::mutex> lck(*p_mutex);
+		Error_manager code = m_protocol.get_scan_points(p_cloud);
+		return code;
+	}
+	else
+	{
+		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+							 " Wanji_lidar_device::get_current_cloud error ");
+	}
+}
+
+
+
+Wanji_lidar_device::Wanji_lidar_device_status Wanji_lidar_device::get_status()
+{
+	updata_status();
+	return m_wanji_lidar_device_status;
+}
+
+void Wanji_lidar_device::updata_status()
+{
+	Async_Client::Communication_status communication_status = m_async_client.get_status();
+	Wj_716_lidar_protocol::Wanji_lidar_protocol_status wanji_lidar_protocol_status = m_protocol.get_status();
+
+	if ( communication_status == Async_Client::Communication_status::E_FAULT ||
+		 wanji_lidar_protocol_status == Wj_716_lidar_protocol::Wanji_lidar_protocol_status::E_FAULT)
+	{
+		m_wanji_lidar_device_status = E_FAULT;
+	}
+	if ( communication_status == Async_Client::Communication_status::E_DISCONNECT )
+	{
+		m_wanji_lidar_device_status = E_DISCONNECT;
+	}
+	else if ( communication_status == Async_Client::Communication_status::E_READY &&
+			  wanji_lidar_protocol_status == Wj_716_lidar_protocol::Wanji_lidar_protocol_status::E_READY )
+	{
+		if ( m_execute_condition.get_pass_ever() )
+		{
+			m_wanji_lidar_device_status  = E_BUSY;
+		}
+		else
+		{
+			m_wanji_lidar_device_status = E_READY;
+		}
+	}
+	else
+	{
+		m_wanji_lidar_device_status = E_UNKNOWN;
+	}
+}
+
+
+//mp_laser_manager_thread 线程执行函数，
+//thread_work 内部线程负责获取点云结果
+void Wanji_lidar_device::execute_thread_fun()
+{
+	LOG(INFO) << " Wanji_lidar_device::execute_thread_fun() start "<< this;
+	Error_manager t_error;
+
+	//雷达管理的独立线程，负责控制管理所有的雷达。
+	while (m_execute_condition.is_alive())
+	{
+		m_execute_condition.wait();
+		if ( m_execute_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			if ( mp_wanji_lidar_scan_task == NULL )
+			{
+				//没有任务就忽略, 直接关闭线程
+				m_wanji_lidar_device_status = E_READY;
+				m_execute_condition.notify_all(false);
+			}
+			else
+			{
+				if ( mp_wanji_lidar_scan_task->is_over_time() )
+				{
+					t_error.error_manager_reset(TASK_TIMEOVER, MINOR_ERROR, "Wanji_lidar_device::get_cloud_thread() TASK_TIMEOVER");
+					mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
+					//故障结束任务
+					end_task();
+				}
+				else
+				{
+					//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
+					//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
+					if( m_protocol.get_scan_time() >
+						mp_wanji_lidar_scan_task->get_command_time() - std::chrono::milliseconds(m_protocol.get_scan_cycle()) )
+					{
+						std::mutex *tp_mutex = mp_wanji_lidar_scan_task->get_task_cloud_lock();
+						pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = mp_wanji_lidar_scan_task->get_task_point_cloud();
+						{
+							std::unique_lock<std::mutex> lck(*tp_mutex);
+							t_error = m_protocol.get_scan_points(tp_cloud);
+						}
+						mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
+						//正常结束任务
+						end_task();
+					}
+					else
+					{
+						//等1ms
+						std::this_thread::sleep_for(std::chrono::milliseconds(1));
+					}
+				}
+			}
+		}
+	}
+	LOG(INFO) << " Wanji_lidar_device::execute_thread_fun() end :"<<this;
+	return;
+}
+

wanji_lidar/wanji_lidar_device.h

@@ -0,0 +1,89 @@
+#ifndef WJ_LIDAR_ENCAPSULATION_HH
+#define WJ_LIDAR_ENCAPSULATION_HH
+
+#include "async_client.h"
+#include "wj_716_lidar_protocol.h"
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_queue.h"
+#include "wanji_lidar_scan_task.h"
+
+class Wanji_lidar_device
+{
+public:
+	enum Wanji_lidar_device_status
+	{//default  = 0
+		E_UNKNOWN              	= 0,    //未知
+		E_READY	             	= 1,    //正常待机
+		E_DISCONNECT			= 2,	//断连
+
+		E_BUSY					= 3, 	//工作正忙
+
+		E_FAULT					=10,	//故障
+	};
+public:
+    // 无参构造函数
+    Wanji_lidar_device();
+    // 析构函数
+    ~Wanji_lidar_device();
+
+	// 初始化
+	Error_manager init(wj::wjLidarParams params);
+	//反初始化
+	Error_manager uninit();
+
+	//对外的接口函数，负责接受并处理任务单，
+	Error_manager execute_task(Task_Base* p_wanji_lidar_scan_task);
+
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+	Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	Error_manager cancel_task(Task_Base* p_wanji_lidar_scan_task);
+
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+
+	// 外部调用获取点云, 获取最新的, 如果没有就会等待点云刷新
+	//input: p_mutex : 输入点云锁的指针
+	//output: cloud : 输出点云智能指针, 本函数会将点云添加到cloud里面
+	//input: command_time: 触发执行的时间点. 从command_time提前一个扫描周期, 然后取最新的点
+	//input: timeout_ms:超时时间, 超时之后自动退出, 返回错误
+	Error_manager get_latest_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+							std::chrono::system_clock::time_point command_time, int timeout_ms=1500);
+
+	//外部获取当前的点云, 就是往前一个周期内的, 如果没有就会报错, 不会等待
+	Error_manager get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+							std::chrono::system_clock::time_point command_time);
+public:
+	Wanji_lidar_device_status get_status();
+protected:
+	void updata_status();
+	//mp_laser_manager_thread 线程执行函数，
+	//thread_work 内部线程负责获取点云结果
+	void execute_thread_fun();
+
+
+
+protected:
+	//状态
+	Wanji_lidar_device_status 					m_wanji_lidar_device_status;
+
+	//子模块
+	Async_Client								m_async_client;		//异步客户端, 负责雷达通信, 接受雷达数据
+	Wj_716_lidar_protocol						m_protocol;        // 万集雷达协议
+
+	//中间缓存
+	Thread_safe_queue<Binary_buf*>				m_communication_data_queue;	//通信数据队列
+
+
+	//任务执行线程
+	std::thread*        						mp_execute_thread;   			//执行的线程指针，内存由本类管理
+	Thread_condition							m_execute_condition;			//执行的条件变量
+
+	Wanji_lidar_scan_task *  					mp_wanji_lidar_scan_task;   //雷达设备任务单的指针，内存由发送方管理。
+
+};
+
+#endif // !WJ_LIDAR_ENCAPSULATION_HH

wanji_lidar/wanji_lidar_scan_task.cpp

@@ -0,0 +1,114 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#include "wanji_lidar_scan_task.h"
+
+//构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
+Wanji_lidar_scan_task::Wanji_lidar_scan_task()
+{
+	//构造函数锁定任务类型为 WANJI_LIDAR_SCAN，后续不允许更改
+	m_task_type = WANJI_LIDAR_SCAN;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	mp_task_point_cloud = NULL;
+	mp_task_cloud_lock = NULL;
+}
+//析构函数
+Wanji_lidar_scan_task::~Wanji_lidar_scan_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Wanji_lidar_scan_task::task_init(std::chrono::system_clock::time_point command_time,
+											   std::mutex* p_task_cloud_lock,
+											   pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Wanji_lidar_scan_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_command_time = command_time;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Wanji_lidar_scan_task::task_init(Task_statu task_statu,
+											   std::string task_statu_information,
+											   void* p_tast_receiver,
+											   std::chrono::milliseconds task_over_time,
+											   std::chrono::system_clock::time_point command_time,
+											   std::mutex* p_task_cloud_lock,
+											   pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Wanji_lidar_scan_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	m_command_time = command_time;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+std::chrono::system_clock::time_point Wanji_lidar_scan_task::get_command_time()
+{
+	return m_command_time;
+}
+
+
+//获取 三维点云容器的智能指针
+std::mutex*  Wanji_lidar_scan_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针
+pcl::PointCloud<pcl::PointXYZ>::Ptr Wanji_lidar_scan_task::get_task_point_cloud()
+{
+	return mp_task_point_cloud;
+}

wanji_lidar/wanji_lidar_scan_task.h

@@ -0,0 +1,77 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#ifndef NNXX_TESTS_WANJI_LIDAR_TASK_H
+#define NNXX_TESTS_WANJI_LIDAR_TASK_H
+#include "Point2D.h"
+#include "Point3D.h"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include "../error_code/error_code.h"
+
+#include <vector>
+#include <mutex>
+
+#include "../task/task_command_manager.h"
+
+
+
+//万集雷达模块的任务指令，从Task_Base继承，
+class Wanji_lidar_scan_task:public Task_Base
+{
+public:
+	Wanji_lidar_scan_task();
+	Wanji_lidar_scan_task(const Wanji_lidar_scan_task& other)= default;
+	Wanji_lidar_scan_task& operator =(const Wanji_lidar_scan_task& other)= default;
+	~Wanji_lidar_scan_task();
+public://API functions
+
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// output:p_task_point_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(std::chrono::system_clock::time_point command_time,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							std::chrono::system_clock::time_point command_time,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+	);
+public://get or set member variable
+
+	std::chrono::system_clock::time_point get_command_time();
+	//获取 三维点云容器的智能指针;
+	std::mutex*  get_task_cloud_lock();
+	//获取 三维点云容器的智能指针
+	pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+
+protected://member variable
+	//下发指令的时间点. 从command_time提前一个扫描周期, 然后取最新的点
+	std::chrono::system_clock::time_point 		m_command_time;
+
+	//三维点云的数据保护锁，任务输入, (实际上多个雷达会同时往一个pcl点云容器里面填充数据,防止数据冲突,需要对容器加锁.)
+	std::mutex*                     			mp_task_cloud_lock;
+	//采集结果，三维点云容器的智能指针，任务输出
+	//这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。
+	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_WANJI_LIDAR_TASK_H

wanji_lidar/wanji_manager.cpp

@@ -0,0 +1,585 @@
+//
+// Created by huli on 2020/9/9.
+//
+
+#include "wanji_manager.h"
+#include "../tool/proto_tool.h"
+
+Wanji_manager::Wanji_manager()
+{
+	m_wanji_manager_status = E_UNKNOW;
+
+	mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+	m_cloud_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
+	mp_collect_cloud_thread = NULL;
+
+	mp_execute_thread = NULL;
+	mp_wanji_manager_task = NULL;
+	mp_task_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+}
+Wanji_manager::~Wanji_manager()
+{
+	wanji_manager_uninit();
+}
+
+
+//初始化 雷达 管理模块。如下三选一
+Error_manager Wanji_manager::wanji_manager_init()
+{
+	return  wanji_manager_init_from_protobuf(WANJI_MANAGER_PARAMETER_PATH);
+}
+//初始化 雷达 管理模块。从文件读取
+Error_manager Wanji_manager::wanji_manager_init_from_protobuf(std::string prototxt_path)
+{
+	wj::wjManagerParams t_wanji_parameters;
+
+	if(!  proto_tool::read_proto_param(prototxt_path,t_wanji_parameters) )
+	{
+		return Error_manager(WJ_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Wanji_manager read_proto_param  failed");
+	}
+
+	return wanji_manager_init_from_protobuf(t_wanji_parameters);
+}
+//初始化 雷达 管理模块。从protobuf读取
+Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters)
+{
+	LOG(INFO) << " ---wanji_manager_init_from_protobuf run--- "<< this;
+	Error_manager t_error;
+	if ( m_wanji_manager_status != E_UNKNOW)
+	{
+		return Error_manager(Error_code::WJ_MANAGER_INIT_REPEAT, Error_level::MINOR_ERROR,
+							 " Wanji_manager::wanji_manager_init_from_protobuf init repeat error ");
+	}
+
+	//创建雷达设备
+	int t_wanji_lidar_size = wanji_parameters.wj_lidar_size();
+	for(int i=0;i<t_wanji_lidar_size;++i)
+	{
+		Wanji_lidar_device* p_wanji_lidar_device(new Wanji_lidar_device);
+		t_error = p_wanji_lidar_device->init(wanji_parameters.wj_lidar(i));
+		if ( t_error != Error_code::SUCCESS )
+		{
+			delete(p_wanji_lidar_device);
+			return t_error;
+		}
+		m_wanji_lidar_device_map[i] = (p_wanji_lidar_device);
+	}
+
+	//创建预测算法
+	int t_region_workers_size = wanji_parameters.regions_size();
+	for(int i=0;i<t_region_workers_size;++i)
+	{
+		Region_worker* p_region_worker(new Region_worker);
+		Point2D_tool::Point2D_box t_point2d_box;
+		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
+		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
+		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
+		t_point2d_box.y_min = wanji_parameters.regions(i).maxy();
+		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			delete(p_region_worker);
+			return t_error;
+		}
+		m_region_worker_map[i] = p_region_worker;
+	}
+
+
+
+	//启动 收集点云的线程。默认 。
+	m_collect_cloud_condition.reset(false, false, false);
+	mp_collect_cloud_thread = new std::thread(&Wanji_manager::collect_cloud_thread_fun, this);
+
+	//启动 执行的线程。默认 。
+	m_execute_condition.reset(false, false, false);
+	mp_execute_thread = new std::thread(&Wanji_manager::execute_thread_fun, this);
+
+	m_wanji_manager_status = E_READY;
+
+	return Error_code::SUCCESS;
+}
+//反初始化 雷达 管理模块。
+Error_manager Wanji_manager::wanji_manager_uninit()
+{
+	LOG(INFO) << " ---wanji_manager_uninit run--- "<< this;
+	//关闭线程
+	if (mp_collect_cloud_thread)
+	{
+		m_collect_cloud_condition.kill_all();
+	}
+	if (mp_execute_thread)
+	{
+		m_execute_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_collect_cloud_thread)
+	{
+		mp_collect_cloud_thread->join();
+		delete mp_collect_cloud_thread;
+		mp_collect_cloud_thread = NULL;
+	}
+	if (mp_execute_thread)
+	{
+		mp_execute_thread->join();
+		delete mp_execute_thread;
+		mp_execute_thread = NULL;
+	}
+	//回收雷达设备
+	for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
+	{
+		delete(iter->second);
+	}
+	m_wanji_lidar_device_map.clear();
+
+	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
+	{
+		delete(iter->second);
+	}
+	m_region_worker_map.clear();
+
+	m_wanji_manager_status = E_UNKNOW;
+
+	//回收下发雷达任务单
+	wanji_lidar_scan_task_map_clear_and_delete();
+
+	return Error_code::SUCCESS;
+}
+
+
+//对外的接口函数，负责接受并处理任务单，
+Error_manager Wanji_manager::execute_task(Task_Base* p_wanji_manager_task)
+{
+	LOG(INFO) << " ---Wanji_manager::execute_task run---  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+//检查指针
+	if (p_wanji_manager_task == NULL) {
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Wanji_manager::execute_task failed, POINTER_IS_NULL");
+	}
+//检查任务类型，
+	if (p_wanji_manager_task->get_task_type() != WANJI_MANAGER_TASK)
+	{
+		return Error_manager(Error_code::WJ_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error ！= WANJI_MANAGER_TASK ");
+	}
+
+//检查接收方的状态
+	t_error = check_status();
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+//接受任务，并将任务的状态改为TASK_SIGNED已签收
+		mp_wanji_manager_task = (Wanji_manager_task *) p_wanji_manager_task;
+		mp_wanji_manager_task->set_task_statu(TASK_SIGNED);
+
+//启动雷达管理模块，的核心工作线程
+		m_wanji_manager_status = E_ISSUED_SCAN;
+		m_execute_condition.notify_all(true);
+
+//将任务的状态改为 TASK_WORKING 处理中
+		mp_wanji_manager_task->set_task_statu(TASK_WORKING);
+
+
+//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+//将任务的状态改为 TASK_ERROR 结束错误
+				mp_wanji_manager_task->set_task_statu(TASK_ERROR);
+			}
+//返回错误码
+			mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
+		}
+	}
+
+	return t_result;
+}
+
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+Error_manager Wanji_manager::end_task()
+{
+	m_execute_condition.notify_all(false);
+
+	//释放下发的任务单
+	wanji_lidar_scan_task_map_clear_and_delete();
+	{
+		std::unique_lock<std::mutex> t_lock(m_task_cloud_mutex);
+		mp_task_cloud->clear();
+	}
+
+	if ( m_wanji_manager_status == E_BUSY ||
+	m_wanji_manager_status == E_ISSUED_SCAN ||
+	m_wanji_manager_status == E_WAIT_SCAN ||
+	m_wanji_manager_status == E_ISSUED_DETECT ||
+	m_wanji_manager_status == E_WAIT_DETECT )
+	{
+		m_wanji_manager_status = E_READY;
+	}
+	//else 状态不变
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_wanji_manager_task !=NULL)
+	{
+		//判断任务单的错误等级，
+		if ( mp_wanji_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			//强制改为TASK_OVER，不管它当前在做什么。
+			mp_wanji_manager_task->set_task_statu(TASK_OVER);
+		}
+		else
+		{
+			//强制改为 TASK_ERROR，不管它当前在做什么。
+			mp_wanji_manager_task->set_task_statu(TASK_ERROR);
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+//取消任务单，由发送方提前取消任务单
+Error_manager Wanji_manager::cancel_task(Task_Base* p_wanji_manager_task)
+{
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Wanji_manager::check_status()
+{
+	if ( m_wanji_manager_status == E_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if ( m_wanji_manager_status == E_BUSY ||
+			  m_wanji_manager_status == E_ISSUED_SCAN ||
+			  m_wanji_manager_status == E_WAIT_SCAN ||
+			  m_wanji_manager_status == E_ISSUED_DETECT ||
+			  m_wanji_manager_status == E_WAIT_DETECT )
+	{
+		return Error_manager(Error_code::WJ_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
+							 " Laser_manager::check_status error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::WJ_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Laser_manager::check_status error ");
+	}
+	return Error_code::SUCCESS;
+}
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Wanji_manager::is_ready()
+{
+	return  m_wanji_manager_status == E_READY;
+}
+
+Wanji_manager::Wanji_manager_status Wanji_manager::get_status()
+{
+	return m_wanji_manager_status;
+}
+std::map<int, Wanji_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
+{
+	return m_wanji_lidar_device_map;
+}
+std::map<int, Region_worker *> & Wanji_manager::get_region_worker_map()
+{
+	return m_region_worker_map;
+}
+
+
+
+
+
+
+//自动收集点云的线程函数
+void Wanji_manager::collect_cloud_thread_fun()
+{
+	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	while (m_collect_cloud_condition.is_alive())
+	{
+		//暂时固定为一个扫描周期, 就循环一次
+		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
+		m_collect_cloud_condition.wait_for_millisecond(WANJI_716_SCAN_CYCLE_MS);
+		if ( m_collect_cloud_condition.is_alive() )
+		{
+			std::chrono::system_clock::time_point t_command_time = std::chrono::system_clock::now();
+			{
+				std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
+				mp_cloud_collection->clear();
+			}
+
+			//重新收集当前的点云, 不允许阻塞
+			for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
+			{
+				t_error = iter->second->get_current_cloud(&m_cloud_collection_mutex,
+												mp_cloud_collection,t_command_time);
+				if ( t_error != Error_code::SUCCESS )
+				{
+					t_result.compare_and_cover_error(t_error);
+				}
+			}
+
+			if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )
+			{
+				{
+					std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
+					m_cloud_updata_time = std::chrono::system_clock::now();
+				}
+				wake_auto_detect();
+			}
+
+
+//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;
+
+			//huli
+		}
+	}
+	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
+	return;
+}
+
+//唤醒自动预测的算法线程
+Error_manager Wanji_manager::wake_auto_detect()
+{
+	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
+	{
+	    iter->second->wake_auto_detect();
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//执行外界任务的执行函数
+void Wanji_manager::execute_thread_fun()
+{
+	LOG(INFO) << " Wanji_manager::execute_thread_fun() start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//雷达管理的独立线程，负责控制管理所有的雷达。
+	while (m_execute_condition.is_alive())
+	{
+		m_execute_condition.wait();
+		if ( m_execute_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//重新循环必须清除错误码.
+			t_error.error_manager_clear_all();
+			t_result.error_manager_clear_all();
+
+			if ( mp_wanji_manager_task == NULL )
+			{
+				//忽略任务, 直接停止线程
+				m_wanji_manager_status = E_READY;
+				m_execute_condition.notify_all(false);
+			}
+
+			switch ( m_wanji_manager_status )
+			{
+			    case E_ISSUED_SCAN:
+			    {
+					//第一步
+					//下发任务, 雷达管理模块给子雷达下发扫描任务.
+					//分发扫描任务给下面的雷达
+					//下发任务给所有的雷达
+					for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
+					{
+						//下发扫描任务
+						t_error = issued_scan_task(iter->first);
+						if ( t_error != SUCCESS)
+						{
+							//下发子任务的故障处理汇总
+							t_result.compare_and_cover_error(t_error);
+						}
+					}
+
+					//故障处理, 只处理2级以上的故障, 一级故障继续执行后面的
+					if(t_result.get_error_level() >= MINOR_ERROR)
+					{
+						m_wanji_manager_status = E_READY;
+						mp_wanji_manager_task->set_task_statu(TASK_ERROR);
+						//因为故障,而提前结束任务.
+						mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
+						end_task();
+					}
+					else
+					{
+						//下发任务之后，修改状态为等待答复。
+						m_wanji_manager_status = E_WAIT_SCAN;
+					}
+			        break;
+			    }
+			    case E_WAIT_SCAN:
+			    {
+					//第二步
+					//等待答复, 注意不能写成死循环阻塞,
+					//子任务没完成就直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive()),
+					// 这样线程仍然被 m_laser_manager_condition 控制
+					//循环检查任务的完成状态
+					std::map<int, Wanji_lidar_scan_task*>::iterator map_iter;
+					for (  map_iter = m_wanji_lidar_scan_task_map.begin(); map_iter != m_wanji_lidar_scan_task_map.end(); ++map_iter)
+					{
+						if ( map_iter->second->is_task_end() == false)
+						{
+							if ( map_iter->second->is_over_time() )
+							{
+								//超时处理。取消任务。
+								m_wanji_lidar_device_map[map_iter->first]->cancel_task(map_iter->second);
+								map_iter->second->set_task_statu(TASK_DEAD);
+								t_error.error_manager_reset(Error_code::WANJI_LIDAR_DEVICE_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+															" Wanji_lidar_scan_task is_over_time ");
+								map_iter->second->set_task_error_manager(t_error);
+
+								continue;
+								//本次子任务超时死亡.直接进行下一个
+							}
+							else
+							{
+								//如果任务还在执行, 那就等待任务继续完成，等1us
+								std::this_thread::sleep_for(std::chrono::microseconds(1));
+								std::this_thread::yield();
+								break;
+								//注意了:这里break之后,map_iter不会指向 m_laser_task_map.end()
+								//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
+							}
+						}
+						//else 任务完成就判断下一个
+					}
+
+					//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
+					if(map_iter == m_wanji_lidar_scan_task_map.end() )
+					{
+						for (auto iter = m_wanji_lidar_scan_task_map.begin(); iter != m_wanji_lidar_scan_task_map.end(); )
+						{
+							//数据汇总，
+							//由于 cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。
+
+							//错误汇总
+							t_result.compare_and_cover_error(iter->second->get_task_error_manager());
+
+							//销毁下发的任务单。
+							delete(iter->second);
+							iter = m_wanji_lidar_scan_task_map.erase(iter);
+							//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+						}
+
+						//故障处理, 只处理2级以上的故障, 一级故障继续执行后面的
+						if(t_result.get_error_level() >= MINOR_ERROR)
+						{
+							m_wanji_manager_status = E_READY;
+							mp_wanji_manager_task->set_task_statu(TASK_ERROR);
+							//因为故障,而提前结束任务.
+							mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
+							end_task();
+						}
+						else
+						{
+							//下发任务之后，修改状态为等待答复。
+							m_wanji_manager_status = E_ISSUED_DETECT;
+						}
+					}
+					//else 直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive())
+			        break;
+			    }
+				case E_ISSUED_DETECT:
+				{
+					m_wanji_manager_status = E_WAIT_DETECT;
+					break;
+				}
+				case E_WAIT_DETECT:
+				{
+					{
+						std::unique_lock<std::mutex> t_lock(m_task_cloud_mutex);
+						std::cout << " huli test :::: " << " mp_task_cloud->size() = " << mp_task_cloud->size() << std::endl;
+					}
+
+					mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
+					//正常结束任务
+					end_task();
+					break;
+				}
+			    default:
+			    {
+
+			        break;
+			    }
+			}
+
+
+
+			//第3步, 下发预测算法任务
+			//huli
+		}
+	}
+	LOG(INFO) << " Wanji_manager::execute_thread_fun() end "<< this;
+	return;
+}
+
+//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+Error_manager Wanji_manager::issued_scan_task(int lidar_index)
+{
+	//判断是否存在
+	std::map<int, Wanji_lidar_device*>::iterator map_iter1 = m_wanji_lidar_device_map.find(lidar_index);
+	if ( map_iter1 == m_wanji_lidar_device_map.end() )
+	{
+		return Error_manager(Error_code::WJ_MANAGER_LASER_INDEX_ERRPR, Error_level::NEGLIGIBLE_ERROR,
+							 " issued_scan_task lidar_index  error ");
+	}
+
+	//查重
+	std::map<int, Wanji_lidar_scan_task*>::iterator map_iter2 = m_wanji_lidar_scan_task_map.find(lidar_index);
+	if ( map_iter2 != m_wanji_lidar_scan_task_map.end() )
+	{
+		//如果重复，则不做任何处理,视为无效行为.
+		//返回轻微错误.仅仅提示作用.
+		return Error_manager(Error_code::WJ_MANAGER_LASER_INDEX_REPEAT, Error_level::NEGLIGIBLE_ERROR,
+							 " issued_scan_task  lidar_index repeart ");
+	}
+
+	//创建雷达扫描任务,
+	// 这里为下发雷达任务分配内存，后续要记得回收。
+	Wanji_lidar_scan_task* t_wanji_lidar_scan_task=new Wanji_lidar_scan_task();
+	t_wanji_lidar_scan_task->task_init(TASK_CREATED, "",
+									   m_wanji_lidar_device_map[lidar_index],
+									   std::chrono::milliseconds(WANJI_716_SCAN_CYCLE_MS),
+									   mp_wanji_manager_task->get_command_time(),
+									   &m_task_cloud_mutex,
+									   mp_task_cloud	);
+
+
+	//保存雷达扫描任务 到 m_laser_task_map
+	m_wanji_lidar_scan_task_map[lidar_index] = t_wanji_lidar_scan_task;
+
+	//发送任务单给雷达
+	return m_wanji_lidar_device_map[lidar_index]->execute_task(t_wanji_lidar_scan_task);
+
+}
+
+
+//释放下发的任务单
+Error_manager Wanji_manager::wanji_lidar_scan_task_map_clear_and_delete()
+{
+	for ( auto map_iter2 = m_wanji_lidar_scan_task_map.begin(); map_iter2 != m_wanji_lidar_scan_task_map.end(); )
+	{
+		//销毁下发的任务单。
+		delete(map_iter2->second);
+		map_iter2 = m_wanji_lidar_scan_task_map.erase(map_iter2);
+		//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+		//for循环不用 ++map_iter2
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+
+
+
+
+

wanji_lidar/wanji_manager.h

@@ -0,0 +1,128 @@
+//
+// Created by huli on 2020/9/9.
+//
+
+#ifndef NNXX_TESTS_WANJI_MANAGER_H
+#define NNXX_TESTS_WANJI_MANAGER_H
+
+#include "../tool/thread_condition.h"
+#include "../tool/singleton.h"
+#include "../error_code/error_code.h"
+#include <thread>
+#include <map>
+
+#include "../wanji_lidar/wanji_lidar_device.h"
+#include "../wanji_lidar/region_worker.h"
+#include "../wanji_lidar/wanji_manager_task.h"
+#include "../wanji_lidar/wj_lidar_conf.pb.h"
+
+class Wanji_manager:public Singleton<Wanji_manager>
+{
+public:
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Wanji_manager>;
+
+#define WANJI_MANAGER_PARAMETER_PATH	"../setting/wanji_manager.prototxt"
+public:
+	//雷达管理的状态
+	enum Wanji_manager_status
+	{
+		E_UNKNOW               	= 0,    //未知
+		E_READY               	= 1,    //准备，待机
+		E_BUSY					= 2, 	//工作正忙
+
+		E_ISSUED_SCAN			= 3, 	//下发任务, 获取点云
+		E_WAIT_SCAN				= 4,	//等待任务, 扫描点云
+
+		E_ISSUED_DETECT			= 5, 	//下发任务, 算法预测
+		E_WAIT_DETECT			= 6, 	//等待任务, 算法预测
+
+		E_FAULT					= 10,	//故障
+	};
+private:
+	Wanji_manager();
+public:
+
+	Wanji_manager(const Wanji_manager& other)= delete;
+	Wanji_manager& operator =(const Wanji_manager& other)= delete;
+	~Wanji_manager();
+public://API functions
+	//初始化 雷达 管理模块。如下三选一
+	Error_manager wanji_manager_init();
+	//初始化 雷达 管理模块。从文件读取
+	Error_manager wanji_manager_init_from_protobuf(std::string prototxt_path);
+	//初始化 雷达 管理模块。从protobuf读取
+	Error_manager wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters);
+	//反初始化 雷达 管理模块。
+	Error_manager wanji_manager_uninit();
+
+
+	//对外的接口函数，负责接受并处理任务单，
+	Error_manager execute_task(Task_Base* p_wanji_manager_task);
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+	Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	Error_manager cancel_task(Task_Base* p_wanji_manager_task);
+
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+
+public://get or set member variable
+	Wanji_manager_status get_status();
+	std::map<int, Wanji_lidar_device*> & get_wanji_lidar_device_map();
+	std::map<int, Region_worker *> & get_region_worker_map();
+protected://member functions
+	//自动收集点云的线程函数
+	void collect_cloud_thread_fun();
+	//唤醒自动预测的算法线程
+	Error_manager wake_auto_detect();
+
+	//执行外界任务的执行函数
+	void execute_thread_fun();
+
+	//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+	Error_manager issued_scan_task(int lidar_index);
+	//释放下发的任务单
+	Error_manager wanji_lidar_scan_task_map_clear_and_delete();
+
+
+protected://member variable
+
+	//状态
+	Wanji_manager_status						m_wanji_manager_status;			//万集管理模块的状态
+
+	std::map<int, Wanji_lidar_device *> 		m_wanji_lidar_device_map; 		// 万集雷达实例指针数组, 内存由本类管理
+	std::map<int, Region_worker *> 				m_region_worker_map;  			// 区域功能实例指针数组, 内存由本类管理
+
+
+	//万集雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息.
+	std::mutex	 								m_cloud_collection_mutex;       // 点云更新互斥锁
+	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由本类管理
+	std::chrono::system_clock::time_point		m_cloud_updata_time;			//扫描点的更新时间.
+	std::thread*        						mp_collect_cloud_thread;   		//收集点云的线程指针，内存由本类管理
+	Thread_condition							m_collect_cloud_condition;		//收集点云的条件变量
+
+
+
+	//任务执行线程
+	std::thread*        						mp_execute_thread;   			//执行的线程指针，内存由本类管理
+	Thread_condition							m_execute_condition;			//执行的条件变量
+
+	Wanji_manager_task*							mp_wanji_manager_task;			//万集管理模块的任务单的指针，内存由发送方管理。
+
+	std::mutex	 								m_task_cloud_mutex;       		//任务点云更新互斥锁
+	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_task_cloud;					//任务点云集合, 内存由本类管理
+	std::map<int, Wanji_lidar_scan_task*>		m_wanji_lidar_scan_task_map;	//万集管理下发给雷达子模块的任务map，内存由本类管理
+
+//	std::map<int, Wanji_lidar_scan_task*>		m_wanji_lidar_scan_task_map;	//万集管理下发给雷达子模块的任务map，内存由本类管理
+
+
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_WANJI_MANAGER_H

wanji_lidar/wanji_manager_task.cpp

@@ -0,0 +1,65 @@
+#include "wanji_manager_task.h"
+
+
+Wanji_manager_task::Wanji_manager_task()
+{
+    m_task_type = Task_type::WANJI_MANAGER_TASK;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_terminal_id = 0;
+}
+
+Wanji_manager_task::~Wanji_manager_task()
+{
+}
+
+//初始化函数
+Error_manager Wanji_manager_task::task_init(int terminal_id, std::chrono::system_clock::time_point command_time	)
+{
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_terminal_id = terminal_id;
+	m_command_time = command_time;
+
+	return Error_code::SUCCESS;
+}
+Error_manager Wanji_manager_task::task_init(Task_statu task_statu,
+						std::string task_statu_information,
+						void* p_tast_receiver,
+						std::chrono::milliseconds task_over_time,
+						int terminal_id,
+						std::chrono::system_clock::time_point command_time
+)
+{
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	m_terminal_id = terminal_id;
+	m_command_time = command_time;
+
+	return Error_code::SUCCESS;
+}
+
+int Wanji_manager_task::get_terminal_id()
+{
+	return m_terminal_id;
+}
+std::chrono::system_clock::time_point Wanji_manager_task::get_command_time()
+{
+	return m_command_time;
+}
+Common_data::Car_wheel_information Wanji_manager_task::get_car_wheel_information()
+{
+	return m_car_wheel_information;
+}
+void Wanji_manager_task::set_car_wheel_information(Common_data::Car_wheel_information car_wheel_information)
+{
+	m_car_wheel_information = car_wheel_information;
+}

wanji_lidar/wanji_manager_task.h

@@ -0,0 +1,46 @@
+#ifndef WJ_LIDAR_TASK_HH
+#define WJ_LIDAR_TASK_HH
+#include <string.h>
+#include <mutex>
+#include <atomic>
+#include <chrono>
+
+#include "../task/task_command_manager.h"
+#include "../error_code/error_code.h"
+#include "../tool/common_data.h"
+
+
+//万集测量任务单
+class Wanji_manager_task : public Task_Base
+{
+public:
+    // 构造函数
+	Wanji_manager_task();
+    // 析构
+    ~Wanji_manager_task();
+	//初始化函数
+	Error_manager task_init(int terminal_id, std::chrono::system_clock::time_point command_time	);
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							int terminal_id,
+							std::chrono::system_clock::time_point command_time
+	);
+public://get or set member variable
+	int get_terminal_id();
+	std::chrono::system_clock::time_point get_command_time();
+	Common_data::Car_wheel_information get_car_wheel_information();
+	void set_car_wheel_information(Common_data::Car_wheel_information car_wheel_information);
+
+private:
+	//终端id
+	int 										m_terminal_id;
+	//下发指令的时间点. 从command_time提前一个扫描周期, 然后取最新的点
+	std::chrono::system_clock::time_point 		m_command_time;
+    // 测量结果
+    Common_data::Car_wheel_information			m_car_wheel_information;
+
+};
+
+#endif // !WJ_LIDAR_TASK_HH

wanji_lidar/wj_716_lidar_protocol.cpp

@@ -0,0 +1,524 @@
+#include "wj_716_lidar_protocol.h"
+#include <iostream>
+//#include "../tool/proto_tool.h"
+
+
+
+Wj_716_lidar_protocol::Wj_716_lidar_protocol()
+{
+	m_wanji_lidar_protocol_status = E_UNKNOWN;
+
+	mp_communication_data_queue = NULL;
+	memset(m_scan_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+	memset(m_scan_point_intensity, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+	memset(m_two_echo_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+	m_current_frame_number = 0;
+
+	mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+	mp_two_echo_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+	mp_thread_analysis = NULL;
+
+	// 设置初始时间为昨天，以避免第一次点云更新前获取到空点云
+	m_scan_cloud_time = std::chrono::system_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
+	m_two_echo_cloud_time =
+	std::chrono::system_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
+
+	memset(&m_polar_coordinates_box, 0, sizeof(Point2D_tool::Polar_coordinates_box));
+	memset(&m_point2D_box, 0, sizeof(Point2D_tool::Point2D_box));
+	memset(&m_point2D_transform, 0, sizeof(Point2D_tool::Point2D_transform));
+
+
+	mp_thread_analysis = NULL;
+}
+
+Wj_716_lidar_protocol::~Wj_716_lidar_protocol()
+{
+	uninit();
+}
+
+// 初始化
+Error_manager Wj_716_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+										  Point2D_tool::Polar_coordinates_box polar_coordinates_box,
+										  Point2D_tool::Point2D_box point2D_box,
+										  Point2D_tool::Point2D_transform point2D_transform)
+{
+	if (p_communication_data_queue == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+
+	mp_communication_data_queue = p_communication_data_queue;
+	m_polar_coordinates_box = polar_coordinates_box;
+	m_point2D_box = point2D_box;
+	m_point2D_transform = point2D_transform;
+	//接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
+	m_condition_analysis.reset(false, true, false);
+	mp_thread_analysis = new std::thread(&Wj_716_lidar_protocol::thread_analysis, this);
+
+
+	m_wanji_lidar_protocol_status = E_READY;
+
+	return Error_code::SUCCESS;
+}
+
+//反初始化
+Error_manager Wj_716_lidar_protocol::uninit()
+{
+	LOG(INFO) << " ---Wj_716_lidar_protocol uninit --- "<< this;
+
+	if ( mp_communication_data_queue )
+	{
+		//终止队列，防止 wait_and_pop 阻塞线程。
+		mp_communication_data_queue->termination_queue();
+	}
+
+	//关闭线程
+	if (mp_thread_analysis)
+	{
+		m_condition_analysis.kill_all();
+	}
+	//回收线程的资源
+	if (mp_thread_analysis)
+	{
+		mp_thread_analysis->join();
+		delete mp_thread_analysis;
+		mp_thread_analysis = NULL;
+	}
+
+	if ( mp_communication_data_queue )
+	{
+		//清空队列
+		mp_communication_data_queue->clear_and_delete();
+	}
+	//队列的内存由上级管理, 这里写空就好了.
+	mp_communication_data_queue = NULL;
+
+	if ( m_wanji_lidar_protocol_status != E_FAULT  )
+	{
+		m_wanji_lidar_protocol_status = E_UNKNOWN;
+	}
+	return Error_code::SUCCESS;
+}
+
+
+//检查状态
+Error_manager Wj_716_lidar_protocol::check_status()
+{
+	if ( m_wanji_lidar_protocol_status == E_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::WJ_PROTOCOL_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Wj_716_lidar_protocol::check_status() error ");
+	}
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Wj_716_lidar_protocol::is_ready()
+{
+	if ( m_wanji_lidar_protocol_status == E_READY )
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+//获取状态
+Wj_716_lidar_protocol::Wanji_lidar_protocol_status Wj_716_lidar_protocol::get_status()
+{
+	return m_wanji_lidar_protocol_status;
+}
+
+//获取扫描周期
+int Wj_716_lidar_protocol::get_scan_cycle()
+{
+	return WANJI_FRAME_SCAN_CYCLE_MS;
+}
+
+
+//获取扫描点云
+Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
+{
+	if ( p_cloud_out.get() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL ");
+	}
+	std::unique_lock<std::mutex> lck(m_scan_mutex);
+	//将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
+	(*p_cloud_out)+=(*mp_scan_points);
+
+	return Error_code::SUCCESS;
+}
+
+
+//获取二次回波点云
+Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
+{
+	if ( p_cloud_out.get() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " Wj_716_lidar_protocol::get_two_echo_points POINTER IS NULL ");
+	}
+
+	std::unique_lock<std::mutex> lck(m_two_echo_mutex);
+	//将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
+	(*p_cloud_out)+=(*mp_two_echo_points);
+
+	return Error_code::SUCCESS;
+}
+
+
+/**
+ * 获取更新扫描点云时间
+ * */
+std::chrono::system_clock::time_point Wj_716_lidar_protocol::get_scan_time()
+{
+	std::unique_lock<std::mutex> lck(m_scan_mutex);
+	return m_scan_cloud_time;
+}
+
+/**
+ * 获取更新二次回波时间
+ * */
+std::chrono::system_clock::time_point Wj_716_lidar_protocol::get_two_echo_time()
+{
+	std::unique_lock<std::mutex> lck(m_two_echo_mutex);
+	return m_two_echo_cloud_time;
+}
+
+
+//解析线程函数
+void Wj_716_lidar_protocol::thread_analysis()
+{
+	LOG(INFO) << " ---Wj_716_lidar_protocol::thread_analysis start ---" << this;
+
+	Error_manager t_error;
+	//接受雷达消息，包括连接,重连和接受数据
+	while (m_condition_analysis.is_alive())
+	{
+		m_condition_analysis.wait();
+		if (m_condition_analysis.is_alive())
+		{
+			std::this_thread::yield();
+
+			if (mp_communication_data_queue == NULL)
+			{
+				m_wanji_lidar_protocol_status = E_FAULT;
+			}
+			else
+			{
+				Binary_buf *tp_binary_buf = NULL;
+				bool is_pop = mp_communication_data_queue->wait_and_pop(tp_binary_buf);
+				if (is_pop == true && tp_binary_buf != NULL)
+				{
+					//检查数据头和长度,
+					if (check_head_and_length(tp_binary_buf))
+					{
+						// 消息完整性检查, 检查末尾的校验码,
+						if (check_xor(tp_binary_buf))
+						{
+							//至此, 我们确认消息已经被正常的接受, 后续错误就不可
+
+							// 万集通信协议, 解析数据, 转化为点云
+							t_error = data_protocol_analysis(tp_binary_buf);
+							//暂时不管错误
+						}
+						//else 错误不管, 当做无效消息处理
+					}
+					//else 错误不管, 当做无效消息处理
+					delete(tp_binary_buf);
+				}
+			}
+		}
+	}
+	LOG(INFO) << " -Wj_716_lidar_protocol::thread_analysis end :" << this;
+	return;
+}
+
+//检查数据头和长度, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
+bool Wj_716_lidar_protocol::check_head_and_length(Binary_buf *p_binary_buf)
+{
+	//检查总长度, 716固定为898byte
+	if (p_binary_buf->get_length() != WANJI_FRAME_LENGTH_716)
+	{
+		return false;
+	}
+	char *tp_data = p_binary_buf->get_buf();
+	//检查数据头, 716的前4个字节固定为0x02020202
+	if (tp_data[0] == 0x02 && tp_data[1] == 0x02 && tp_data[2] == 0x02 && tp_data[3] == 0x02)
+	{
+		//4~7字节合并为32位的int, 是数据消息里面的长度值, 不包括数据头(4byte) 长度(4byte) 尾部校验码(1byte)
+		unsigned int l_u32reallen = (tp_data[4] << 24) |
+									(tp_data[5] << 16) |
+									(tp_data[6] << 8) |
+									(tp_data[7] << 0);
+		if (l_u32reallen + 9 == WANJI_FRAME_LENGTH_716)
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		return false;
+	}
+}
+
+// 消息完整性检查, 检查末尾的校验码, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
+bool Wj_716_lidar_protocol::check_xor(Binary_buf *p_binary_buf)
+{
+	int i = 0;
+	char check = 0;
+	//校验码的运算, 就是把中间所有的数据异或, 不包括数据头(4byte) 长度(4byte) 尾部校验码(1byte)
+	char *tp_data = p_binary_buf->get_buf() + 8;
+	int t_length = p_binary_buf->get_length() - 9;
+
+	for (i = 0; i < t_length; i++)
+	{
+		check ^= *tp_data++;
+		//后++, tp_data在最后 还会右移一个
+	}
+	if (check == *tp_data)
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+// 万集通信协议, 解析数据, 转化为点云
+Error_manager Wj_716_lidar_protocol::data_protocol_analysis(Binary_buf *p_binary_buf)
+{
+	Error_manager t_error;
+
+	char *data = p_binary_buf->get_buf();
+	int len = p_binary_buf->get_length();
+
+	//消息指令类型判断, data[8] == 0x73 为数据类型, data[9] == 0x52||0x53 为传输方向(发送或接受)
+	if ((data[8] == 0x73 && data[9] == 0x52) ||
+		(data[8] == 0x73 && data[9] == 0x53)) //command type:0x73(s) 0x52/0X53(R/S)
+	{
+		//716雷达扫描的数据量很大, 一次完整的扫描信息分成了6条消息分开发送,
+		//数据的包号l_n32PackageNo, 实际为1~6, 无限循环, 循环完毕后,6条加起来就是一个完整的雷达扫描数据
+		//奇数包发送前面的405个点的数据, 偶数包发送后面406个点的数据
+		int l_n32PackageNo = (data[50] << 8) + data[51];
+		//数据的帧号l_u32FrameNo, 实际为0~2^32, 相同帧号的6条消息表示这6条是雷达一次扫描的完整消息,
+		unsigned int l_u32FrameNo = (data[46] << 24) + (data[47] << 16) + (data[48] << 8) + data[49];
+
+		//1~2包为扫描点到雷达的距离, 405+406 = 811,
+		if (l_n32PackageNo == 1)
+		{
+			//重置当前的帧号, 清空数据, 防止出错后复用上一次的数据.
+			m_current_frame_number = l_u32FrameNo;
+			memset(m_scan_point_intensity, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+			memset(m_scan_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+			memset(m_two_echo_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
+
+			for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
+			{
+				m_scan_point_distance[i] =
+				(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+				m_scan_point_distance[i] /= 1000.0;//毫米->米
+				//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
+				if (m_scan_point_distance[i] >= 25 || m_scan_point_distance[i] == 0)
+				{
+					m_scan_point_distance[i] = NAN;
+				}
+			}
+		}
+		else if (l_n32PackageNo == 2)
+		{
+			//检查帧号的一致性
+			if (m_current_frame_number == l_u32FrameNo)
+			{
+				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
+				{
+					m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
+					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+					m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] /= 1000.0;//毫米->米
+					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
+					if (m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] >= 25 ||
+						m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] == 0)
+					{
+						m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] = NAN;
+					}
+				}
+				//至此就得到了扫描点到雷达的距离, 如有需要, 可以再此使用
+
+
+				//数据加锁的范围
+				{
+					std::unique_lock<std::mutex> lck(m_scan_mutex);
+					mp_scan_points->clear();
+					// 把点到雷达的距离, 转化为标准坐标下的点云
+					t_error = polar_coordinates_transform_cloud(m_scan_point_distance,
+																WANJI_SCAN_POINT_NUMBER_TOTAL, mp_scan_points);
+					m_scan_cloud_time = std::chrono::system_clock::now();
+				}
+			}
+			else
+			{
+				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+			}
+		}
+			//1~2包为扫描点的光强, 405+406 = 811,
+		else if (l_n32PackageNo == 3)
+		{
+			//检查帧号的一致性
+			if (m_current_frame_number == l_u32FrameNo)
+			{
+				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
+				{
+					m_scan_point_intensity[i] =
+					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+				}
+			}
+			else
+			{
+				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+			}
+		}
+		else if (l_n32PackageNo == 4)
+		{
+			//检查帧号的一致性
+			if (m_current_frame_number == l_u32FrameNo)
+			{
+				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
+				{
+					m_scan_point_intensity[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
+					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+				}
+				//至此就得到了扫描点的光强, 如有需要, 可以再此使用
+
+			}
+			else
+			{
+				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+			}
+		}
+		else if (l_n32PackageNo == 5)
+		{
+			//检查帧号的一致性
+			if (m_current_frame_number == l_u32FrameNo)
+			{
+				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
+				{
+					m_two_echo_point_distance[i] =
+					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+					m_two_echo_point_distance[i] /= 1000.0;//毫米->米
+					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
+					if (m_two_echo_point_distance[i] >= 25 || m_two_echo_point_distance[i] == 0)
+					{
+						m_two_echo_point_distance[i] = NAN;
+					}
+				}
+			}
+			else
+			{
+				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+			}
+		}
+		else if (l_n32PackageNo == 6)
+		{
+			//检查帧号的一致性
+			if (m_current_frame_number == l_u32FrameNo)
+			{
+				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
+				{
+					m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
+					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
+					m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] /= 1000.0;//毫米->米
+					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
+					if (m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] >= 25 ||
+						m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] == 0)
+					{
+						m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] = NAN;
+					}
+				}
+				//至此就得到了二次反射的扫描点到雷达的距离, 如有需要, 可以再此使用
+
+
+				//数据加锁的范围
+				{
+					std::unique_lock<std::mutex> lck(m_two_echo_mutex);
+					mp_two_echo_points->clear();
+					// 把点到雷达的距离, 转化为标准坐标下的点云
+					t_error = polar_coordinates_transform_cloud(m_two_echo_point_distance,
+																WANJI_SCAN_POINT_NUMBER_TOTAL, mp_two_echo_points);
+					m_two_echo_cloud_time = std::chrono::system_clock::now();
+				}
+			}
+			else
+			{
+				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+			}
+		}
+		return Error_manager(Error_code::SUCCESS);
+	}
+	else
+	{
+		return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
+							 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
+	}
+	return Error_code::SUCCESS;
+}
+
+// 把点到雷达的距离, 转化为标准坐标下的点云
+Error_manager Wj_716_lidar_protocol::polar_coordinates_transform_cloud(float *p_point_distance, int number,
+																	   pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
+{
+	if (p_point_distance == NULL || p_cloud_out.get() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	else
+	{
+		pcl::PointXYZ point;
+		for (int i = 0; i < number; ++i)
+		{
+			float angle = WANJI_SCAN_POINT_ANGLE_MIN + i * WANJI_SCAN_POINT_ANGLE_INCREMENT;
+			//判断极坐标点是否在限制范围
+			if (Point2D_tool::limit_with_polar_coordinates_box(p_point_distance[i], angle,
+															   &m_polar_coordinates_box))
+			{
+				//极坐标 -> 平面坐标
+				float x = p_point_distance[i] * cos(angle);
+				float y = p_point_distance[i] * sin(angle);
+
+				//判断平面坐标点是否在限制范围
+				if (Point2D_tool::limit_with_point2D_box(x, y, &m_point2D_box))
+				{
+					//平面坐标的转换, 可以进行旋转和平移, 不能缩放
+					point.x = x * m_point2D_transform.m00 + y * m_point2D_transform.m01 + m_point2D_transform.m02;
+					point.y = x * m_point2D_transform.m10 + y * m_point2D_transform.m11 + m_point2D_transform.m12;
+					//添加到最终点云
+					p_cloud_out->push_back(point);
+				}
+				//else直接忽略
+			}
+			//else直接忽略
+		}
+
+	}
+	return Error_code::SUCCESS;
+}
+

wanji_lidar/wj_716_lidar_protocol.h

@@ -0,0 +1,146 @@
+#ifndef WJ_716_LIDAR_PROTOCOL_H
+#define WJ_716_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+#include <mutex>
+#include <chrono>
+#include <math.h>
+
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/common/common.h>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::Message;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+
+#include "glog/logging.h"
+
+#include "wj_lidar_conf.pb.h"
+
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_queue.h"
+#include "../tool/thread_condition.h"
+#include "../tool/point2D_tool.h"
+#include "../tool/common_data.h"
+
+class Wj_716_lidar_protocol
+{
+public:
+	//万集雷达解析协议状态
+	enum Wanji_lidar_protocol_status
+	{//default E_UNKNOWN = 0
+		E_UNKNOWN               = 0,    //未知
+		E_READY               	= 1,    //正常待机
+
+		E_FAULT					= 10,	//故障
+	};
+	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
+#define WANJI_FRAME_SCAN_CYCLE_MS 		WANJI_716_SCAN_CYCLE_MS
+
+//万集716的通信数据长度, 每帧都是固定 898 byte
+#define WANJI_FRAME_LENGTH_716 		898
+//万集716的数据头
+#define WANJI_FRAME_HEAD_716 		0x02020202
+//万集716的点数, 一共811个点, 分开发送时, 前者405个点, 后者406个点
+#define WANJI_SCAN_POINT_NUMBER_TOTAL 		811
+#define WANJI_SCAN_POINT_NUMBER_FORMER		405
+#define WANJI_SCAN_POINT_NUMBER_LATTER		406
+//万集716的角度范围, 角度-135~135, 弧度-2.35619449~2.35619449
+#define WANJI_SCAN_POINT_ANGLE_MIN			-2.35619449
+#define WANJI_SCAN_POINT_ANGLE_MAX			2.35619449
+//万集716的角度分辨率, (每一度3个点),  135*2/(811-1)=0.333333333, 弧度0.005817764
+#define WANJI_SCAN_POINT_ANGLE_INCREMENT	0.005817764
+
+public:
+	// 构造
+	Wj_716_lidar_protocol();
+	// 析构
+	~Wj_716_lidar_protocol();
+
+	// 初始化
+	Error_manager init(Thread_safe_queue<Binary_buf*>* p_communication_data_queue,
+					   Point2D_tool::Polar_coordinates_box polar_coordinates_box,
+					   Point2D_tool::Point2D_box point2D_box,
+					   Point2D_tool::Point2D_transform 	point2D_transform);
+	//反初始化
+	Error_manager uninit();
+
+	//检查状态
+	Error_manager check_status();
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+public:
+	//获取状态
+	Wanji_lidar_protocol_status get_status();
+
+	//获取扫描周期
+	int get_scan_cycle();
+
+	// 获取扫描点云
+	Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+	//获取二次回波点云
+	Error_manager get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+	// 获取扫描点云更新时间点
+	std::chrono::system_clock::time_point get_scan_time();
+	std::chrono::system_clock::time_point get_two_echo_time();
+
+private:
+	//解析线程函数
+	void thread_analysis();
+	//检查数据头和长度, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
+	bool check_head_and_length(Binary_buf* p_binary_buf);
+	// 消息完整性检查, 检查末尾的校验码, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
+	bool check_xor(Binary_buf* p_binary_buf);
+	// 万集通信协议, 解析数据, 转化为点云
+	Error_manager data_protocol_analysis(Binary_buf* p_binary_buf);
+	// 把点到雷达的距离, 转化为标准坐标下的点云
+	Error_manager polar_coordinates_transform_cloud(float* p_point_distance, int number, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+
+protected:
+	//状态
+	std::atomic<Wanji_lidar_protocol_status>			m_wanji_lidar_protocol_status;	//万集雷达解析协议状态
+
+	//数据
+	Thread_safe_queue<Binary_buf*>*		mp_communication_data_queue;	//通信数据队列, 内存由上级管理
+	//雷达扫描点的极坐标
+	float 								m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_TOTAL];//雷达扫描点的距离, 核心数据
+	float 								m_scan_point_intensity[WANJI_SCAN_POINT_NUMBER_TOTAL];//雷达扫描点的光强,
+	float 								m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_TOTAL];//二次反射点的距离
+	unsigned int 						m_current_frame_number;		//当前数据的帧号
+
+	//雷达扫描点平面坐标
+	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;				//扫描点的点云, 内存由本类管理
+	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_two_echo_points;			//二次反射点的点云, 内存由本类管理
+	// 点云获取互斥锁
+	std::mutex 							m_scan_mutex;				//扫描点的锁
+	std::mutex 							m_two_echo_mutex;			//二次反射点的锁
+	// 扫描点云更新时间点
+	std::chrono::system_clock::time_point m_scan_cloud_time;		//扫描点的更新时间
+	std::chrono::system_clock::time_point m_two_echo_cloud_time;	//二次反射点的更新时间
+
+	//控制参数
+	Point2D_tool::Polar_coordinates_box m_polar_coordinates_box;	//极坐标的限定范围
+	Point2D_tool::Point2D_box			m_point2D_box;				//平面坐标的限定范围
+	Point2D_tool::Point2D_transform 	m_point2D_transform;		//平面坐标的转换矩阵
+
+	//解析数据的线程, 自动从队列取出原始数据, 然后解析为点云
+	std::thread *						mp_thread_analysis;     	// 解析线程, 内存由本模块管理
+	Thread_condition					m_condition_analysis;		//解析线程的条件变量
+};
+
+#endif // WJ_716_LIDAR_PROTOCOL_H

wanji_lidar/wj_lidar_conf.proto

@@ -0,0 +1,57 @@
+syntax = "proto2";
+package wj;
+
+message wjManagerParams
+{
+    repeated wjLidarParams wj_lidar=1;
+    repeated Region regions=2;
+    optional string fence_data_path=3 [default=""];
+    optional string fence_log_path=4 [default=""];
+    optional string plc_ip_address=5 [default="192.168.0.1"];
+}
+
+message Region
+{
+    required float minx=1;
+    required float maxx=2;
+    required float miny=3;
+    required float maxy=4;
+}
+
+message wjLidarParams
+{
+	optional float angle_min=1 [default=-2.35619449];
+	optional float angle_max=2 [default=2.35619449];
+	optional float angle_increment=3 [default=0.00582];
+	optional float time_increment=4 [default=0.000062];
+	optional int32 range_min=5 [default=0];
+	optional int32 range_max=6 [default=30];
+	required netConfig net_config=7;
+    required Transform2d transform=8;
+	required scanLimit scan_limit=9;
+}
+
+message netConfig
+{
+	optional string ip_address=1 [default=""];
+	optional int32 port=2 [default=8000];
+}
+
+message Transform2d
+{
+    optional float m00=1 [default=1.0];
+    optional float m01=2 [default=0.0];
+    optional float m02=3 [default=0.0];
+    optional float m10=4 [default=0.0];
+    optional float m11=5 [default=1.0];
+    optional float m12=6 [default=0.0];
+}
+
+message scanLimit
+{
+	optional float dist_limit=1 [default=8.0];
+    optional float minx=2 [default=-6];
+    optional float maxx=3 [default=-0.2];
+    optional float miny=4 [default=-3.5];
+    optional float maxy=5 [default=3.5];
+}