增加wj滤波器,待普爱测试稳定性

error_code/error_code.h

@@ -306,7 +306,10 @@ enum Error_code
     WJ_LIDAR_TASK_INVALID_TASK,									//万集任务模块，无效任务
     WJ_LIDAR_TASK_MEASURE_FAILED,								//万集任务模块，测量失败
 
-
+    // 万集滤波
+    WJ_FILTER_ERROR_BASE                                        =0x06060000,
+    WJ_FILTER_LACK_OF_RESULT,                                   //万集滤波,结果不足以进行滤波
+    WJ_FILTER_FLUCTUATING,                                      //万集滤波,结果波动过大
 
     //task module, 任务模块  error from 0x10010000-0x1001FFFF
 	TASK_MODULE_ERROR_BASE 							= 0x10010000,

tool/common_data.h

@@ -4,7 +4,8 @@
 
 #ifndef NNXX_TESTS_COMMON_DATA_H
 #define NNXX_TESTS_COMMON_DATA_H
-
+#include <chrono>
+#include <cmath>
 
 class Common_data
 {
@@ -42,9 +43,91 @@ public:
 		float front_theta = 0;			//整车的前轮的旋转角
 
 		bool correctness = false;			//整车的校准标记位
-
+		public:
+		Car_wheel_information& operator+=(const Car_wheel_information& other)
+		{
+			this->center_x += other.center_x;
+			this->center_y += other.center_y;
+			this->car_angle += other.car_angle;
+			this->wheel_base += other.wheel_base;
+			this->wheel_width += other.wheel_width;
+			this->front_theta += other.front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator-=(const Car_wheel_information& other)
+		{
+			this->center_x -= other.center_x;
+			this->center_y -= other.center_y;
+			this->car_angle -= other.car_angle;
+			this->wheel_base -= other.wheel_base;
+			this->wheel_width -= other.wheel_width;
+			this->front_theta -= other.front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator/=(int scalar)
+		{
+			if(scalar==0)
+				return *this;
+			this->center_x /= scalar;
+			this->center_y /= scalar;
+			this->car_angle /= scalar;
+			this->wheel_base /= scalar;
+			this->wheel_width /= scalar;
+			this->front_theta /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
+		float calc_score()
+		{
+			float weights[] = {1.0f, 1.0f, 0.5f, 1.0f, 0.5f, 0.5f};
+			float final_score = 0.0f;
+			final_score += fabs(weights[0] * this->center_x);
+			final_score += fabs(weights[1] * this->center_y);
+			final_score += fabs(weights[2] * this->car_angle);
+			final_score += fabs(weights[3] * this->wheel_base);
+			final_score += fabs(weights[4] * this->wheel_width);
+			final_score += fabs(weights[5] * this->front_theta);
+
+			return final_score;
+		}
 	};
 
+	// 带时间戳的四轮测量信息
+	struct Car_wheel_information_stamped
+	{
+		Car_wheel_information wheel_data;
+		std::chrono::system_clock::time_point measure_time;
+				public:
+		Car_wheel_information_stamped& operator+=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data += other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped& operator-=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data -= other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped& operator-(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data -= other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped& operator/=(int scalar)
+		{
+			this->wheel_data /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.1
+		float calc_score()
+		{
+			return wheel_data.calc_score();
+		}
+	};
 
 	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);

wanji_lidar/measure_filter.h

@@ -0,0 +1,117 @@
+/*
+ * @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>
+
+#define FILTER_SIZE 5
+#define MAX_QUEUE_SIZE 10
+#define MAX_TIME_INTERVAL_MILLI 1000 
+
+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;
+        // 未创建队列
+        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));
+        }
+        else//已创建
+        { 
+            m_measure_results_map[terminal_id].push_back(data);
+            // 剔除超时数据
+            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();
+            }
+        }
+    }
+
+    // 获取滤波后最新测量数据
+    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::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;
+        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_final_result /= t_result_vec.size() - 2;
+        result = t_final_result.wheel_data;
+        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

wanji_lidar/region_worker.cpp

@@ -4,6 +4,9 @@
 #include "region_worker.h"
 //#include "plc_data.h"
 
+// 测量结果滤波,不影响现有结构
+#include "measure_filter.h"
+
 Region_worker::Region_worker()
 {
 	m_region_worker_status = E_UNKNOWN;
@@ -42,6 +45,14 @@ Error_manager Region_worker::init(Point2D_tool::Point2D_box point2D_box, std::mu
 	return Error_code::SUCCESS;
 }
 
+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, int index)
+{
+	m_index = index;
+
+	return init(point2D_box, p_cloud_collection_mutex, p_cloud_collection);
+}
+
 //开始自动预测的算法线程
 Error_manager Region_worker::start_auto_detect()
 {
@@ -236,6 +247,15 @@ void Region_worker::auto_detect_thread_fun()
 
 			//无论结果如何,都要刷新时间, 表示这次定位已经执行了.
 			m_detect_updata_time = std::chrono::system_clock::now();
+			
+			// 测量正确,更新到滤波器
+			if(m_car_wheel_information.correctness)
+			{
+				Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
+				t_wheel_info_stamped.wheel_data = m_car_wheel_information;
+				t_wheel_info_stamped.measure_time = m_detect_updata_time;
+				Measure_filter::get_instance_references().update_data(m_index, t_wheel_info_stamped);
+			}
 		}
 	}
 	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;

wanji_lidar/region_worker.h

@@ -16,8 +16,6 @@
 #include "../verify/Verify_result.h"
 #include "../tool/thread_condition.h"
 #include "../tool/common_data.h"
-
-
 /**
  * 区域功能类，负责自动检测区域状态并更新到plc
  * */
@@ -35,24 +33,27 @@ public:
 #define REGION_WORKER_DETECT_ERROR 3
 
 //区域功能类 的预测算法的时间周期, 一般为100ms, 我们这里设一个较大值200ms
-#define REGION_WORKER_DETECT_CYCLE_MS	200
+#define REGION_WORKER_DETECT_CYCLE_MS 200
 
-//万集区域功能的状态
+	//万集区域功能的状态
 	enum Region_worker_status
 	{
-		E_UNKNOWN              	= 0,    //未知
-		E_READY               	= 1,    //准备，待机
-		E_BUSY					= 2, 	//工作正忙
+		E_UNKNOWN = 0, //未知
+		E_READY = 1,   //准备，待机
+		E_BUSY = 2,	   //工作正忙
 
-		E_FAULT					= 10,	//故障
+		E_FAULT = 10, //故障
 	};
-public:
 
+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 init(Point2D_tool::Point2D_box point2D_box, std::mutex* p_cloud_collection_mutex,
+					   pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_collection, int index);
 
 	//开始自动预测的算法线程
 	Error_manager start_auto_detect();
@@ -79,7 +80,6 @@ public:
 	Error_manager get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
 											 std::chrono::system_clock::time_point command_time);
 
-
 public:
 	Region_worker_status get_status();
 
@@ -92,6 +92,9 @@ protected:
 	void auto_detect_thread_fun();
 
 protected:
+
+	int m_index;// 区域编号,仅普爱使用
+
 	//状态
 	Region_worker_status						m_region_worker_status;			//万集区域功能的状态
 

wanji_lidar/wanji_manager.cpp

@@ -74,7 +74,8 @@ Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParam
 		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);
+		// changed by yct, 传入终端index,仅普爱使用
+		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection, i);
 		if ( t_error != Error_code::SUCCESS )
 		{
 			delete(p_region_worker);