CleanRobot-UESTC/融合/lidar.h

/**
* @file         lidar.h
* @author       LD Robot
* @version      V01

* @brief
* @note
* @attention    COPYRIGHT LDROBOT
**/

#ifndef __LIPKG_H
#define __LIPKG_H
#include <stdint.h>
#include <vector>
#include <array>
#include <iostream>
#include <functional>
#include <thread>
#include <atomic>
#include <inttypes.h>
#include <mutex>
#include <string>
#include <condition_variable>
#include <sys/file.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <memory.h>
#include <libudev.h>
#include<opencv2/opencv.hpp>
#include <ros/ros.h>

struct PointData
{
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
	float angle;
	uint16_t distance;
	uint8_t confidence;
	//ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
	double x;
	double y;
	PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0)
	{
		this->angle = angle;
		this->distance = distance;
		this->confidence = confidence;
		this->x = x;
		this->y = y;
	}
	PointData() {}
	friend std::ostream& operator<<(std::ostream& os, const PointData& data)
	{
		os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y;
		return  os;
	}
};
typedef std::vector<PointData> Points2D;


enum
{
	PKG_HEADER = 0x54,
	PKG_VER_LEN = 0x2C,
	POINT_PER_PACK = 12,
};
typedef struct  __attribute__((packed))
{
	uint16_t distance;
	uint8_t  confidence;
}LidarPointStructDef;
typedef struct  __attribute__((packed))
{
	uint8_t           header;
	uint8_t           ver_len;
	uint16_t          speed;
	uint16_t          start_angle;
	LidarPointStructDef point[POINT_PER_PACK];
	uint16_t          end_angle;
	uint16_t          timestamp;
	uint8_t           crc8;
}LiDARFrameTypeDef;
struct FrameData
{
	ros::Time timestamp;
	float angle_min;
	float angle_max;
	uint32_t len;
	std::vector<uint16_t> distance;
	std::vector<uint8_t> intensities;
};
class LiPkg
{
public:
	LiPkg();
	double GetSpeed(void);	/*Lidar spin speed (Hz)*/
	uint16_t GetTimestamp(void) { return mTimestamp; }   /*time stamp of the packet */
	bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */
	bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/
	long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/
	const std::array<PointData, POINT_PER_PACK>& GetPkgData(void);/*original data package*/
	bool Parse(const uint8_t* data, long len);/*parse single packet*/
	virtual void Transform(std::vector<PointData>& tmp) = 0;/*transform raw data to stantard data */
	bool AssemblePacket();/*combine stantard data into data frames and calibrate*/
	const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; }
	std::vector<cv::Mat> scan_to_pointcloud(FrameData mFrameData);
private:
	uint16_t mTimestamp;
	double mSpeed;
	std::vector<uint8_t> mDataTmp;
	long mErrorTimes;
	std::array<PointData, POINT_PER_PACK>mOnePkg;
	std::vector<PointData> mFrameTemp;
	FrameData mFrameData;
	bool mIsPkgReady;
	bool mIsFrameReady;
};
class LD14_LiPkg : public LiPkg
{
public:
	virtual void Transform(std::vector<PointData>& tmp);
};

enum class LDVersion
{
	LD_ZERO,  /*Zero  generation lidar*/
	LD_THREE, /*Third generation lidar*/
	LD_EIGHT, /*Eight generation radar*/
	LD_NINE,  /*Nine  generation radar*/
	LD_FOURTEENTH /*Fourteenth generation radar*/
};

class SlTransform
{
private:
	bool to_right_hand = true;
	double offset_x;
	double offset_y;

public:
	SlTransform(LDVersion version, bool to_right_hand = false);
	Points2D Transform(const Points2D& data);
	~SlTransform();
};



class CmdInterfaceLinux
{
public:
	CmdInterfaceLinux(int32_t ver);
	~CmdInterfaceLinux();

	bool Open(std::string& port_name);
	bool Close();
	bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len);
	bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len);
	bool GetCmdDevices(std::vector<std::pair<std::string, std::string> >& device_list);
	void SetReadCallback(std::function<void(const char*, size_t length)> callback) { mReadCallback = callback; }
	bool IsOpened() { return mIsCmdOpened.load(); };
	bool IsExited() { return mRxThreadExitFlag.load(); };

private:
	std::thread* mRxThread;
	static void mRxThreadProc(void* param);
	long long mRxCount;
	int32_t version;
	int32_t mComHandle;
	std::atomic<bool> mIsCmdOpened, mRxThreadExitFlag;
	std::function<void(const char*, size_t length)> mReadCallback;
};

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