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"

Region_worker::Region_worker()
{
	m_region_worker_status = E_UNKNOWN;

	mp_cloud_collection_mutex = NULL;
	mp_cloud_collection = NULL;

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

	mp_auto_detect_thread = NULL;


}

/**
 * 析构
 * */
Region_worker::~Region_worker()
{

}

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)
{
	m_detector.init(point2D_box);

	mp_cloud_collection_mutex = p_cloud_collection_mutex;
	mp_cloud_collection = 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)
{
	return m_detector.detect(p_cloud_mutex, p_cloud_in, car_wheel_information);
}


// 获得中心点、角度等测量数据,  新算法, 可以测量前轮旋转
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 m_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_updata_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_updata_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_updata_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 ");
	}
}

Region_worker::Region_worker_status Region_worker::get_status()
{
	return m_region_worker_status;
}

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



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);
}


//自动预测的线程函数
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())
	{
		//暂时固定为一个扫描周期, 就循环一次
		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
//        if(m_index==5)
//            LOG(INFO) << "before wait";
		m_auto_detect_condition.wait();
//        if(m_index==5)
//            LOG(INFO) << "after wait";
		if ( m_auto_detect_condition.is_alive() )
		{
//			std::cout << " huli test :::: " << "  = " << "---------------------------------------------------------------------------------" << std::endl;
//            if(m_index==5)
//			    LOG(INFO) << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size();
//
//			auto start   = std::chrono::system_clock::now();

			t_error = detect_wheel_result_ex(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;
//                if(m_index==5) {
//                    std::cout << " huli test :::: " << " x = " << m_car_wheel_information.center_x << std::endl;
//                    std::cout << " huli test :::: " << " y = " << m_car_wheel_information.center_y << std::endl;
//                    std::cout << " huli test :::: " << " c = " << m_car_wheel_information.car_angle << std::endl;
//                    std::cout << " huli test :::: " << " wheelbase = " << m_car_wheel_information.wheel_base
//                              << std::endl;
//                    std::cout << " huli test :::: " << " width = " << m_car_wheel_information.wheel_width << std::endl;
//                    std::cout << " huli test :::: " << " front_theta = " << m_car_wheel_information.front_theta
//                              << std::endl;
//                    std::cout << " huli test :::: " << " correctness = " << m_car_wheel_information.correctness
//                              << std::endl;
//                }
			}
			else
			{
				std::unique_lock<std::mutex> t_lock(*mp_cloud_collection_mutex);
				m_car_wheel_information.correctness = false;
//				if(m_index==5)
//                    LOG(WARNING)<< " t_error = " << t_error;
			}

			//无论结果如何,都要刷新时间, 表示这次定位已经执行了.
			m_detect_updata_time = std::chrono::system_clock::now();
//            if(m_index==5)
//                LOG(INFO) << "before filter";
			// 测量正确,更新到滤波器
			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);
			}
//            if(m_index==5)
//                LOG(INFO) << "after filter";
		}
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}