LidarMeasure/laser/Sick511FileLaser.cpp

#include "Sick511FileLaser.h"
#include <unistd.h>

RegisterLaser(Sick511File);
CSick511FileLaser::CSick511FileLaser(int id, Laser_proto::laser_parameter laser_param)
:Laser_base(id, laser_param)
,m_start_read(false)
{
}

CSick511FileLaser::~CSick511FileLaser()
{
	if (m_stream_read.is_open())
		m_stream_read.close();
}



//雷达链接设备，为3个线程添加线程执行函数。
Error_manager CSick511FileLaser::connect_laser()
{
	std::string ip = m_laser_param.laser_ip();
	char file[255] = { 0 };
	sprintf(file, "%s/laser%d.data", ip.c_str(), m_laser_id + 1);
	if (m_stream_read.is_open())
	{
		m_stream_read.close();
	}
	m_stream_read.open(file, ios::in | ios::binary);
	if (!m_stream_read.good())
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							 "m_stream_read.open error ");
	m_laser_statu = LASER_READY;
	m_file = file;
	return Laser_base::connect_laser();
}
//雷达断开链接，释放3个线程
Error_manager CSick511FileLaser::disconnect_laser()
{
	if(m_stream_read.is_open())
		m_stream_read.close();
	return Laser_base::disconnect_laser();
}
//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
Error_manager CSick511FileLaser::execute_task(Task_Base* p_laser_task)
{

}
//检查雷达状态,是否正常运行
Error_manager CSick511FileLaser::check_laser()
{

}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager CSick511FileLaser::start_scan()
{
	std::lock_guard<std::mutex> lk(m_mutex);

	if (!this->is_ready())
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							" is_ready error ");

	m_start_read = true;
	return Laser_base::start_scan();

}
//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
Error_manager CSick511FileLaser::stop_scan()
{
	std::lock_guard<std::mutex> lk(m_mutex);
	m_start_read = false;
	return Error_code::SUCCESS;
}
//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
Error_manager CSick511FileLaser::end_task()
{

}



Buf_type CSick511FileLaser::transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points)
{
	////ƴ���ϴ�δ�������

	Buf_type type = BUF_UNKNOW;
	Binary_buf frame;

	if (m_last_data.get_length() > 0)
	{
		if (pData)
			frame = m_last_data + (*pData);
		else
			frame = m_last_data;
		m_last_data = Binary_buf();
	}
	else if(pData)
	{
		frame = (*pData);
	}
	else
	{
		return type;
	}

	int head = FindHead(frame.get_buf(), frame.get_length());
	int tail = FindTail(frame.get_buf(), frame.get_length());

	if (tail >= 0)
	{
		if (tail < frame.get_length() - 1)		//���������
		{
			m_last_data = Binary_buf(frame.get_buf() + tail + 1, frame.get_length() - tail - 1);
		}
		if (head >= 0 && head < tail)
		{
			////���
			Binary_buf data(frame.get_buf() + head, tail - head + 1);
			if (data.is_equal_front( "$SLSSTP"))
				type = BUF_STOP;
			else if (data.is_equal_front( "$SLSSTA"))
				type = BUF_START;
			else if (data.is_equal_front( "$SCLRDY"))
				type = BUF_READY;
			else if (data.is_equal_front( "$SHWERR"))
				type = BUF_ERROR;
			else if (data.is_equal_front( "$N"))
				type = BUF_DATA;

			if (type == BUF_DATA)
			{
				////��������
				points.clear();
				if (data.get_length() <= 29)
					return type;

				////����
				unsigned char angle_1 = 0;
				unsigned char angle_2 = 0;
				memcpy(&angle_1, (data.get_buf())+26, 1);
				memcpy(&angle_2, (data.get_buf())+27, 1);
				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
				float start_angle = 0.0;
				float freq = 0.0;

				std::vector<float> distance;
				if (GetData(&data, distance, freq, start_angle))
				{
					float beta = (beta_a)*DEGREES;
					float sin_beta = sin(beta);
					float cos_beta = cos(beta);
					for (int i = 0; i < distance.size(); ++i)
					{
						if (distance[i] < 0.001)
							continue;

						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
						float sin_alpha = sin(alpha);
						float cos_alpha = cos(alpha);

						float x = distance[i] * sin_alpha;
						float y = distance[i] * cos_alpha * sin_beta;
						float z = distance[i] * cos_alpha * cos_beta;

						points.push_back(CPoint3D(x, y, z));
					}
					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
				}

			}
			else if (type == BUF_STOP)
			{
				m_laser_statu = LASER_READY;
			}
		}
	}
	else if (head >= 0)
	{
		m_last_data = Binary_buf(frame.get_buf() + head, frame.get_length() - head);
	}

	return type;
}

bool CSick511FileLaser::receive_buf_to_queue(Binary_buf& data)
{
	if (m_start_read == false)
		return false;
	if (!m_stream_read.is_open())
		return false;

	if (m_stream_read.eof())
	{
		stop_scan();
		m_stream_read.close();
		usleep(100*1000);
		m_stream_read.open(m_file.c_str(), ios::in | ios::binary);
	}

	char buf[512] = { 0 };
	m_stream_read.read(buf, 512);
	int count = m_stream_read.gcount();
	if (count > 0)
	{
		Binary_buf bin_data(buf, count);
		data = bin_data;
		return true;
	}
	return false;
}

int CSick511FileLaser::FindHead(char* buf, int b_len)
{
	int i = 0;
	if (b_len < 2)
		return 0;
	for (i = 0; i <= b_len; i++)
	{
		if (b_len > 10)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
				return i;
		}
		if (b_len > 7)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
				return i;
		}
	}
	return -1;
}

int CSick511FileLaser::FindTail(char* buf, int b_len)
{
	int i = 0;

	for (i = 0; i <= b_len; i++)
	{
		if (b_len >= i + 5)
		{
			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
				return i + 4;
		}
	}
	return -9999999;
}

bool CSick511FileLaser::GetData(Binary_buf* pData, std::vector<float>& distance,
								float& freq, float& start_angle)
{
	struct stData
	{
		const char* data;
		int length;
	};
	std::vector<struct stData> strDatas;
	int start = 0;
	int end = 0;
	int LMDscandata_index = -1;
	for (int i = 0; i < pData->get_length(); ++i)
	{
		if ((*pData)[i] == ' ')
		{
			end = i;
			if (end > start)
			{
				struct stData strData;
//				strData.data = (*pData + start);
				strData.data = ( pData->get_buf() + start);
				strData.length = end - start;

				strDatas.push_back(strData);
				if (strncmp(strData.data, "LMDscandata", 11) == 0)
					LMDscandata_index = strDatas.size() - 1;
			}
			end = i + 1;
			start = end;
		}
	}

	if (strDatas.size() > 26 + LMDscandata_index - 1)
	{
		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
		start_angle = float(start_angle_l)*0.01;

		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
		freq = float(freq_l)*0.0001;

		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
		long count = Str0x2Long(count_str.data, count_str.length);
		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
		{
			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
			{
				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
				distance.push_back(dis);
			}
			return true;
		}
	}
	return false;
}
long CSick511FileLaser::Str0x2Long(const char* data, int len)
{
	long sum = 0;
	for (int i = 0; i < len; ++i)
	{
		char c = data[i];
		int n = 0;
		if (c >= 48 && c <= 57)
			n = c - 48;
		else if (c >= 65 && c <= 70)
			n = c - 65 + 10;
		sum += n*pow(16, len - i - 1);
	}
	return sum;
}