175 lines
4.4 KiB
C++
175 lines
4.4 KiB
C++
/**
|
||
* @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 ********/
|