Measure/Modules/MeasureNode/wanji_lidar/wanji_716N_device.cpp

#include "wanji_716N_device.h"



// 万集雷达封装类构造函数
Wanji_716N_lidar_device::Wanji_716N_lidar_device()
{
	m_wanji_lidar_device_status = E_UNKNOWN;
}

// 万集雷达封装类析构函数
Wanji_716N_lidar_device::~Wanji_716N_lidar_device()
{
	uninit();
}

// 初始化
Error_manager Wanji_716N_lidar_device::init(wj::wjLidarParams params)
{
	Error_manager t_error;
	LOG(INFO) << " Wanji_716N_lidar_device::init "<< this;

	//唤醒队列
	m_communication_data_queue.wake_queue();

	m_lidar_id = params.lidar_id();

	//控制参数
	Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;	//极坐标的限定范围
	Point2D_tool::Point2D_box			t_point2D_box;				//平面坐标的限定范围
	Point2D_tool::Point2D_transform 	t_point2D_transform;		//平面坐标的转换矩阵

	t_polar_coordinates_box.angle_min = params.angle_min();
	t_polar_coordinates_box.angle_max = params.angle_max();
	t_polar_coordinates_box.distance_min = params.range_min();
	t_polar_coordinates_box.distance_max = params.range_max();

	t_point2D_box.x_min = params.scan_limit().minx();
	t_point2D_box.x_max = params.scan_limit().maxx();
	t_point2D_box.y_min = params.scan_limit().miny();
	t_point2D_box.y_max = params.scan_limit().maxy();

	t_point2D_transform.m00 = params.transform().m00();
	t_point2D_transform.m01 = params.transform().m01();
	t_point2D_transform.m02 = params.transform().m02();
	t_point2D_transform.m10 = params.transform().m10();
	t_point2D_transform.m11 = params.transform().m11();
	t_point2D_transform.m12 = params.transform().m12();

	//初始化通信协议
	t_error = m_protocol.init(&m_communication_data_queue, t_polar_coordinates_box, t_point2D_box, t_point2D_transform);
	if (t_error != SUCCESS)
	{
		return t_error;
	}

	//初始化雷达通信模块
	std::string	t_ip = params.net_config().ip_address();
	int t_port = params.net_config().port();

	t_error = m_async_client.init(t_ip, t_port, &m_communication_data_queue);
	if ( t_error != SUCCESS )
	{
		return t_error;
	}


	//启动 内部线程。默认wait。
	m_execute_condition.reset(false, false, false);
	// mp_execute_thread = new std::thread(&Wanji_716N_lidar_device::execute_thread_fun, this);

	m_wanji_lidar_device_status = E_READY;

	return t_error;
}

//反初始化
Error_manager Wanji_716N_lidar_device::uninit()
{
//终止队列，防止 wait_and_pop 阻塞线程。
	m_communication_data_queue.termination_queue();

	//关闭线程
	if (mp_execute_thread)
	{
		m_execute_condition.kill_all();
	}
	//回收线程的资源
	if (mp_execute_thread)
	{
		mp_execute_thread->join();
		delete mp_execute_thread;
		mp_execute_thread = NULL;
	}

	m_async_client.uninit();
	m_protocol.uninit();

//清空队列
	m_communication_data_queue.clear_and_delete();

	if ( m_wanji_lidar_device_status != E_FAULT  )
	{
		m_wanji_lidar_device_status = E_UNKNOWN;
	}
	return Error_code::SUCCESS;
}

//检查雷达状态,是否正常运行
Error_manager Wanji_716N_lidar_device::check_status()
{
	updata_status();
	if ( m_wanji_lidar_device_status == E_READY )
	{
		return Error_code::SUCCESS;
	}
	else if(m_wanji_lidar_device_status == E_BUSY)
	{
	    return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
	    					"  Wanji_716N_lidar_device::check_status() error ");
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Wanji_716N_lidar_device::check_status() error ");
	}
	return Error_code::SUCCESS;
}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Wanji_716N_lidar_device::is_ready()
{
	updata_status();
	return m_wanji_lidar_device_status == E_READY;
}

//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
Error_manager Wanji_716N_lidar_device::get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
												   std::chrono::system_clock::time_point command_time, int timeout_ms)
{
	if ( p_mutex == NULL || p_cloud.get() == 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_protocol.get_scan_time() > command_time  )
		{
			std::unique_lock<std::mutex> lck(*p_mutex);
			Error_manager code = m_protocol.get_scan_points(p_cloud);
			return code;
		}
		//else 等待下一次数据更新

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

//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
Error_manager Wanji_716N_lidar_device::get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::system_clock::time_point command_time)
{
	if ( p_mutex == NULL || p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
	if( m_protocol.get_scan_time() > command_time )
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		Error_manager code = m_protocol.get_scan_points(p_cloud);
		return code;
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_716N_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::system_clock::time_point command_time)
{
	if ( p_mutex == NULL || p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		Error_manager code = m_protocol.get_scan_points(p_cloud);
		return code;
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_716N_lidar_device::get_last_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
//注:函数内部不加锁, 由外部统一加锁.
Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
												 std::chrono::system_clock::time_point command_time)
{
	if ( p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms*2) )
	{
		Error_manager code = m_protocol.get_scan_points(p_cloud);
		// LOG(WARNING) << " !!!!!!!!!!!!! wj scan cloud: "<<p_cloud->size();
		return code;
	}
	else
	{
    //    LOG(WARNING) << "get cloud failed..... "<<m_protocol.get_scan_time().time_since_epoch().count()<<", "<<command_time.time_since_epoch().count();
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_716N_lidar_device::get_last_cloud error ");
	}
}

Wanji_716N_lidar_device::Wanji_716N_device_status Wanji_716N_lidar_device::get_status()
{
	updata_status();
	return m_wanji_lidar_device_status;
}

void Wanji_716N_lidar_device::updata_status()
{
	Async_Client::Communication_status communication_status = m_async_client.get_status();
	// Wj_716N_lidar_protocol::Wanji_716N_device_status wanji_lidar_protocol_status = m_protocol.get_status();

	if ( communication_status == Async_Client::Communication_status::E_FAULT)// ||
		//  wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_status::E_FAULT)
	{
		m_wanji_lidar_device_status = E_FAULT;
	}
	if ( communication_status == Async_Client::Communication_status::E_DISCONNECT )
	{
		m_wanji_lidar_device_status = E_DISCONNECT;
	}
	else if ( communication_status == Async_Client::Communication_status::E_READY)// &&
			//   wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_status::E_READY )
	{
		if ( m_execute_condition.get_pass_ever() )
		{
			m_wanji_lidar_device_status  = E_BUSY;
		}
		else
		{
			m_wanji_lidar_device_status = E_READY;
		}
	}
	else
	{
		m_wanji_lidar_device_status = E_UNKNOWN;
	}
}