CT_project/测量节点/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>
#include "../message/message_base.pb.h"

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


//车位表
#define PARKSPACE_ID_BASE							0
#define PASSAGEWAY_ID_BASE							1100

//唯一码的默认长度 32byte
#define COMMAND_KEY_DEFAULT_LENGTH					32



	
	//整车的测量信息
	struct Car_measure_information
	{
		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
		float car_angle = 0;			//整车的车身旋转角,
		float car_length = 0;			//整车的长度, 用于规避碰撞
		float car_width = 0;			//整车的宽度, 用于规避碰撞
		float car_height = 0;			//整车的高度, 用于规避碰撞
		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
		float car_front_theta = 0;			//整车的前轮的旋转角

		bool correctness = false;			//整车的校准标记位

	};
	
	
	//四轮的测量信息
	struct Car_wheel_information
	{
		float car_center_x = 0;				//整车的中心点x值, 四轮的中心
		float car_center_y = 0;				//整车的中心点y值, 四轮的中心
		float car_angle = 0;			//整车的车身旋转角,

		float car_wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
		float car_wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
		float car_front_theta = 0;			//整车的前轮的旋转角

		bool correctness = false;			//整车的校准标记位

		float uniform_car_x = 0;
		float uniform_car_y = 0;

		float single_wheel_width = 0;		// 单轮宽
		float single_wheel_length = 0;		// 单轮长

		int range_status = 0;
        int terminal_range_status = 0;

	public:
		Car_wheel_information& operator+=(const Car_wheel_information& other)
		{
			this->car_center_x += other.car_center_x;
			this->car_center_y += other.car_center_y;
			this->car_angle += other.car_angle;
			this->car_wheel_base += other.car_wheel_base;
			this->car_wheel_width += other.car_wheel_width;
			this->car_front_theta += other.car_front_theta;
			this->single_wheel_width += other.single_wheel_width;
			this->single_wheel_length += other.single_wheel_length;

			this->correctness &= other.correctness;
			return *this;
		}
		Car_wheel_information& operator-=(const Car_wheel_information& other)
		{
			this->car_center_x -= other.car_center_x;
			this->car_center_y -= other.car_center_y;
			this->car_angle -= other.car_angle;
			this->car_wheel_base -= other.car_wheel_base;
			this->car_wheel_width -= other.car_wheel_width;
			this->car_front_theta -= other.car_front_theta;
			this->single_wheel_width -= other.single_wheel_width;
			this->single_wheel_length -= other.single_wheel_length;

			this->correctness &= other.correctness;
			return *this;
		}
		Car_wheel_information& operator/=(int scalar)
		{
			if(scalar==0)
				return *this;
			this->car_center_x /= scalar;
			this->car_center_y /= scalar;
			this->car_angle /= scalar;
			this->car_wheel_base /= scalar;
			this->car_wheel_width /= scalar;
			this->car_front_theta /= scalar;
			this->single_wheel_width /= scalar;
			this->single_wheel_length /= scalar;
			return *this;
		}
		Car_wheel_information& operator=(const Car_wheel_information& other)
		{
			this->car_center_x = other.car_center_x;
			this->car_center_y = other.car_center_y;
			this->car_angle = other.car_angle;
			this->car_wheel_base = other.car_wheel_base;
			this->car_wheel_width = other.car_wheel_width;
			this->car_front_theta = other.car_front_theta;
			this->single_wheel_width = other.single_wheel_width;
			this->single_wheel_length = other.single_wheel_length;

			this->correctness = other.correctness;
			this->uniform_car_x = other.uniform_car_x;
			this->uniform_car_y = other.uniform_car_y;
			this->range_status = other.range_status;
            this->terminal_range_status = other.terminal_range_status;
			return *this;
		}
		// 定义评分规则, 
		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
		float calc_score()
		{
			float weights[] = {0.5f, 0.5f, 0.25f, 1.0f, 0.25f, 0.1f};
			float final_score = 0.0f;
			final_score += fabs(weights[0] * this->car_center_x);
			final_score += fabs(weights[1] * this->car_center_y);
			final_score += fabs(weights[2] * this->car_angle);
			final_score += fabs(weights[3] * this->car_wheel_base);
			final_score += fabs(weights[4] * this->car_wheel_width);
			final_score += fabs(weights[5] * this->car_front_theta);

			return final_score;
		}

		std::string to_string()
        {
            char buf[512]={0};
            sprintf(buf, "cx:%.4f cy:%.4f ang:%.4f wb:%.4f w:%.4f fr:%.4f sww:%.3f swl:%.3f", car_center_x, car_center_y, car_angle, car_wheel_base, car_wheel_width, car_front_theta, single_wheel_width, single_wheel_length);
            return std::string(buf);
        }

		// 旋转正位(theta至90度)后车辆中心
		void theta_uniform(float turnplate_cx, float turnplate_cy, float &x, float &y)
		{
			float d_theta = (90.0f - car_angle) * M_PI / 180.0f;
			float car_x = car_center_x - turnplate_cx;
			float car_y = car_center_y - turnplate_cy;
			x = cos(d_theta) * car_x - sin(d_theta) * car_y + turnplate_cx;
			y = sin(d_theta) * car_x + cos(d_theta) * car_y + turnplate_cy;
		}

		// 更新旋转正位后自身缓存的车辆中心
		void theta_uniform(float turnplate_cx, float turnplate_cy)
		{
			theta_uniform(turnplate_cx, turnplate_cy, uniform_car_x, uniform_car_y);
		}
	};

	// 带时间戳的四轮测量信息
	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;
		}
		Car_wheel_information_stamped& operator=(const Car_wheel_information_stamped& other)
		{
			this->wheel_data = other.wheel_data;
			this->measure_time = other.measure_time;
			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();
		}
	};



	struct Car_information
	{
		std::string                license;              //车辆凭证号
		float                      car_length=0;           //车长
		float                      car_width=0;            //车宽
		float                      car_height=0;           //车高
		float                      car_wheel_base = 0;	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
		float                      car_wheel_width = 0;	//整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
	};


//车位状态枚举
	enum Parkspace_status
	{
		PARKSPACE_STATUS_UNKNOW		= 0,
		PARKSPACE_EMPTY     		= 1,         //空闲，可分配
		PARKSPACE_OCCUPIED    		= 2,         //被占用，不可分配
		PARKSPACE_RESERVED  		= 3,         //被预约，预约车辆可分配, 暂时不用
		PARKSPACE_LOCKED 			= 4,         //临时锁定，不可分配
		PARKSPACE_ERROR 			= 5,         //车位机械结构或硬件故障
	};

	enum Direction
	{
		DIRECTION_UNKNOW = 0,
		DIRECTION_FORWARD = 1,
		DIRECTION_BACKWARD = 2,
	};

//车位分配路线（根据中跑车的路线来定）
	enum Parkspace_path
	{
		UNKNOW_PATH = 0,
		OPTIMAL_PATH = 1,
		LEFT_PATH = 2,
		RIGHT_PATH = 3,
		TEMPORARY_CACHE_PATH = 4,

	};
	//车位类型
	enum Parkspace_type
	{
		UNKNOW_PARKSPACE_TYPE = 0,
		MIN_PARKINGSPACE = 1,//小车位
		MID_PARKINGSPACE = 2,//中车位
		BIG_PARKINGSPACE = 3,//大车位
	};

	//汽车类型
	enum Car_type
	{
		UNKNOW_CAR_TYPE = 0,
		MIN_CAR = 1,//小车
		MID_CAR = 2,//中车
		BIG_CAR = 3,//大车
	};

	//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
	struct Parkspace_information
	{
		int              	parkingspace_index_id=0;         //车位编号
		Parkspace_type     	parkingspace_type=UNKNOW_PARKSPACE_TYPE;               //车位类型

		int              	parkingspace_unit_id=0;            //区块编号
		int              	parkingspace_floor_id=0;                //楼层
		int              	parkingspace_room_id=0;                //同层编号
		Direction    		parkingspace_direction=DIRECTION_UNKNOW;            //前后

		float              	parkingspace_width=0;                //车位宽
		float              	parkingspace_height=0;               //车位高
		Parkspace_status   	parkingspace_status=PARKSPACE_EMPTY;     //车位当前状态

		Car_information    	car_information;              //当前车位存入车辆的凭证号
		std::string        	car_entry_time;          //入场时间
		std::string        	car_leave_time;          //离场时间

		Parkspace_path     	parkspace_path = UNKNOW_PATH;            // 车位分配路线
		float              	path_estimate_time = 0;        //车位分配路线 time(s)
		Parkspace_status   	parkspace_status_target=PARKSPACE_EMPTY;     //车位目标状态
	};

	static void copy_data(Car_measure_information& car_measure_information_out, const message::Locate_information& locate_information_in);
	static void copy_data(message::Locate_information& locate_information_out, const Car_measure_information& car_measure_information_in);
	static void copy_data(Car_information& car_information_out, const message::Car_info& car_info_in);
	static void copy_data(message::Car_info& car_info_out, const Car_information& car_information_in);
	static void copy_data(Parkspace_information& parkspace_information_out, const message::Parkspace_info& parkspace_info_in);
	static void copy_data(message::Parkspace_info& parkspace_info_out, const Parkspace_information& parkspace_information_in);


	static void transform_data(Car_information& car_information_out, const Car_measure_information& car_measure_information_in);
	static void transform_data(Car_measure_information& car_measure_information_out, const Car_information& car_informatio_in);
	static void transform_data(Car_wheel_information& car_wheel_information_out, const Car_measure_information& car_measure_information_in);
	static void transform_data(Car_measure_information& car_measure_information_out, const Car_wheel_information& car_wheel_information_in);

	static void scaling(Car_measure_information& car_measure_information, float rate);
	static void scaling(Car_information& car_information, float rate);
	static void scaling(Parkspace_information& parkspace_information, float rate);


	static bool approximate_rate(float a, float b, float rate);
	static bool approximate_difference(float a, float b, float difference);









	//调度模块
	//调度流程类型
	enum Dispatch_process_type
	{
		DISPATCH_PROCESS_TYPE_UNKNOW 			= 0,    //未知
		DISPATCH_PROCESS_STORE  				= 101,    //存车
		DISPATCH_PROCESS_PICKUP					= 102,	//取车
	};


	//调度动作方向   0=未知，1=存车，2=取车
	enum Dispatch_motion_direction
	{
		DISPATCH_MOTION_DIRECTION_UNKNOWN					= 0,
		DISPATCH_MOTION_DIRECTION_STORE              		= 1,    //
		DISPATCH_MOTION_DIRECTION_PICKUP					= 2,    //
	};

	//出入口方向   0=未知，1=入口，2=出口
	enum Passageway_direction
	{
		PASSAGEWAY_DIRECTION_UNKNOWN             			= 0,    //
		PASSAGEWAY_DIRECTION_INLET               			= 1,    //
		PASSAGEWAY_DIRECTION_OUTLET							= 2,    //
	};


	//楼上车位方向   0=未知，1=朝南，2=朝北
	enum Parkingspace_direction
	{
		PARKINGSPACE_DIRECTION_UNKNOWN					= 0,    //
		PARKINGSPACE_DIRECTION_SOUTH               		= 1,    //
		PARKINGSPACE_DIRECTION_NORTH               		= 2,    //
	};

	//防撞雷达标志位  0=未知，1=位置正常，2=位置异常
	enum Anticollision_lidar_flag
	{
		ANTICOLLISION_LIDAR_UNKNOWN               	= 0,    //
		ANTICOLLISION_LIDAR_NORMAL               	= 1,    //
		ANTICOLLISION_LIDAR_ERROR               	= 2,    //
	};
};


#endif //NNXX_TESTS_COMMON_DATA_H