5-15 现场 增加horizon 雷达

laser/Laser.cpp

@@ -232,7 +232,6 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 	m_save_flag = is_save;
 	m_points_log_tool.close();
 	m_binary_log_tool.close();
-
 	if (is_save == false)
 	{
 		return Error_code::SUCCESS;
@@ -542,6 +541,8 @@ Error_manager Laser_base::set_laser_matrix(double* p_matrix, int size)
 	}
 	else
 	{
+	    if(m_laser_id==5)
+	        std::cout<<"----------------------   horizon matrix";
 		memcpy(mp_laser_matrix, p_matrix, LASER_MATRIX_ARRAY_SIZE * sizeof(double));
 		return Error_code::SUCCESS;
 	}

laser/LivoxHorizon.cpp

@@ -0,0 +1,78 @@
+
+
+#include "LivoxHorizon.h"
+
+RegisterLaser(Horizon)
+
+
+
+CHorizonLaser::CHorizonLaser(int id, Laser_proto::laser_parameter laser_param)
+:CLivoxLaser(id, laser_param)
+{
+
+        //设备livox扫描最大帧数
+        m_frame_maxnum = laser_param.frame_num();
+        //判断参数类型，
+        if(laser_param.type()=="Horizon")
+        {
+            //填充雷达设备的广播码
+            g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
+            //初始化livox
+            InitLivox();
+        }
+
+
+}
+
+CHorizonLaser::~CHorizonLaser()
+{
+
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CHorizonLaser::start_scan()
+{
+    LOG(INFO) << " Horizon start :"<<this;
+    return CLivoxLaser::start_scan();
+}
+//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
+// 纯虚函数，必须由子类重载，
+Buf_type CHorizonLaser::transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)
+{
+    static int count=0;
+
+    if ( p_binary_buf ==NULL )
+    {
+        return BUF_UNKNOW;
+    }
+    else
+    {
+        //livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
+        LivoxExtendRawPoint *tp_Livox_data = (LivoxExtendRawPoint *)p_binary_buf->get_buf();
+        //计算这一帧数据有多少个三维点。
+        int t_count = p_binary_buf->get_length() / (sizeof(LivoxExtendRawPoint));
+
+        if (t_count <= 0)
+        {
+            return BUF_UNKNOW;
+        }
+        else
+        {
+            //转变三维点格式，并存入vector。
+            for (int i = 0; i < t_count; ++i)
+            {
+                //LivoxExtendRawPoint 转为 CPoint3D
+                //int32_t 转 double。不要信号强度
+                LivoxExtendRawPoint t_livox_point = tp_Livox_data[i];
+                CPoint3D t_point3D;
+                t_point3D.x = t_livox_point.x;
+                t_point3D.y = t_livox_point.y;
+                t_point3D.z = t_livox_point.z;
+                point3D_cloud.push_back(t_point3D);
+//				std::cout << "huli points:" << t_point3D.x << " : " << t_point3D.y << " : " << t_point3D.z<< std::endl;
+            }
+            return BUF_DATA;
+        }
+    }
+
+}
+

laser/LivoxHorizon.h

@@ -0,0 +1,41 @@
+#ifndef __LIVOX_HORIZON__H
+#define __LIVOX_HORIZON__H
+
+#include "Laser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+#include "LivoxLaser.h"
+
+
+
+
+//大疆livox雷达，从Laser_base继承。
+class CHorizonLaser :public CLivoxLaser
+{
+
+public:
+	CHorizonLaser() = delete;
+	CHorizonLaser(const CHorizonLaser& other) = delete;
+	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
+	//input：id： 雷达设备的id，（唯一索引）
+	//input：laser_param：雷达的参数，
+	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
+	CHorizonLaser(int id, Laser_proto::laser_parameter laser_param);
+	~CHorizonLaser();
+    //雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+    virtual Error_manager start_scan();
+
+	//将二进制消息转化为三维点云的功能函数，并将其存入 任务单 , 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
+};
+
+
+
+
+
+
+
+
+#endif

laser/LivoxLaser.cpp

@@ -38,6 +38,28 @@ CLivoxLaser::~CLivoxLaser()
 //雷达链接设备，为3个线程添加线程执行函数。
 Error_manager CLivoxLaser::connect_laser()
 {
+    Error_manager t_error;
+    //设置点云变换矩阵
+    double matrix[12]={0};
+    matrix[0]=m_laser_param.mat_r00();
+    matrix[1]=m_laser_param.mat_r01();
+    matrix[2]=m_laser_param.mat_r02();
+    matrix[3]=m_laser_param.mat_r03();
+
+    matrix[4]=m_laser_param.mat_r10();
+    matrix[5]=m_laser_param.mat_r11();
+    matrix[6]=m_laser_param.mat_r12();
+    matrix[7]=m_laser_param.mat_r13();
+
+    matrix[8]=m_laser_param.mat_r20();
+    matrix[9]=m_laser_param.mat_r21();
+    matrix[10]=m_laser_param.mat_r22();
+    matrix[11]=m_laser_param.mat_r23();
+    t_error = set_laser_matrix(matrix, 12);
+    if ( t_error != Error_code::SUCCESS )
+    {
+        return t_error;
+    }
 	return Laser_base::connect_laser();
 }
 //雷达断开链接，释放3个线程
@@ -89,8 +111,8 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 
 
 		//设置保存文件的路径
-		std::string save_path = mp_laser_task->get_task_save_path();
-		t_error = set_open_save_path(save_path);
+		std::string save_path = mp_laser_task->get_save_path();
+		t_error = set_open_save_path(save_path,m_laser_param.is_save_txt());
 		if ( t_error != Error_code::SUCCESS )
 		{
 			//文件保存文件的路径的设置 允许失败。继续后面的动作
@@ -151,15 +173,16 @@ Error_manager CLivoxLaser::end_task()
 //子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
 bool CLivoxLaser::is_ready()
 {
+
 	//livox雷达设备的状态，livox sdk后台线程的状态
 	if ( m_laser_statu == LASER_READY &&
 		 g_devices[m_handle].device_state != kDeviceStateDisconnect  )
 	{
-		true;
+		return true;
 	}
 	else
 	{
-	    false;
+	    return false;
 	}
 }
 
@@ -259,7 +282,7 @@ void CLivoxLaser::UpdataHandle()
 	}
 }
 
-void CLivoxLaser::OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
+void CLivoxLaser::OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
 	CLivoxLaser* laser = (CLivoxLaser*)data;
 
 	if (status == kStatusSuccess) {
@@ -397,11 +420,22 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 			if (g_count[handle] >= livox->m_frame_maxnum)
 				return;
 			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
-			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
-
-			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
-			livox->m_queue_livox_data.push(data_bin);
-			//std::cout << "huli m_queue_livox_data.push " << std::endl;
+            if(data ->data_type == kCartesian)
+            {
+                LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+                Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
+                livox->m_queue_livox_data.push(data_bin);
+            }
+            else if(data ->data_type == kExtendCartesian)
+            {
+                LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
+                Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxExtendRawPoint));
+                livox->m_queue_livox_data.push(data_bin);
+            }
+            else
+            {
+                return;
+            }
 
 			g_count[handle]++;
 

laser/LivoxLaser.h

@@ -74,7 +74,7 @@ protected:
 	static void LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
 	static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
 	static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
-	static void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+	static void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
 
 protected:
 	uint8_t									m_handle;

laser/LivoxMid100Laser.cpp

@@ -32,28 +32,7 @@ CLivoxMid100Laser::~CLivoxMid100Laser()
 //雷达链接设备，为3个线程添加线程执行函数。
 Error_manager CLivoxMid100Laser::connect_laser()
 {
-	Error_manager t_error;
-	//设置点云变换矩阵
-	double matrix[12]={0};
-	matrix[0]=m_laser_param.mat_r00();
-	matrix[1]=m_laser_param.mat_r01();
-	matrix[2]=m_laser_param.mat_r02();
-	matrix[3]=m_laser_param.mat_r03();
-
-	matrix[4]=m_laser_param.mat_r10();
-	matrix[5]=m_laser_param.mat_r11();
-	matrix[6]=m_laser_param.mat_r12();
-	matrix[7]=m_laser_param.mat_r13();
-
-	matrix[8]=m_laser_param.mat_r20();
-	matrix[9]=m_laser_param.mat_r21();
-	matrix[10]=m_laser_param.mat_r22();
-	matrix[11]=m_laser_param.mat_r23();
-	t_error = set_laser_matrix(matrix, 12);
-	if ( t_error != Error_code::SUCCESS )
-	{
-		return t_error;
-	}
+
 	return CLivoxLaser::connect_laser();
 }
 //雷达断开链接，释放3个线程
@@ -113,8 +92,9 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 
 
 		//设置保存文件的路径
-		std::string save_path = mp_laser_task->get_task_save_path();
-		t_error = set_open_save_path(save_path);
+		std::string save_path = mp_laser_task->get_save_path();
+
+		t_error = set_open_save_path(save_path,m_laser_param.is_save_txt());
 		if ( t_error != Error_code::SUCCESS )
 		{
 			//文件保存文件的路径的设置 允许失败。继续后面的动作

wj_lidar/measure_filter.h

@@ -0,0 +1,140 @@
+/*
+ * @Description: 测量结果滤波器
+ * @Author: yct
+ * @Date: 2021-03-15 14:41:46
+ * @LastEditTime: 2021-03-16 16:30:08
+ * @LastEditors: yct
+ */
+
+#ifndef MEASURE_FILTER_HH
+#define MEASURE_FILTER_HH
+#include "../tool/singleton.h"
+#include "../tool/common_data.h"
+#include "../error_code/error_code.h"
+#include <map>
+#include <deque>
+#include <chrono>
+#include <vector>
+#include <algorithm>
+#include "glog/logging.h"
+
+#define FILTER_SIZE 10
+#define MAX_QUEUE_SIZE 15
+#define MAX_TIME_INTERVAL_MILLI 7000
+
+class Measure_filter : public Singleton<Measure_filter>
+{
+    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+    friend class Singleton<Measure_filter>;
+
+public:
+    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Measure_filter(const Measure_filter &) = delete;
+    Measure_filter &operator=(const Measure_filter &) = delete;
+    ~Measure_filter() = default;
+
+    // 更新测量数据回调
+    void update_data(int terminal_id, Common_data::Car_wheel_information_stamped data)
+    {
+        if(!data.wheel_data.correctness)
+            return;
+//        LOG(INFO) << data.wheel_data.to_string();
+        // 未创建队列
+        if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
+        {
+            std::deque<Common_data::Car_wheel_information_stamped> t_deque;
+            t_deque.push_back(data);
+            m_measure_results_map.insert(std::pair<int, std::deque<Common_data::Car_wheel_information_stamped>>(terminal_id, t_deque));
+//            LOG(INFO) <<"queue inserted";
+        }
+        else//已创建
+        { 
+            m_measure_results_map[terminal_id].push_back(data);
+//            LOG(INFO) <<"data pushed "<<m_measure_results_map[terminal_id].size();
+            // 剔除超时数据
+            while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-m_measure_results_map[terminal_id].front().measure_time).count() > MAX_TIME_INTERVAL_MILLI)
+            {
+                m_measure_results_map[terminal_id].pop_front();
+            }
+            // 维持队列长度
+            while(m_measure_results_map[terminal_id].size() > MAX_QUEUE_SIZE)
+            {
+                m_measure_results_map[terminal_id].pop_front();
+            }
+        }
+    }
+
+    bool has_enough_data(int terminal_id)
+    {
+        if(m_measure_results_map.find(terminal_id) != m_measure_results_map.end() && m_measure_results_map[terminal_id].size() >= FILTER_SIZE)
+            return true;
+        else
+            return false;
+    }
+
+    // 获取滤波后最新测量数据
+    Error_manager get_filtered_wheel_information(int terminal_id, Common_data::Car_wheel_information& result)
+    {
+        // 检查数据量
+        if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map.find(terminal_id)->second.size() < FILTER_SIZE)
+        {
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR,
+                    (std::string("缺少足够用于滤波的结果")+
+                    std::to_string(terminal_id)+
+                    std::string(m_measure_results_map.find(terminal_id) == m_measure_results_map.end()?
+                        "end":
+                        std::to_string(m_measure_results_map.find(terminal_id)->second.size()))).c_str());
+        }
+        // 填充待滤波结果到数组
+        std::deque<Common_data::Car_wheel_information_stamped>* tp_deque = &m_measure_results_map.find(terminal_id)->second;
+        std::vector<Common_data::Car_wheel_information_stamped> t_result_vec;
+        Common_data::Car_wheel_information_stamped t_avg_result;
+        for (std::deque<Common_data::Car_wheel_information_stamped>::reverse_iterator t_iter = tp_deque->rbegin(); t_iter != tp_deque->rend(); t_iter++)
+        {
+            t_result_vec.push_back(*t_iter);
+            t_avg_result += *t_iter;
+            if (t_result_vec.size() >= FILTER_SIZE)
+                break;
+        }
+        if(t_result_vec.size()<FILTER_SIZE)
+            return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, "结果缺失");
+        // 各结果计算平均,并打分
+        t_avg_result /= t_result_vec.size();
+        std::vector<float> t_score_vec, t_sorted_score_vec;
+        for (size_t i = 0; i < t_result_vec.size(); i++)
+        {
+            t_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
+            t_sorted_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
+        }
+        // 排序,丢掉最高两个(即与均值偏差最大两个,剩下平均获得最终值)
+        std::sort(t_sorted_score_vec.begin(), t_sorted_score_vec.end());
+        Common_data::Car_wheel_information_stamped t_final_result;
+        int t_result_count = 0;
+        for (size_t i = 0; i < t_score_vec.size(); i++)
+        {
+            if(t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-1] || t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-2])
+            {
+                continue;
+            }else{
+                t_final_result += t_result_vec[i];
+                t_result_count++;
+            }
+        }
+        if(t_result_count<=0)
+            return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, "结果波动");
+        t_final_result /= t_result_count;
+        result = t_final_result.wheel_data;
+
+//        LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
+        return SUCCESS;
+    }
+
+private:
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    Measure_filter() = default;
+
+    // 各终端测量数据队列
+    std::map<int, std::deque<Common_data::Car_wheel_information_stamped> > m_measure_results_map;
+};
+
+#endif // !MEASURE_FILTER_HH