puai_wj_2021/wanji_lidar/region_worker.cpp

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

Region_worker::Region_worker()
{
	m_region_worker_status = E_UNKNOW;

	mp_cloud_collection_mutex = NULL;
	mp_cloud_collection = NULL;
	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::wake_auto_detect()
{
	//唤醒一次
	m_auto_detect_condition.notify_all(false, true);
	return Error_code::SUCCESS;
}

// 获得中心点、角度等测量数据
Error_manager Region_worker::get_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::get_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);
}





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

			t_error = get_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::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;
				m_detect_updata_time = std::chrono::system_clock::now();
			}
			else
			{
//				std::cout << " huli test :::: " << " t_error = " << t_error << std::endl;
			}


//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;

			//huli
		}
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}