add 716N lidar support, need test

setting/wanji_manager.prototxt

@@ -2,278 +2,6 @@ fence_data_path:"/home/zx/data/fence_wj"
 #fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
 distribution_mode:false
 
-# #1
-# 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.201"
-#         port:2112
-#     }
-#     transform
-#     {
-#         m00:-0.100059
-# 		m01:0.9949814
-# 		m02:-1.470
-# 		m10:0.9949814
-# 		m11:0.1000593
-# 		m12:-2.285
-#     }
-#     scan_limit
-#     {
-#         dist_limit:8
-#         minx:0.1
-#         maxx:5.5
-#         miny:-4
-#         maxy:4
-#     }
-#     lidar_id:0
-# }
-
-#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:2112
-    }
-    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
-    }
-    lidar_id:1
-}
-
-#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:2112
-    }
-    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
-    }
-    lidar_id:2
-}
-
-#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:2112
-    }
-    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
-    }
-    lidar_id:3
-}
-
-#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:2112
-    }
-    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
-    }
-    lidar_id: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:2112
-    }
-    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
-    }
-    lidar_id:5
-}
-
-#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:2112
-    }
-    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
-    }
-    lidar_id:6
-}
-
-# #8
-# 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.208"
-#         port:2112
-#     }
-#     transform
-#     {
-#         m00:0.0473273
-# 		m01:-0.9988794
-# 		m02:-1.398
-# 		m10:-0.9988794
-# 		m11:-0.0473273
-# 		m12:3.35600000
-#     }
-#     scan_limit
-#     {
-#         dist_limit:8
-#         minx:0.1
-#         maxx:5.5
-#         miny:-4
-#         maxy:4
-#     }
-#     lidar_id:7
-# }
-
 #9
 wj_lidar
 {
@@ -285,7 +13,7 @@ wj_lidar
     range_max:30
     net_config
     {
-        ip_address:"192.168.2.209"
+        ip_address:"192.168.1.219"
         port:2112
     }
     transform
@@ -319,7 +47,7 @@ wj_lidar
     range_max:30
     net_config
     {
-        ip_address:"192.168.2.210"
+        ip_address:"192.168.1.220"
         port:2112
     }
     transform
@@ -353,7 +81,7 @@ wj_lidar
     range_max:30
     net_config
     {
-        ip_address:"192.168.2.211"
+        ip_address:"192.168.1.221"
         port:2112
     }
     transform
@@ -387,7 +115,7 @@ wj_lidar
     range_max:30
     net_config
     {
-        ip_address:"192.168.2.212"
+        ip_address:"192.168.1.222"
         port:2112
     }
     transform
@@ -410,75 +138,6 @@ wj_lidar
     lidar_id:11
 }
 
-#13
-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.213"
-        port:2112
-    }
-    transform
-    {
-        m00:-0.0459619
-		m01:0.9989432
-		m02:15.555
-		m10:0.9989432
-		m11:0.0459619
-		m12:-2.5410000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:3.25
-    }
-    lidar_id:12
-}
-
-#14
-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.214"
-        port:2112
-    }
-    transform
-    {
-        m00:-0.1357627
-		m01:-0.9907414
-		m02:18.616
-		m10:-0.9907414
-		m11:0.1357627
-		m12:3.48700000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5
-        miny:-4
-        maxy:4
-    }
-    lidar_id:13
-}
-
-
 # #1
 # regions
 # {
@@ -504,29 +163,92 @@ wj_lidar
 # 	maxy:3.43
 # }
 
-#4
+# #4
+# regions
+# {
+#     # minx:8.7
+#     # maxx:12
+#     minx:-1.5
+#     maxx:1.5
+#     miny:-2.40
+#     maxy:3.45
+
+#     minz:0.025
+# 	maxz:0.5
+#     region_id:3
+#     turnplate_cx:0.0
+#     turnplate_cy:0.0
+#     border_minx:-1.3
+#     border_maxx:1.3
+#     plc_offsetx:0.0
+#     plc_offsety:0.0
+#     plc_offset_degree:0.0
+#     plc_border_miny:-7.51
+#     car_min_width:1.55
+#     car_max_width:1.95
+#     car_min_wheelbase:2.3
+#     car_max_wheelbase:3.15
+#     turnplate_angle_limit_anti_clockwise:5.3
+#     turnplate_angle_limit_clockwise:5.3
+
+#     lidar_exts
+#     {
+#         lidar_id:3
+#         calib
+#         {
+#             cx:-14.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:4
+#         calib
+#         {
+#             cx:-14.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:10
+#         calib
+#         {
+#             cx:-14.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:11
+#         calib
+#         {
+#             cx:-14.3
+#             cy:0.0
+#         }
+#     }
+# }
+
+#5
 regions
 {
-    # minx:8.7
-    # maxx:12
-    minx:-1.5
-    maxx:1.5
-    miny:-2.40
-    maxy:3.45
-
-    minz:0.025
+    minx:-1.6
+	maxx:1.6
+	miny:-2.6
+	maxy:2.6
+	minz:0.025
 	maxz:0.5
-    region_id:3
+    region_id:5
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
-    plc_offset_degree:0.0
+    border_minx:-1.2
+    border_maxx:1.2
+    plc_offsetx:1.913
+    plc_offsety:-6.078
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
-    car_max_width:1.95
+    car_max_width:1.92
     car_min_wheelbase:2.3
     car_max_wheelbase:3.15
     turnplate_angle_limit_anti_clockwise:5.3
@@ -534,19 +256,19 @@ regions
 
     lidar_exts
     {
-        lidar_id:3
+        lidar_id:8
         calib
         {
-            cx:-14.3
+            cx:0.0
             cy:0.0
         }
     }
     lidar_exts
     {
-        lidar_id:4
+        lidar_id:9
         calib
         {
-            cx:-14.3
+            cx:0.0
             cy:0.0
         }
     }
@@ -555,7 +277,7 @@ regions
         lidar_id:10
         calib
         {
-            cx:-14.3
+            cx:0.0
             cy:0.0
         }
     }
@@ -564,83 +286,74 @@ regions
         lidar_id:11
         calib
         {
-            cx:-14.3
+            cx:0.0
             cy:0.0
         }
     }
 }
 
-# #5
+# #6
 # regions
 # {
-#     minx:12.1
-#     maxx:15.4
+#     # minx:15.8
+#     # maxx:18.8
+#     minx:-1.5
+#     maxx:1.5
 #     miny:-2.40
 #     maxy:3.45
-# }
 
-#6
-regions
-{
-    # minx:15.8
-    # maxx:18.8
-    minx:-1.5
-    maxx:1.5
-    miny:-2.40
-    maxy:3.45
-
-    minz:0.025
-	maxz:0.5
-    region_id:5
-    turnplate_cx:0.0
-    turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
-    plc_offset_degree:0.0
-    plc_border_miny:-7.51
-    car_min_width:1.55
-    car_max_width:1.95
-    car_min_wheelbase:2.3
-    car_max_wheelbase:3.15
-    turnplate_angle_limit_anti_clockwise:5.3
-    turnplate_angle_limit_clockwise:5.3
+#     minz:0.025
+# 	maxz:0.5
+#     region_id:5
+#     turnplate_cx:0.0
+#     turnplate_cy:0.0
+#     border_minx:-1.3
+#     border_maxx:1.3
+#     plc_offsetx:0.0
+#     plc_offsety:0.0
+#     plc_offset_degree:0.0
+#     plc_border_miny:-7.51
+#     car_min_width:1.55
+#     car_max_width:1.95
+#     car_min_wheelbase:2.3
+#     car_max_wheelbase:3.15
+#     turnplate_angle_limit_anti_clockwise:5.3
+#     turnplate_angle_limit_clockwise:5.3
 
-    lidar_exts
-    {
-        lidar_id:5
-        calib
-        {
-            cx:-17.3
-            cy:0.0
-        }
-    }
-    lidar_exts
-    {
-        lidar_id:6
-        calib
-        {
-            cx:-17.3
-            cy:0.0
-        }
-    }
-    lidar_exts
-    {
-        lidar_id:12
-        calib
-        {
-            cx:-17.3
-            cy:0.0
-        }
-    }
-    lidar_exts
-    {
-        lidar_id:13
-        calib
-        {
-            cx:-17.3
-            cy:0.0
-        }
-    }
-}
+#     lidar_exts
+#     {
+#         lidar_id:5
+#         calib
+#         {
+#             cx:-17.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:6
+#         calib
+#         {
+#             cx:-17.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:12
+#         calib
+#         {
+#             cx:-17.3
+#             cy:0.0
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:13
+#         calib
+#         {
+#             cx:-17.3
+#             cy:0.0
+#         }
+#     }
+# }

system/system_executor.cpp

@@ -488,12 +488,23 @@ Error_manager System_executor::encapsulate_send_status()
 	}
 
 	// wj lidar
+#if LIDAR_TYPE == 1
 	std::map<int, Wanji_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
 	std::map<int, Wanji_lidar_device::Wanji_lidar_device_status> t_wj_lidar_status_map;
 	for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
 	{
 		t_wj_lidar_status_map.emplace(std::pair<int, Wanji_lidar_device::Wanji_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
 	}
+#else
+	std::map<int, Wanji_716N_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
+	std::map<int, Wanji_716N_lidar_device::Wanji_716N_device_status> t_wj_lidar_status_map;
+	for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
+	{
+		t_wj_lidar_status_map.emplace(std::pair<int, Wanji_716N_lidar_device::Wanji_716N_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
+	}
+#endif
+	
+	
 
 	// wj region
 	std::map<int, Region_worker *> t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
@@ -552,7 +563,11 @@ Error_manager System_executor::encapsulate_send_status()
 			// 
 			for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
 			{
-				std::map<int, Wanji_lidar_device::Wanji_lidar_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
+#if LIDAR_TYPE == 1
+	std::map<int, Wanji_lidar_device::Wanji_lidar_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
+#else
+	std::map<int, Wanji_716N_lidar_device::Wanji_716N_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
+#endif
 				if (t_status_iter == t_wj_lidar_status_map.end())
 				{
 					LOG(WARNING) << "wj lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";

wanji_lidar/wanji_716N_device.cpp

@@ -0,0 +1,278 @@
+#include "wanji_716N_device.h"
+
+
+
+// 万集雷达封装类构造函数
+Wanji_716N_lidar_device::Wanji_716N_lidar_device()
+{
+	m_wanji_lidar_device_status = E_UNKNOWN;
+}
+
+// 万集雷达封装类析构函数
+Wanji_716N_lidar_device::~Wanji_716N_lidar_device()
+{
+	uninit();
+}
+
+// 初始化
+Error_manager Wanji_716N_lidar_device::init(wj::wjLidarParams params)
+{
+	Error_manager t_error;
+	LOG(INFO) << " Wanji_716N_lidar_device::init "<< this;
+
+	//唤醒队列
+	m_communication_data_queue.wake_queue();
+
+	m_lidar_id = params.lidar_id();
+
+	//控制参数
+	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(t_ip, t_port, &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_716N_lidar_device::execute_thread_fun, this);
+
+	m_wanji_lidar_device_status = E_READY;
+
+	return t_error;
+}
+
+//反初始化
+Error_manager Wanji_716N_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_716N_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_716N_lidar_device::check_status() error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Wanji_716N_lidar_device::check_status() error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Wanji_716N_lidar_device::is_ready()
+{
+	updata_status();
+	return m_wanji_lidar_device_status == E_READY;
+}
+
+//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+Error_manager Wanji_716N_lidar_device::get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+												   std::chrono::system_clock::time_point command_time, int timeout_ms)
+{
+	if ( p_mutex == NULL || p_cloud.get() == NULL )
+	{
+	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+	    					"  POINTER IS NULL ");
+	}
+	//判断是否超时
+	while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
+	{
+		//获取指令时间之后的点云, 如果没有就会循环
+		if( m_protocol.get_scan_time() > command_time  )
+		{
+			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_716N_lidar_device::get_new_cloud_and_wait error ");
+}
+
+//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
+Error_manager Wanji_716N_lidar_device::get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													std::chrono::system_clock::time_point command_time)
+{
+	if ( p_mutex == NULL || p_cloud.get() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
+	if( m_protocol.get_scan_time() > command_time )
+	{
+		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_716N_lidar_device::get_current_cloud error ");
+	}
+}
+
+//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													std::chrono::system_clock::time_point command_time)
+{
+	if ( p_mutex == NULL || p_cloud.get() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	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_716N_lidar_device::get_last_cloud error ");
+	}
+}
+
+//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+//注:函数内部不加锁, 由外部统一加锁.
+Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+												 std::chrono::system_clock::time_point command_time)
+{
+	if ( p_cloud.get() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	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) )
+	{
+		Error_manager code = m_protocol.get_scan_points(p_cloud);
+//		LOG(WARNING) << p_cloud->size();
+		return code;
+	}
+	else
+	{
+//        LOG(WARNING) << "get cloud failed.....";
+		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+							 " Wanji_716N_lidar_device::get_last_cloud error ");
+	}
+}
+
+Wanji_716N_lidar_device::Wanji_716N_device_status Wanji_716N_lidar_device::get_status()
+{
+	updata_status();
+	return m_wanji_lidar_device_status;
+}
+
+void Wanji_716N_lidar_device::updata_status()
+{
+	Async_Client::Communication_status communication_status = m_async_client.get_status();
+	// Wj_716N_lidar_protocol::Wanji_716N_device_status wanji_lidar_protocol_status = m_protocol.get_status();
+
+	if ( communication_status == Async_Client::Communication_status::E_FAULT)// ||
+		//  wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_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_716N_lidar_protocol::Wanji_716N_device_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;
+	}
+}
+

wanji_lidar/wanji_716N_device.h

@@ -0,0 +1,105 @@
+/*
+ * @Author: yct 18202736439@163.com
+ * @Date: 2022-09-22 18:00:51
+ * @LastEditors: yct 18202736439@163.com
+ * @LastEditTime: 2022-09-25 22:10:59
+ * @FilePath: /puai_wj_2021/wanji_lidar/wanji_716N_device.h
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#ifndef WJ_716N_ENCAPSULATION_HH
+#define WJ_716N_ENCAPSULATION_HH
+
+#include <chrono>
+#include <mutex>
+#include <thread>
+#include <atomic>
+#include <string>
+#include <unistd.h>
+#include "glog/logging.h"
+
+#include "async_client.h"
+#include "wj_716N_lidar_protocol.h"
+
+#include "wj_lidar_conf.pb.h"
+
+class Wanji_716N_lidar_device
+{
+public:
+	//万集设备身状态
+	enum Wanji_716N_device_status
+	{
+		E_UNKNOWN              	= 0,    //未知
+		E_READY	             	= 1,    //正常待机
+		E_DISCONNECT			= 2,	//断连
+
+		E_BUSY					= 3, 	//工作正忙
+
+		E_FAULT					=10,	//故障
+	};
+
+public:
+    // 无参构造函数
+    Wanji_716N_lidar_device();
+    // 析构函数
+    ~Wanji_716N_lidar_device();
+
+	// 初始化
+	Error_manager init(wj::wjLidarParams params);
+	//反初始化
+	Error_manager uninit();
+
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+
+	int get_lidar_id() { return m_lidar_id; }
+
+	//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+	Error_manager get_new_cloud_and_wait(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);
+	//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+	Error_manager get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+							std::chrono::system_clock::time_point command_time);
+
+	//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+	//注:函数内部不加锁, 由外部统一加锁.
+	Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+								 std::chrono::system_clock::time_point command_time);
+public:
+	Wanji_716N_device_status get_status();
+protected:
+	void updata_status();
+	// //mp_laser_manager_thread 线程执行函数，
+	// //thread_work 内部线程负责获取点云结果
+	// void execute_thread_fun();
+
+
+
+protected:
+	// 雷达编号
+	int m_lidar_id;
+
+	//状态
+	Wanji_716N_device_status 					m_wanji_lidar_device_status;	//万集设备身状态
+
+	//子模块
+	Async_Client								m_async_client;		//异步客户端, 负责雷达通信, 接受雷达数据
+	wj_716N_lidar_protocol						m_protocol;        // 万集雷达协议
+
+	//中间缓存
+	Thread_safe_queue<Binary_buf*>				m_communication_data_queue;	//通信数据队列
+
+
+	//任务执行线程
+	std::thread*        						mp_execute_thread;   			//执行的线程指针，内存由本类管理
+	Thread_condition							m_execute_condition;			//执行的条件变量
+
+};
+
+#endif // !WJ_716N_ENCAPSULATION_HH

wanji_lidar/wanji_manager.cpp

@@ -130,7 +130,12 @@ Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParam
 		// 非分布式模式创建所有雷达，否则只创建对应区域标记的雷达
 		if (!wanji_parameters.distribution_mode() || (wanji_parameters.distribution_mode() && lidar_selection_set.find(wanji_parameters.wj_lidar(i).lidar_id()) != lidar_selection_set.end() ))
 		{
-			Wanji_lidar_device *p_wj_lidar_device(new Wanji_lidar_device);
+#if LIDAR_TYPE == 1
+	Wanji_lidar_device *p_wj_lidar_device(new Wanji_lidar_device);
+#else
+	Wanji_716N_lidar_device *p_wj_lidar_device(new Wanji_716N_lidar_device);
+#endif
+			// Wanji_lidar_device *p_wj_lidar_device(new Wanji_lidar_device);
 			t_error = p_wj_lidar_device->init(wanji_parameters.wj_lidar(i));
 			if (t_error != Error_code::SUCCESS)
 			{
@@ -380,10 +385,15 @@ 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()
+#if LIDAR_TYPE == 1
+	std::map<int, Wanji_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
+#else
+	std::map<int, Wanji_716N_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
+#endif
 {
 	return m_wanji_lidar_device_map;
 }
+
 std::map<int, Region_worker *> & Wanji_manager::get_region_worker_map()
 {
 	return m_region_worker_map;
@@ -433,7 +443,6 @@ void Wanji_manager::collect_cloud_thread_fun()
 						save_cloud_txt(t_cloud, t_filename);
 						LOG(INFO) << "lidar intrin cloud has been saved in " + t_filename;
 					}
-					
 					// 点云放入对应缓存
 					for (std::map<int, Region_worker *>::iterator iter_region = m_region_worker_map.begin(); iter_region != m_region_worker_map.end(); iter_region++)
 					{
@@ -461,7 +470,6 @@ void Wanji_manager::collect_cloud_thread_fun()
 						}
 					}
 				}
-
 				update_region_cloud();
 
 // 				if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )

wanji_lidar/wanji_manager.h

@@ -12,11 +12,15 @@
 #include <thread>
 #include <map>
 
+#include "../wanji_lidar/wanji_716N_device.h"
 #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"
 
+// 1 716, 2 716N
+#define LIDAR_TYPE 2
+
 struct Timestamped_cloud_wj
 {
 	public:
@@ -160,7 +164,13 @@ public://API functions
 
 public://get or set member variable
 	Wanji_manager_status get_status();
+
+#if LIDAR_TYPE == 1
 	std::map<int, Wanji_lidar_device*> & get_wanji_lidar_device_map();
+#else
+	std::map<int, Wanji_716N_lidar_device*> & get_wanji_lidar_device_map();
+#endif
+
 	std::map<int, Region_worker *> & get_region_worker_map();
 protected://member functions
 	//自动收集点云的线程函数
@@ -185,7 +195,12 @@ protected://member variable
 	//状态
 	Wanji_manager_status						m_wanji_manager_status;			//万集管理模块的状态
 
+#if LIDAR_TYPE == 1
 	std::map<int, Wanji_lidar_device *> 		m_wanji_lidar_device_map; 		// 万集雷达实例指针数组, 内存由本类管理
+#else
+	std::map<int, Wanji_716N_lidar_device *> 		m_wanji_lidar_device_map;
+#endif
+	
 	std::map<int, Region_worker *> 				m_region_worker_map;  			// 区域功能实例指针数组, 内存由本类管理
 
 

wanji_lidar/wj_716N_lidar_protocol.cpp

@@ -0,0 +1,458 @@
+#include "wj_716N_lidar_protocol.h"
+#include <iostream>
+
+
+
+wj_716N_lidar_protocol::wj_716N_lidar_protocol()
+{
+    memset(&m_sdata, 0, sizeof(m_sdata));
+//        marker_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 50);
+//        ros::Time scan_time = ros::Time::now();
+//        scan.header.stamp = scan_time;
+    long scan_time=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    scan.header.stamp.msecs = scan_time;
+
+    mp_communication_data_queue = NULL;
+
+    freq_scan = 1;
+    m_u32PreFrameNo = 0;
+    m_u32ExpectedPackageNo = 0;
+    m_n32currentDataNo = 0;
+    total_point = 1081;
+
+    scan.header.frame_id = "wj_716N_lidar_frame";
+    scan.angle_min = -2.35619449;
+    scan.angle_max = 2.35619449;
+    scan.angle_increment = 0.017453 / 4;
+    scan.time_increment = 1 / 15.00000000 / 1440;
+    scan.range_min = 0;
+    scan.range_max = 30;
+    scan.ranges.resize(1081);
+    scan.intensities.resize(1081);
+    scan.x.resize(1081);
+    scan.y.resize(1081);
+    scan.angle_to_point.resize(1081);
+
+    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;
+    // adjust angle_max to max_ang config param
+    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
+
+    mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    mp_thread_analysis = nullptr;
+
+    std::cout << "wj_716N_lidar_protocl start success" << std::endl;
+}
+
+bool wj_716N_lidar_protocol::setConfig(wj_716_lidarConfig &new_config, uint32_t level)
+{
+
+    config_ = new_config;
+    scan.header.frame_id = config_.frame_id;
+    scan.angle_min = config_.min_ang;
+    scan.angle_max = config_.max_ang;
+    scan.range_min = config_.range_min;
+    scan.range_max = config_.range_max;
+    freq_scan = config_.frequency_scan;
+
+    scan.angle_increment = 0.017453 / 4;
+    if (freq_scan == 1) //0.25°_15hz
+    {
+        scan.time_increment = 1 / 15.00000000 / 1440;
+        total_point = 1081;
+    }
+    else if (freq_scan == 2) //0.25°_25hz
+    {
+        scan.time_increment = 1 / 25.00000000 / 1440;
+        total_point = 1081;
+    }
+
+    // adjust angle_min to min_ang config param
+    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;
+    // adjust angle_max to max_ang config param
+    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
+    int samples = index_end - index_start;
+//        scan.ranges.resize(samples);
+//        scan.intensities.resize(samples);
+
+    std::cout << "frame_id:" << scan.header.frame_id << std::endl;
+    std::cout << "min_ang:" << scan.angle_min << std::endl;
+    std::cout << "max_ang:" << scan.angle_max << std::endl;
+    std::cout << "angle_increment:" << scan.angle_increment << std::endl;
+    std::cout << "time_increment:" << scan.time_increment << std::endl;
+    std::cout << "range_min:" << scan.range_min << std::endl;
+    std::cout << "range_max:" << scan.range_max << std::endl;
+    std::cout << "samples_per_scan:" << samples << std::endl;
+    return true;
+}
+
+// 初始化
+Error_manager wj_716N_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 ");
+	}
+    wj_716_lidarConfig t_config;
+    t_config.min_ang = polar_coordinates_box.angle_min;
+    t_config.max_ang = polar_coordinates_box.angle_max;
+    t_config.range_min = polar_coordinates_box.distance_min;
+    t_config.range_max = polar_coordinates_box.distance_max;
+    setConfig(t_config, 0);
+
+	mp_communication_data_queue = p_communication_data_queue;
+	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_716N_lidar_protocol::thread_analysis, this);
+
+	return Error_code::SUCCESS;
+}
+
+//反初始化
+Error_manager wj_716N_lidar_protocol::uninit()
+{
+	LOG(INFO) << " ---wj_716N_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;
+
+	return Error_code::SUCCESS;
+}
+
+//获取扫描点云
+Error_manager wj_716N_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;
+}
+
+//解析线程函数
+void wj_716N_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(dataProcess((unsigned char *)tp_binary_buf->get_buf(), tp_binary_buf->get_length()))
+                    {
+                        t_error = SUCCESS;
+                    }else{
+                        t_error = ERROR;
+                    }
+					//else 错误不管, 当做无效消息处理
+					delete(tp_binary_buf);
+				}
+			}
+		}
+	}
+	LOG(INFO) << " -Wj_716_lidar_protocol::thread_analysis end :" << this;
+	return;
+}
+
+
+bool wj_716N_lidar_protocol::dataProcess(unsigned char *data, const int reclen)
+    {
+
+        if (reclen > MAX_LENGTH_DATA_PROCESS)
+        {
+            m_sdata.m_u32out = 0;
+            m_sdata.m_u32in = 0;
+            return false;
+        }
+
+        if (m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
+        {
+            m_sdata.m_u32out = 0;
+            m_sdata.m_u32in = 0;
+            return false;
+        }
+        memcpy(&m_sdata.m_acdata[m_sdata.m_u32in], data, reclen * sizeof(char));
+        m_sdata.m_u32in += reclen;
+        while (m_sdata.m_u32out < m_sdata.m_u32in)
+        {
+            if (m_sdata.m_acdata[m_sdata.m_u32out] == 0xFF && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0xAA)
+            {
+                unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 2] << 8) |
+                                        (m_sdata.m_acdata[m_sdata.m_u32out + 3] << 0);
+                l_u32reallen = l_u32reallen + 4;
+
+                if (l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
+                {
+                    if (OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out], l_u32reallen))
+                    {
+                        m_sdata.m_u32out += l_u32reallen;
+                    }
+                    else
+                    {
+                        std::cout << "continuous search frame header" << std::endl;
+                        m_sdata.m_u32out++;
+                    }
+                }
+                else if (l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
+                {
+                    m_sdata.m_u32out++;
+                }
+                else
+                {
+                    break;
+                }
+            }
+            else
+            {
+                m_sdata.m_u32out++;
+            }
+        } //end while(m_sdata.m_u32out < m_sdata.m_u32in)
+
+        if (m_sdata.m_u32out >= m_sdata.m_u32in)
+        {
+            m_sdata.m_u32out = 0;
+            m_sdata.m_u32in = 0;
+        }
+        else if (m_sdata.m_u32out < m_sdata.m_u32in && m_sdata.m_u32out != 0)
+        {
+            movedata(m_sdata);
+        }
+        return true;
+    }
+
+    void wj_716N_lidar_protocol::movedata(DataCache &sdata)
+    {
+        for (int i = sdata.m_u32out; i < sdata.m_u32in; i++)
+        {
+            sdata.m_acdata[i - sdata.m_u32out] = sdata.m_acdata[i];
+        }
+        sdata.m_u32in = sdata.m_u32in - sdata.m_u32out;
+        sdata.m_u32out = 0;
+    }
+
+    bool wj_716N_lidar_protocol::OnRecvProcess(unsigned char *data, int len)
+    {
+        if (len > 0)
+        {
+            if (checkXor(data, len))
+            {
+                protocl(data, len);
+            }
+            else
+            {
+                return false;
+            }
+        }
+        else
+        {
+            return false;
+        }
+        return true;
+    }
+
+    bool wj_716N_lidar_protocol::protocl(unsigned char *data, const int len)
+    {
+
+        if ((data[22] == 0x02 && data[23] == 0x02) || (data[22] == 0x02 && data[23] == 0x01)) //command type:0x02 0x01/0X02
+        {
+
+            heartstate = true;
+            int l_n32TotalPackage = data[80];
+            int l_n32PackageNo = data[81];
+            unsigned int l_u32FrameNo = (data[75] << 24) + (data[76] << 16) + (data[77] << 8) + data[78];
+            int l_n32PointNum = (data[83] << 8) + data[84];
+            int l_n32Frequency = data[79];
+
+            if(l_n32Frequency != freq_scan)
+            {
+                std::cout << "The scan frequency does not match the one you setted!"<< std::endl;
+                return false;
+            }
+
+
+
+            if(m_u32PreFrameNo != l_u32FrameNo)
+            {
+                m_u32PreFrameNo = l_u32FrameNo;
+                m_u32ExpectedPackageNo = 1;
+                m_n32currentDataNo = 0;
+            }
+
+            if (l_n32PackageNo == m_u32ExpectedPackageNo && m_u32PreFrameNo == l_u32FrameNo)
+            {
+                if(data[82] == 0x00) //Dist
+                {
+                    for (int j = 0; j < l_n32PointNum; j++)
+                    {
+                        scandata[m_n32currentDataNo] = (((unsigned char)data[85 + j * 2]) << 8) +
+                                                       ((unsigned char)data[86 + j * 2]);
+                        scandata[m_n32currentDataNo] /= 1000.0;
+                        scanintensity[m_n32currentDataNo] = 0;
+                        if (scandata[m_n32currentDataNo] > scan.range_max || scandata[m_n32currentDataNo] < scan.range_min || scandata[m_n32currentDataNo] == 0)
+                        {
+                            scandata[m_n32currentDataNo] = NAN;
+                        }
+                        m_n32currentDataNo++;
+                    }
+                    m_u32ExpectedPackageNo++;
+                }
+                else if(data[82] == 0x01 && m_n32currentDataNo >= total_point) //intensities
+                {
+                    for (int j = 0; j < l_n32PointNum; j++)
+                    {
+                        scanintensity[m_n32currentDataNo - total_point] = (((unsigned char)data[85 + j * 2]) << 8) +
+                                                                          ((unsigned char)data[86 + j * 2]);
+                        m_n32currentDataNo++;
+                    }
+                    m_u32ExpectedPackageNo++;
+                }
+
+                if(m_u32ExpectedPackageNo - 1 == l_n32TotalPackage)
+                {
+
+                    m_read_write_mtx.lock();
+                    mp_scan_points->clear();
+                    for (int i = index_start; i < index_end; i++)
+                    {
+                        scan.ranges[i - index_start] = scandata[i];
+                        //std::cout<<scan.ranges[i - index_start]<<std::endl;
+                        update_data(i-index_start);
+
+                        if(scandata[i - index_start] == NAN)
+                        {
+                            scan.intensities[i - index_start] = 0;
+                        }
+                        else
+                        {
+                            scan.intensities[i - index_start] = scanintensity[i];
+                        }
+
+                        pcl::PointXYZ point;
+                        //判断平面坐标点是否在限制范围
+                        double x = scan.x[i-index_start];
+                        double y = scan.y[i-index_start];
+                        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;
+                            //添加到最终点云
+                            mp_scan_points->push_back(point);
+                        }
+                    }
+                    scan.header.stamp.msecs=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+                    m_read_write_mtx.unlock();
+
+//                    ros::Time scan_time = ros::Time::now();
+//                    scan.header.stamp = scan_time;
+//                    marker_pub.publish(scan);
+                }
+            } 
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    //index点数组下标【0～l_n32PointCount-1】 l_n32PointCount总点数
+    void wj_716N_lidar_protocol::update_data(int index)
+    {
+        if(index>=total_point)
+        {
+            return;
+        }
+
+//    std::cout<<"该圈的数据总条数："<<total_point_number_per_circle<<std::endl;
+//    std::cout<<"数据点对应的数组下标："<<index<<std::endl;
+//    每个下标对应的弧度
+        scan.angle_to_point[index] = scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1);
+        scan.y[index]=scan.ranges[index]*cos(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));
+        scan.x[index]=scan.ranges[index]*sin(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));
+
+//        scan.angle_to_point[total_point-1-index] = scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1);
+//        scan.y[total_point-1-index]=scan.ranges[index]*cos(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));
+//        scan.x[total_point-1-index]=scan.ranges[index]*sin(scan.angle_min+(total_point-1-index)*(scan.angle_max-scan.angle_min)/(total_point-1));
+
+//    if(index!=540)
+//       std::cout<<"每个点对应的弧度: "<<scan.angle_to_point[total_point-1-index]<<", 每个点对应的距离： "<<scan.ranges[total_point-1-index]<<", (x,y): ("<<scan.x[total_point-1-index]<<","<<scan.y[total_point-1-index]<<")"<<std::endl;
+    }
+
+    bool wj_716N_lidar_protocol::checkXor(unsigned char *recvbuf, int recvlen)
+    {
+        int i = 0;
+        unsigned char check = 0;
+        unsigned char *p = recvbuf;
+        int len;
+        if (*p == 0xFF)
+        {
+            p = p + 2;
+            len = recvlen - 6;
+            for (i = 0; i < len; i++)
+            {
+                check ^= *p++;
+            }
+            p++;
+            if (check == *p)
+            {
+                return true;
+            }
+            else
+                return false;
+        }
+        else
+        {
+            return false;
+        }
+    }
+

wanji_lidar/wj_716N_lidar_protocol.h

@@ -0,0 +1,159 @@
+
+#ifndef WJ_716N_LIDAR_PROTOCOL_HH
+#define WJ_716N_LIDAR_PROTOCOL_HH
+#include <iostream>
+#include <mutex>
+#include <chrono>
+#include <math.h>
+
+#include "glog/logging.h"
+#include "string.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"
+
+#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 "pcl/point_types.h"
+#include "pcl/point_cloud.h"
+#include "pcl/conversions.h"
+#include "pcl/common/common.h"
+
+// using namespace std;
+
+typedef struct {
+    std::string frame_id="laser";
+    double min_ang=-2.35619449;
+    double max_ang=2.35619449;
+    double range_min=0.0;
+    double range_max=30.0;
+    int frequency_scan=1;
+}wj_716_lidarConfig;
+
+#define MAX_LENGTH_DATA_PROCESS 200000
+typedef struct TagDataCache
+{
+    unsigned char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+}DataCache;
+
+
+struct Stamp            //    Laser内参
+{
+    //long secs;
+    long msecs;
+};
+
+struct Header           //    Laser内参
+{
+    int seq;
+    //替换成只用ms表示
+    Stamp stamp;
+    std::string frame_id;
+};
+struct Laser            //    雷达数据及雷达参数
+{
+    Header header;
+    double angle_min;
+    double angle_max;
+    double angle_increment;
+
+    double range_min;
+    double range_max;
+
+    double time_increment;
+    long long scan_time;
+
+    std::vector<double> ranges;
+    std::vector<double> x;
+    std::vector<double> y;
+    std::vector<double> angle_to_point;
+    std::vector<double> intensities;
+};
+
+
+class wj_716N_lidar_protocol{
+public:
+    wj_716N_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();
+
+    //获取扫描周期
+	int get_scan_cycle(){return (config_.frequency_scan==1?67:40);}
+	// 获取扫描点云
+	Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+	// 获取扫描点云更新时间点
+	std::chrono::system_clock::time_point get_scan_time(){return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(std::chrono::milliseconds(scan.scan_time));}
+
+    bool dataProcess(unsigned char *data,const int reclen);
+    bool protocl(unsigned char *data,const int len);
+    bool OnRecvProcess(unsigned char *data, int len);
+    bool checkXor(unsigned char *recvbuf, int recvlen);
+    void send_scan(const char *data,const int len);
+
+    bool setConfig(wj_716_lidarConfig &new_config,uint32_t level);
+
+    void update_data(int index);
+
+//    bool heartstate;
+
+//    wj_716N_lidar::Laser scan;
+//    std::mutex m_read_write_mtx;
+
+
+private:
+
+    //解析线程函数
+	void thread_analysis();
+    void movedata(DataCache &sdata);
+    DataCache   m_sdata;
+    wj_716_lidarConfig config_;
+    unsigned int m_u32PreFrameNo;
+    unsigned int m_u32ExpectedPackageNo;
+    int m_n32currentDataNo;
+    float scandata[1081];
+    float scanintensity[1081];
+
+ public:
+    Laser scan;
+    int freq_scan;
+    int resizeNum;
+    int index_start;
+    int index_end;
+    bool heartstate;
+    int total_point;
+    std::mutex m_read_write_mtx;
+
+    Thread_safe_queue<Binary_buf*>*		mp_communication_data_queue;	//通信数据队列, 内存由上级管理
+
+    //雷达扫描点平面坐标
+	pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;				//扫描点的点云, 内存由本类管理
+	// 点云获取互斥锁
+	std::mutex 							m_scan_mutex;				//扫描点的锁
+
+    //控制参数
+	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_716N_LIDAR_PROTOCOL_HH