puai_wj_2021/wanji_lidar/region_worker.cpp

//
// Created by zx on 2019/12/9.
//
#include "region_worker.h"
//#include "plc_data.h"

// 测量结果滤波,不影响现有结构
#include "../tool/measure_filter.h"

// 增加车辆停止状态判断
#include "../tool/region_status_checker.h"

Region_worker::Region_worker()
{
	m_region_worker_status = E_UNKNOWN;

	mp_cloud_collection_mutex = NULL;
	mp_cloud_collection = NULL;

	m_detect_update_time = std::chrono::system_clock::now() - std::chrono::hours(1);

	mp_auto_detect_thread = NULL;


}

/**
 * 析构
 * */
Region_worker::~Region_worker()
{
    if (mp_auto_detect_thread)
    {
        m_auto_detect_condition.kill_all();
        // Close Capturte Thread
        if (mp_auto_detect_thread->joinable())
        {
            mp_auto_detect_thread->join();
            delete mp_auto_detect_thread;
            mp_auto_detect_thread = nullptr;
        }
    }

    // 将创建的检测器析构
    if(mp_detector)
    {
        delete mp_detector;
        mp_detector = nullptr;
    }
    
    if(mp_cloud_collection_mutex)
    {
        delete mp_cloud_collection_mutex;
        mp_cloud_collection_mutex = nullptr;
    }

}

Error_manager Region_worker::init(wj::Region region)
{
	m_region_param = region;
	mp_detector = new Region_detector(m_region_param);

	mp_cloud_collection_mutex = new std::mutex();
    mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>); // = p_cloud_collection;

    m_auto_detect_condition.reset(false, false, false);
	mp_auto_detect_thread = new std::thread(&Region_worker::auto_detect_thread_fun, this);

	m_region_worker_status = E_READY;

	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()
{
	//唤醒一次
	m_auto_detect_condition.notify_all(false, true);
	return Error_code::SUCCESS;
}
//关闭自动预测的算法线程
Error_manager Region_worker::stop_auto_detect()
{
	//唤醒一次
	m_auto_detect_condition.notify_all(false);
	return Error_code::SUCCESS;
}

// 获得中心点、角度等测量数据
Error_manager Region_worker::detect_wheel_result(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information)
{
    std::lock_guard<std::mutex> lck(*p_cloud_mutex);
	double x, y, c, wheelbase, width, front_theta;
	auto res = mp_detector->detect(p_cloud_in, x, y, c, front_theta, wheelbase, width, false);
	car_wheel_information.car_center_x = x;
	car_wheel_information.car_center_y = y;
	car_wheel_information.car_angle = c;
	car_wheel_information.car_wheel_base = wheelbase;
	car_wheel_information.car_wheel_width = width;
	car_wheel_information.car_front_theta = front_theta;
	if(res == SUCCESS)
	{
		car_wheel_information.correctness = true;
	}else
	{
		car_wheel_information.correctness = false;
	}
	return res;
}

// 获得中心点、角度等测量数据,  新算法, 可以测量前轮旋转
Error_manager Region_worker::detect_wheel_result_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, Common_data::Car_wheel_information& car_wheel_information)
{
	return ERROR; //mp_detector->detect_ex(p_cloud_mutex, p_cloud_in, car_wheel_information);
}


//外部调用获取新的车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会等待车轮定位信息刷新, 直到超时, (内置while)
Error_manager Region_worker::get_new_wheel_information_and_wait(Common_data::Car_wheel_information* p_car_wheel_information,
												 std::chrono::system_clock::time_point command_time, int timeout_ms)
{
	if ( p_car_wheel_information == 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_detect_update_time > command_time  )
		{
			*p_car_wheel_information = m_car_wheel_information;
			return Error_code::SUCCESS;
		}
		//else 等待下一次数据更新

		//等1ms
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}
	//超时退出就报错
	return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
						 " Region_worker::get_new_wheel_information_and_wait error ");
}

//外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Region_worker::get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
											std::chrono::system_clock::time_point command_time)
{
	if ( p_car_wheel_information == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					"  POINTER IS NULL ");
	}
	//获取指令时间之后的信息, 如果没有就会报错, 不会等待
	if( m_detect_update_time > command_time )
	{
		*p_car_wheel_information = m_car_wheel_information;
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
							 " Region_worker::get_current_wheel_information error ");
	}
}
//外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Region_worker::get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
										 std::chrono::system_clock::time_point command_time)
{
	if ( p_car_wheel_information == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
	if( m_detect_update_time > command_time - std::chrono::milliseconds(REGION_WORKER_DETECT_CYCLE_MS*2) )
	{
		*p_car_wheel_information = m_car_wheel_information;
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::WJ_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
							 " Region_worker::get_last_wheel_information error ");
	}
}

std::chrono::system_clock::time_point Region_worker::get_detect_updata_time()
{
	return m_detect_update_time;
}

Error_manager Region_worker::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // // 点云z转90度，调试用
    //Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
    //for (size_t i = 0; i < cloud->size(); i++)
    //{
    //    Eigen::Vector3d t_point(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
    //    t_point = rot_z.toRotationMatrix() * t_point;
    //    cloud->points[i].x = t_point.x();
    //    cloud->points[i].y = t_point.y();
    //    cloud->points[i].z = t_point.z();
    //}
    // 限定锁区域，解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住，造成死锁无法结束程序的问题。
    {
        std::lock_guard<std::mutex> lck(*mp_cloud_collection_mutex);
        mp_cloud_collection = cloud;
        // LOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: " << cloud->size();
        m_detect_update_time = std::chrono::system_clock::now();
    }
    m_auto_detect_condition.notify_one(false, true);

    // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    // std::chrono::duration<double> time_used_update = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0);
    // std::cout << "update cloud time: " << time_used_update.count() << std::endl;
    return SUCCESS;
}


cv::RotatedRect Region_worker::create_rotate_rect(float length,float width,float angle,float cx,float cy)
{
    const double C_PI=3.14159265;
    float theta=C_PI*(angle/180.0);
    float a00=cos(theta);
    float a01=-sin(theta);
    float a10=sin(theta);
    float a11=cos(theta);
    cv::Point2f point[4];
    point[0].x=-length/2.0;
    point[0].y=-width/2.0;

    point[1].x=-length/2.0;
    point[1].y=width/2.0;

    point[2].x=length/2.0;
    point[2].y=-width/2.0;

    point[3].x=length/2.0;
    point[3].y=width/2.0;

    std::vector<cv::Point2f> point_vec;
    for(int i=0;i<4;++i)
    {
        float x=point[i].x*a00+point[i].y*a01+cx;
        float y=point[i].x*a10+point[i].y*a11+cy;
        point_vec.push_back(cv::Point2f(x,y));
    }
    return cv::minAreaRect(point_vec);
}


int Region_worker::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx, double plate_cy, double theta)
{
    if(cloud->size() <= 0)
        return Range_status_wj::Range_correct;
    int res = Range_status_wj::Range_correct;
    // 计算转盘旋转后点云坐标
    double theta_rad = theta * M_PI / 180.0;
    pcl::PointCloud<pcl::PointXYZ> t_cloud;
    for (size_t i = 0; i < cloud->size(); i++)
    {
        Eigen::Vector2d t_point(cloud->points[i].x - plate_cx, cloud->points[i].y - plate_cy);
        pcl::PointXYZ t_pcl_point;
        t_pcl_point.x = (t_point.x() * cos(theta_rad) - t_point.y() * sin(theta_rad)) + plate_cx;
        t_pcl_point.y = (t_point.x() * sin(theta_rad) + t_point.y() * cos(theta_rad)) + plate_cy;
        t_pcl_point.z = cloud->points[i].z;
        t_cloud.push_back(t_pcl_point);
    }
    pcl::PointXYZ min_p, max_p;
    pcl::getMinMax3D(t_cloud, min_p, max_p);
    // 判断左右超界情况
    if(min_p.x < m_region_param.border_minx())
        res |= Range_status_wj::Range_left;
    if(max_p.x > m_region_param.border_maxx())
        res |= Range_status_wj::Range_right;

    // LOG_IF(WARNING, m_region_param.region_id() == 4)<< "border minmax x: "<< min_p.x<<", "<<max_p.x<<", res: "<<res;
    return res;
}

int Region_worker::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx, double plate_cy, double theta)
{
    int res = Range_status_wj::Range_correct;
    if(cloud->size() <= 0)
        return res;

    // 计算转盘旋转后车辆中心点坐标
    Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y);
    Eigen::Vector2d car_uniform_center;
    double theta_rad = theta * M_PI / 180.0;
    car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad);
    car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad);
    // 车位位于y负方向，判断后轮超界情况
    double rear_wheel_y = car_uniform_center.y() + m_region_param.plc_offsety() - 0.5 * measure_result.car_wheel_base;
    if(rear_wheel_y < m_region_param.plc_border_miny())
    {
        res |= Range_status_wj::Range_back;
    }
    // 判断车辆宽度超界情况
    if (measure_result.car_wheel_width < m_region_param.car_min_width() || measure_result.car_wheel_width > m_region_param.car_max_width())
    {
        res |= Range_status_wj::Range_car_width;
    }
    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < m_region_param.car_min_wheelbase() || measure_result.car_wheel_base > m_region_param.car_max_wheelbase())
    {
        res |= Range_status_wj::Range_car_wheelbase;
    }
    // 判断车辆旋转角超界情况
    double dtheta = 90-measure_result.car_angle;
    if (dtheta > m_region_param.turnplate_angle_limit_anti_clockwise())
    {
        res |= Range_status_wj::Range_angle_clock;
    }
    if (dtheta < -m_region_param.turnplate_angle_limit_clockwise())
    {
        res |= Range_status_wj::Range_angle_anti_clock;
    }
    // // 判断车辆前轮角回正情况
    // if (fabs(measure_result.car_front_theta) > 8.0)
    // {
    //     res |= Range_status_wj::Range_steering_wheel_nozero;
    // }
    res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
    return res;
}

//自动预测的线程函数
void Region_worker::auto_detect_thread_fun()
{
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;

	while (m_auto_detect_condition.is_alive())
	{
		//暂时固定为一个扫描周期, 就循环一次
		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
		m_auto_detect_condition.wait();
		if ( m_auto_detect_condition.is_alive() )
		{
//			auto start   = std::chrono::system_clock::now();

			t_error = detect_wheel_result(mp_cloud_collection_mutex, mp_cloud_collection, m_car_wheel_information);

//			auto end   = std::chrono::system_clock::now();
//			auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
//			std::cout <<  "花费了"
//				 << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "毫秒" << std::endl;

			// if ( t_error == SUCCESS  )
			// {
			// 	std::unique_lock<std::mutex> t_lock(*mp_cloud_collection_mutex);
			// 	m_car_wheel_information.correctness = true;
			// }
			// else
			// {
			// 	std::unique_lock<std::mutex> t_lock(*mp_cloud_collection_mutex);
			// 	m_car_wheel_information.correctness = false;
			// }

			// //无论结果如何,都要刷新时间, 表示这次定位已经执行了.
			// m_detect_update_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_update_time;
			// 	Measure_filter::get_instance_references().update_data(get_terminal_id(), t_wheel_info_stamped);
			// }

			std::lock_guard<std::mutex> lck(*mp_cloud_collection_mutex);
            m_detect_update_time = std::chrono::system_clock::now();
            // 增加滤波轴距
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
            // 车辆移动检测
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped_for_car_move;
            if (t_error == SUCCESS)
            {
                m_car_wheel_information.correctness = true;

                m_car_wheel_information.theta_uniform(m_region_param.turnplate_cx(), m_region_param.turnplate_cy());
                // 超界校验
                {
                    int res = outOfRangeDetection(mp_cloud_collection, m_car_wheel_information, m_region_param.turnplate_cx(), m_region_param.turnplate_cy(), 90.0 - m_car_wheel_information.car_angle);
                    m_car_wheel_information.range_status = res;
                }
                // 添加plc偏移
                m_car_wheel_information.car_center_x += m_region_param.plc_offsetx();
                m_car_wheel_information.car_center_y += m_region_param.plc_offsety();
                m_car_wheel_information.uniform_car_x += m_region_param.plc_offsetx();
                m_car_wheel_information.uniform_car_y += m_region_param.plc_offsety();
                m_car_wheel_information.car_angle += m_region_param.plc_offset_degree();

                t_wheel_info_stamped.wheel_data = m_car_wheel_information;
                t_wheel_info_stamped.measure_time = m_detect_update_time;
                Measure_filter::get_instance_references().update_data(m_region_param.region_id(), t_wheel_info_stamped);

                // 20211228 added by yct, car movement checking, human and door detection
                t_wheel_info_stamped_for_car_move = t_wheel_info_stamped;
                t_wheel_info_stamped_for_car_move.wheel_data.car_center_x -= m_region_param.plc_offsetx();
                t_wheel_info_stamped_for_car_move.wheel_data.car_center_y -= m_region_param.plc_offsety();
                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_x -= m_region_param.plc_offsetx();
                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_y -= m_region_param.plc_offsety();
                t_wheel_info_stamped_for_car_move.wheel_data.car_angle -= m_region_param.plc_offset_degree();
                // // wj don't test if statble
                // Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
                // // success means car stable
                // if(car_status_res == SUCCESS)
                // {
                //     m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
                // }
                // else{
                //     m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
                //     if(m_region_param.region_id()==4){
                //         LOG(WARNING)<<"success: "<<car_status_res.to_string()<<std::endl;
                //     }
                // }
                // Region_status_checker::get_instance_references().add_measure_data(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);

                t_error = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region_param.region_id(), t_wheel_info_stamped.wheel_data);
                if (t_error == SUCCESS)
                {
                    m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base;
                    m_car_wheel_information.car_front_theta = t_wheel_info_stamped.wheel_data.car_front_theta;
                    // 临时添加，滤波后前轮超界
                    if(fabs(m_car_wheel_information.car_front_theta)>8.0)
                    {
                        m_car_wheel_information.range_status |= Range_status_wj::Range_steering_wheel_nozero;
                    }
                }
                // else{
                //     std::cout<<t_error.to_string()<<std::endl;
                // }
                // LOG_IF(INFO, m_region_param.region_id() == 4 || m_region_param.region_id() == 5) << m_car_wheel_information.to_string();
            }
            else
            {
                m_car_wheel_information.correctness = false;
                // LOG_IF(ERROR, m_region_param.region_id() == 4 || m_region_param.region_id() == 5) <<t_error<<", "<< m_car_wheel_information.to_string();

                // 20211228 added by yct, car movement checking, human and door detection
                // wj don't test if statble
                // Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
                // // success means car stable
                // if(car_status_res == SUCCESS)
                // {
                //     m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
                // }else
                // {
                //     m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
                //     // if(m_region_param.region_id()==4){
                //     //     std::cout<<"failed: "<<car_status_res.to_string()<<std::endl;
                //     // }
                // }
            }
		}
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}