puai_wj_2021/tool/common_data.h

//
// Created by huli on 2020/9/8.
//

#ifndef NNXX_TESTS_COMMON_DATA_H
#define NNXX_TESTS_COMMON_DATA_H
#include <chrono>
#include <cmath>
#include <string>
#include <string.h>

class Common_data
{
public:
	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
#define WANJI_716_SCAN_CYCLE_MS 		75
	//vlp16雷达扫描周期100ms, (频率10hz), 一般设置大一些
#define VLP16_SCAN_CYCLE_MS 		110

	//整车的测量信息
	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;			//整车的校准标记位
		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, 0.1f, 0.05f, 0.05f};
			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;
		}

		std::string to_string()
        {
            char buf[512]={0};
            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", center_x, center_y, car_angle, wheel_base, wheel_width, front_theta);
            return std::string(buf);
        }
	};

	// 带时间戳的四轮测量信息
	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)
		{
            Car_wheel_information_stamped t_info;
            t_info = *this;
            t_info.wheel_data -= other.wheel_data;
			return t_info;
		}
		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);



};


#endif //NNXX_TESTS_COMMON_DATA_H