CleanRobot-UESTC/融合/lidar.cpp

/**
 * @file         lipkg.cpp
 * @author       LD Robot
 * @version      V01
 * @brief
 * @note
 * @attention    COPYRIGHT LDROBOT
 **/

#include "lidar.h"
#include <math.h>
#include <algorithm>

// #ifdef USE_SLBI
// #include "slbi.h"
// #endif

// #ifdef USE_SLBF
// #include "slbf.h"
// #endif
//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000)
#define MAX_ACK_BUF_LEN 2304000

#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)

static const uint8_t CrcTable[256] =
	{
		0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
		0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
		0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
		0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
		0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
		0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
		0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
		0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
		0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
		0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
		0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
		0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
		0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
		0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
		0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
		0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
		0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
		0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
		0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
		0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
		0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
		0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8};

CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr),
													mRxCount(0),
													mReadCallback(nullptr),
													version(ver)
{
	mComHandle = -1;
}
CmdInterfaceLinux::~CmdInterfaceLinux()
{
	Close();
}
bool CmdInterfaceLinux::Open(std::string &port_name)
{
	int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);

	mComHandle = open(port_name.c_str(), flags);
	if (-1 == mComHandle)
	{
		std::cout << "CmdInterfaceLinux::Open " << port_name <<  " error !" << std::endl;
		return false;
	}

	// get port options
	struct termios options;
	if (-1 == tcgetattr(mComHandle, &options))
	{
		Close();
		std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl;
		return false;
	}

	options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS);
	options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD);
	options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
									ISIG | IEXTEN); //|ECHOPRT
	options.c_oflag &= (tcflag_t) ~(OPOST);
	options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK);

	options.c_cc[VMIN] = 0;
	options.c_cc[VTIME] = 0;

	switch (version)
	{
	case 0:
		cfsetispeed(&options, B115200);
		break;
	case 3:
		cfsetispeed(&options, B115200);
		break;
	case 6:
		cfsetispeed(&options, B230400);
		break;
	case 8:
		cfsetispeed(&options, B115200);
		break;
	case 14:
		cfsetispeed(&options, B115200);
		break;
	default:
		cfsetispeed(&options, B115200);
	}

	if (tcsetattr(mComHandle, TCSANOW, &options) < 0)
	{
		std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl;
		Close();
		return false;
	}

	tcflush(mComHandle, TCIFLUSH);

	mRxThreadExitFlag = false;
	mRxThread = new std::thread(mRxThreadProc, this);
	mIsCmdOpened = true;

	return true;
}
bool CmdInterfaceLinux::Close()
{
	if (mIsCmdOpened == false)
	{
		return true;
	}

	mRxThreadExitFlag = true;

	if (mComHandle != -1)
	{
		close(mComHandle);
		mComHandle = -1;
	}

	if ((mRxThread != nullptr) && mRxThread->joinable())
	{
		mRxThread->join();
		delete mRxThread;
		mRxThread = NULL;
	}

	mIsCmdOpened = false;

	return true;
}
bool CmdInterfaceLinux::GetCmdDevices(std::vector<std::pair<std::string, std::string>> &device_list)
{
	struct udev *udev;
	struct udev_enumerate *enumerate;
	struct udev_list_entry *devices, *dev_list_entry;
	struct udev_device *dev;

	udev = udev_new();
	if (!udev)
	{
		return false;
	}
	enumerate = udev_enumerate_new(udev);
	udev_enumerate_add_match_subsystem(enumerate, "tty");
	udev_enumerate_scan_devices(enumerate);
	devices = udev_enumerate_get_list_entry(enumerate);
	udev_list_entry_foreach(dev_list_entry, devices)
	{
		const char *path;

		path = udev_list_entry_get_name(dev_list_entry);
		dev = udev_device_new_from_syspath(udev, path);
		std::string dev_path = std::string(udev_device_get_devnode(dev));
		dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device");
		if (dev)
		{
			std::pair<std::string, std::string> p;
			p.first = dev_path;
			p.second = udev_device_get_sysattr_value(dev, "product");
			device_list.push_back(p);
			udev_device_unref(dev);
		}
		else
		{
			continue;
		}
	}
	udev_enumerate_unref(enumerate);
	udev_unref(udev);
	return true;
}
bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len)
{
	static timespec timeout = {0, (long)(100 * 1e6)};
	int32_t len = -1;

	if (IsOpened())
	{
		fd_set read_fds;
		FD_ZERO(&read_fds);
		FD_SET(mComHandle, &read_fds);
		int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL);
		if (r < 0)
		{
			// Select was interrupted
			if (errno == EINTR)
			{
				return false;
			}
		}
		else if (r == 0) // timeout
		{
			return false;
		}

		if (FD_ISSET(mComHandle, &read_fds))
		{
			len = (int32_t)read(mComHandle, rx_buf, rx_buf_len);
			if ((len != -1) && rx_len)
			{
				*rx_len = len;
			}
		}
	}
	return len == -1 ? false : true;
}
bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len)
{
	int32_t len = -1;

	if (IsOpened())
	{
		len = (int32_t)write(mComHandle, tx_buf, tx_buf_len);
		if ((len != -1) && tx_len)
		{
			*tx_len = len;
		}
	}
	return len == -1 ? false : true;
}
void CmdInterfaceLinux::mRxThreadProc(void *param)
{
	CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param;
	char *rx_buf = new char[MAX_ACK_BUF_LEN + 1];
	uchar failedNum = 0;
	while (!cmd_if->mRxThreadExitFlag.load())
	{
		uint32_t readed = 0;
		bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed);
		if (res && readed)
		{
			cmd_if->mRxCount += readed;
			if (cmd_if->mReadCallback != nullptr)
			{
				cmd_if->mReadCallback(rx_buf, readed);
			}
			failedNum = 0;
		}
		else 
		{
			if(++failedNum == 255)
				break;
		}
	}
	cmd_if->mRxThreadExitFlag = true;
	delete[] rx_buf;
}

LiPkg::LiPkg() : mTimestamp(0),
				 mSpeed(0),
				 mErrorTimes(0),
				 mIsFrameReady(false),
				 mIsPkgReady(false)
{
}
double LiPkg::GetSpeed(void)
{
	return mSpeed / 360.0;
}
bool LiPkg::Parse(const uint8_t *data, long len)
{
	for (int i = 0; i < len; i++)
	{
		mDataTmp.push_back(*(data + i));
	}

	if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
		return false;

	if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100)
	{
		mErrorTimes++;
		mDataTmp.clear();
		return false;
	}

	uint16_t start = 0;

	while (start < mDataTmp.size() - 2)
	{
		start = 0;
		while (start < mDataTmp.size() - 2)
		{
			if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN))
			{
				break;
			}

			if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5))))
			{
				break;
			}
			start++;
		}

		if (start != 0)
		{
			mErrorTimes++;
			for (int i = 0; i < start; i++)
			{
				mDataTmp.erase(mDataTmp.begin());
			}
		}

		if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
			return false;

		LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data();
		uint8_t crc = 0;

		for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++)
		{
			crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff];
		}

		if (crc == pkg->crc8)
		{
			double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360;
			if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2)
			{
				mErrorTimes++;
			}
			else
			{
				mSpeed = pkg->speed;
				mTimestamp = pkg->timestamp;
				uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000;
				float step = diff / (POINT_PER_PACK - 1) / 100.0;
				float start = (double)pkg->start_angle / 100.0;
				float end = (double)(pkg->end_angle % 36000) / 100.0;
				PointData data;
				for (int i = 0; i < POINT_PER_PACK; i++)
				{
					data.distance = pkg->point[i].distance;
					data.angle = start + i * step;
					if (data.angle >= 360.0)
					{
						data.angle -= 360.0;
					}
					data.confidence = pkg->point[i].confidence;
					mOnePkg[i] = data;
					mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence));
				}
				// prevent angle invert
				mOnePkg.back().angle = end;

				mIsPkgReady = true;
			}

			for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++)
			{
				mDataTmp.erase(mDataTmp.begin());
			}

			if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
			{
				break;
			}
		}
		else
		{
			mErrorTimes++;
			/*only remove header,not all frame,because lidar data may contain head*/
			for (int i = 0; i < 2; i++)
			{
				mDataTmp.erase(mDataTmp.begin());
			}
		}
	}

	return true;
}
bool LiPkg::AssemblePacket()
{
	float last_angle = 0;
	std::vector<PointData> tmp;
	int count = 0;
	for (auto n : mFrameTemp)
	{
		/*wait for enough data, need enough data to show a circle*/
		if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */
		{
			mFrameData.len = tmp.size();
			Transform(tmp); /*transform raw data to stantard data */

			if (tmp.size() == 0)
			{
				mFrameTemp.clear();
				mIsFrameReady = false;
				return false;
			}

#ifdef USE_SLBI
			Slbi sb(mSpeed);
			sb.FindBarcode(tmp);
#endif

#ifdef USE_SLBF
			Slbf sb(mSpeed);
			tmp = sb.NearFilter(tmp);
#endif

			std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b)
					  { return a.angle < b.angle; });
			mFrameData.angle_min = tmp[0].angle;
			mFrameData.angle_max = tmp.back().angle;
			mFrameData.distance.clear();
			mFrameData.intensities.clear();
			mFrameData.distance.resize(mFrameData.len);
			mFrameData.intensities.resize(mFrameData.len);
			float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len;
			float angle_acc = mFrameData.angle_min;
			int tmp_count = 0;
			/* interpolation method */
			for (uint32_t i = 0; i < mFrameData.len; i++)
			{
				if (angle_acc >= tmp[tmp_count].angle)
				{
					mFrameData.distance[i] = tmp[tmp_count].distance;
					mFrameData.intensities[i] = tmp[tmp_count].confidence;
					tmp_count++;
					if (tmp_count == tmp.size())
					{
						break;
					}
				}
				else
				{
					mFrameData.distance[i] = 0;
					mFrameData.intensities[i] = 0;
				}
				angle_acc += step;
			}
			std::vector<PointData> tmp2;
			for (uint32_t i = count; i < mFrameTemp.size(); i++)
			{
				tmp2.push_back(mFrameTemp[i]);
			}
			mFrameTemp.clear();
			mFrameTemp = tmp2;
			mIsFrameReady = true;
			return true;
		}
		else
		{
			tmp.push_back(n); /* getting data */
		}

		count++;
		last_angle = n.angle;
	}

	return false;
}
const std::array<PointData, POINT_PER_PACK> &LiPkg::GetPkgData(void)
{
	mIsPkgReady = false;
	return mOnePkg;
}

std::vector<cv::Mat> LiPkg::scan_to_pointcloud(FrameData mFrameData)
{
	u_int32_t len = mFrameData.len;
	float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min);
	float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max);
	cv::Mat pointcloud2(len, 3, CV_32F);
	cv::Mat intensities(1, len, CV_32F);
	cv::Mat pointdis(1, len, CV_32F);
	float a_increment = (angle_max - angle_min) / (float)(len - 1);
	for (int i = 0; i < len; i++)
	{
		float dis = mFrameData.distance[i] / 1000.f;
		intensities.at<float>(0, i) = mFrameData.intensities[i];
		pointdis.at<float>(0, i) = dis;

		float x_temp = cos(angle_min + i * a_increment) * dis;
		float y_temp = sin(angle_min + i * a_increment) * dis;
		pointcloud2.at<float>(i, 0) = x_temp;
		pointcloud2.at<float>(i, 1) = y_temp;
		pointcloud2.at<float>(i, 2) = 0.0;
	}

	return std::vector<cv::Mat>{pointcloud2, intensities, pointdis};
}

/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/

SlTransform::SlTransform(LDVersion version, bool to_right_hand)
{
	switch (version)
	{
	case LDVersion::LD_ZERO:
	case LDVersion::LD_NINE:
		offset_x = 8.1;
		offset_y = -22.5156;
		break;

	case LDVersion::LD_THREE:
	case LDVersion::LD_EIGHT:
		offset_x = 5.9;
		offset_y = -20.14;
		break;

	case LDVersion::LD_FOURTEENTH:
		offset_x = 5.9;
		offset_y = -20.14;
		break;

	default:
		break;
	}
}
Points2D SlTransform::Transform(const Points2D &data)
{
	Points2D tmp2;
	for (auto n : data)
	{
		/*Filter out invalid data*/
		if (n.distance == 0)
		{
			continue;
		}
		/*transfer the origin to the center of lidar circle*/
		/*The default direction of radar rotation is clockwise*/
		/*transfer to the right-hand coordinate system*/
		float right_hand = (360.f - n.angle);
		double x = n.distance + offset_x;
		double y = n.distance * 0.11923 + offset_y;
		double d = sqrt(x * x + y * y);
		double shift = atan(y / x) * 180.f / 3.14159;
		/*Choose whether to use the right-hand system according to the flag*/
		double angle;
		if (to_right_hand)
			angle = right_hand + shift;
		else
			angle = n.angle - shift;

		if (angle > 360)
		{
			angle -= 360;
		}
		if (angle < 0)
		{
			angle += 360;
		}

		tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y));
	}

	return tmp2;
}
SlTransform::~SlTransform()
{
}
void LD14_LiPkg::Transform(std::vector<PointData> &tmp)
{
	SlTransform trans(LDVersion::LD_FOURTEENTH);
	tmp = trans.Transform(tmp);
	// std::cout << "Transform LD_FOURTEENTH !!" << std::endl;
}