diff --git a/CMakeLists.txt b/CMakeLists.txt index 0f7d739..8f6b8ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX) # version set(YDLIDAR_SDK_VERSION_MAJOR 1) set(YDLIDAR_SDK_VERSION_MINOR 1) -set(YDLIDAR_SDK_VERSION_PATCH 15) +set(YDLIDAR_SDK_VERSION_PATCH 16) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h index 19306a5..13a253e 100644 --- a/core/common/DriverInterface.h +++ b/core/common/DriverInterface.h @@ -7,608 +7,584 @@ #include "ydlidar_def.h" #include - -namespace ydlidar { -namespace core { -using namespace base; -namespace common { - -class DriverInterface { - public: - /** - * @brief Set and Get LiDAR single channel. - * Whether LiDAR communication channel is a single-channel - * @note For a single-channel LiDAR, if the settings are reversed.\n - * an error will occur in obtaining device information and the LiDAR will Faied to Start.\n - * For dual-channel LiDAR, if th setttings are reversed.\n - * the device information cannot be obtained.\n - * Set the single channel to match the LiDAR. - * @remarks - -
G1/G2/G2A/G2C false -
G4/G5/G4B/G4PRO/G6/G7/F4/F4PRO false -
S4/S4B/X4/R2/G4C false -
S2/X2/X2L true -
TG15/TG30/TG50 false -
TX8/TX20 true -
T5/T15 false -
true -
- * @see DriverInterface::setSingleChannel and DriverInterface::getSingleChannel - */ - PropertyBuilderByName(bool, SingleChannel, protected); - /** - * @brief Set and Get LiDAR Type. - * @note Refer to the table below for the LiDAR Type.\n - * Set the LiDAR Type to match the LiDAR. - * @remarks - -
G1/G2A/G2/G2C [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) -
G4/G5/G4B/G4C/G4PRO [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) -
G6/G7/F4/F4PRO [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) -
S4/S4B/X4/R2/S2/X2/X2L [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) -
TG15/TG30/TG50/TX8/TX20 [TYPE_TOF](\ref LidarTypeID::TYPE_TOF) -
T5/T15 [TYPE_TOF_NET](\ref LidarTypeID::TYPE_TOF_NET) -
- * @see [LidarTypeID](\ref LidarTypeID) - * @see DriverInterface::setLidarType and DriverInterface::getLidarType - */ - PropertyBuilderByName(int, LidarType, protected); - //设备类型（串口或网络） - PropertyBuilderByName(uint8_t, DeviceType, protected); - /** - * @brief Set and Get Sampling interval. - * @note Negative correlation between sampling interval and lidar sampling rate.\n - * sampling interval = 1e9 / sampling rate(/s)\n - * Set the LiDAR sampling interval to match the LiDAR. - * @see DriverInterface::setPointTime and DriverInterface::getPointTime - */ - PropertyBuilderByName(uint32_t, PointTime, protected); - /** - * @brief Set and Get LiDAR Support Motor DTR. - * @note The current paramter settings are only valid - * if the LiDAR is connected to the serial port adapter via USB.\n - * If the LiDAR does not have external motor enable line, - * the current paramters do not need to be set.\n - * Set the LiDAR Motro DTR to match the LiDAR. - * @remarks - -
S4/S4B/S2/X2/X2L/X4 true -
TX8/TX20 true -
G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 false -
G1/G2A/G2C/R2/G2/G4B false -
TG15/TG30/TG50 false -
T5/T15 false -
- * @see DriverInterface::setSupportMotorDtrCtrl and DriverInterface::getSupportMotorDtrCtrl - */ - PropertyBuilderByName(bool, SupportMotorDtrCtrl, protected); - - PropertyBuilderByName(bool, HeartBeat, protected); - //是否开启调试 - PropertyBuilderByName(bool, Debug, protected); - //扫描频率 - PropertyBuilderByName(float, ScanFreq, protected); - //是否底板优先 - PropertyBuilderByName(bool, Bottom, protected); - //是否已获取到设备信息 - PropertyBuilderByName(int, HasDeviceInfo, protected); - - /** - * @par Constructor - * - */ - DriverInterface() : - serial_port(""), - m_baudrate(8000), - m_intensities(false), - m_intensityBit(10), - scan_node_buf(NULL), - scan_node_count(0), - nodeIndex(0), - retryCount(0), - isAutoReconnect(true), - isAutoconnting(false) { - m_SingleChannel = false; - m_LidarType = TYPE_TRIANGLE; - m_DeviceType = YDLIDAR_TYPE_SERIAL; - m_PointTime = 0; - m_SupportMotorDtrCtrl = true; - m_HeartBeat = false; - m_isScanning = false; - m_isConnected = false; - m_config.motor_rpm = 1200; - m_config.laserScanFrequency = 50; - m_config.correction_angle = 20640; - m_config.correction_distance = 6144; - m_driverErrno = NoError; - m_InvalidNodeCount = 0; - m_BufferSize = 0; - m_Debug = false; - m_ScanFreq = 0; - m_Bottom = true; - m_HasDeviceInfo = EPT_None; - } - - virtual ~DriverInterface() {} - - /** - * @brief Connecting Lidar \n - * After the connection if successful, you must use ::disconnect to close - * @param[in] port_path serial port - * @param[in] baudrate serial baudrate，YDLIDAR-SS： - * 230400 G2-SS-1 R2-SS-1 - * @return connection status - * @retval 0 success - * @retval < 0 failed - * @note After the connection if successful, you must use ::disconnect to close - * @see function ::YDlidarDriver::disconnect () - */ - virtual result_t connect(const char *port_path, uint32_t baudrate = 8000) = 0; - - /** - * @brief Returns a human-readable description of the given error code - * or the last error code of a socket or serial port - * @param isTCP TCP or UDP - * @return error information - */ - virtual const char *DescribeError(bool isTCP = true) = 0; - - static const char *DescribeDriverError(DriverError err) { - char const *errorString = "Unknown error"; - - switch (err) { - case NoError: - errorString = ("No error"); - break; - - case DeviceNotFoundError: - errorString = ("Device is not found"); - break; - - case PermissionError: - errorString = ("Device is not permission"); - break; - - case UnsupportedOperationError: - errorString = ("unsupported operation"); - break; - - case NotOpenError: - errorString = ("Device is not open"); - break; - - case TimeoutError: - errorString = ("Operation timed out"); - break; - - case BlockError: - errorString = ("Device Block"); - break; - - case NotBufferError: - errorString = ("Device Failed"); - break; - - case TrembleError: - errorString = ("Device Tremble"); - break; - - case LaserFailureError: - errorString = ("Laser Failure"); - break; - - default: - // an empty string will be interpreted as "Unknown error" - break; - } - - return errorString; - } - - - /*! - * @brief Disconnect the LiDAR. - */ - virtual void disconnect() = 0; - - /** - * @brief Get SDK Version \n - * static function - * @return Version - */ - virtual std::string getSDKVersion() = 0; - - /** - * @brief Is the Lidar in the scan \n - * @return scanning status - * @retval true scanning - * @retval false non-scanning - */ - virtual bool isscanning() const = 0; - - /** - * @brief Is it connected to the lidar \n - * @return connection status - * @retval true connected - * @retval false Non-connected - */ - virtual bool isconnected() const = 0; - - /** - * @brief Is there intensity \n - * @param[in] isintensities intentsity - * true intensity - * false no intensity - */ - virtual void setIntensities(const bool &isintensities) { - m_intensities = isintensities; - } - virtual void setIntensityBit(int bit) { - m_intensityBit = bit; - } - - /** - * @brief whether to support hot plug \n - * @param[in] enable hot plug : - * true support - * false no support - */ - virtual void setAutoReconnect(const bool &enable) = 0; - - /** - * @brief Get current scan update configuration. - * @returns scanCfg structure. - */ - virtual lidarConfig getFinishedScanCfg() const { - return m_config; - } - - /** - * @brief get Health status \n - * @return result status - * @retval RESULT_OK success - * @retval RESULT_FAILE or RESULT_TIMEOUT failed - */ - virtual result_t getHealth(device_health &health, - uint32_t timeout = DEFAULT_TIMEOUT) = 0; - - /** - * @brief get Device information \n - * @param[in] di Device information - * @param[in] timeout timeout - * @return result status - * @retval RESULT_OK success - * @retval RESULT_FAILE or RESULT_TIMEOUT failed - */ - virtual result_t getDeviceInfo( - device_info &di, - uint32_t timeout = DEFAULT_TIMEOUT) = 0; - - //获取级联雷达设备信息 - virtual result_t getDeviceInfo( - std::vector& dis, - uint32_t timeout = DEFAULT_TIMEOUT / 2) { - device_info di; - result_t ret = getDeviceInfo(di, timeout); - if (IS_OK(ret)) { - device_info_ex die; - memcpy(&die.di, &di, DEVICEINFOSIZE); - dis.push_back(die); - } - return ret; - } - - //获取设备信息 - virtual bool getDeviceInfoEx(device_info &di) { - UNUSED(di); - return false; - } - - /** - * @brief Turn on scanning \n - * @param[in] force Scan mode - * @param[in] timeout timeout - * @return result status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Just turn it on once - */ - virtual result_t startScan(bool force = false, - uint32_t timeout = DEFAULT_TIMEOUT) = 0; - - /** - * @brief turn off scanning \n - * @return result status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - */ - virtual result_t stop() = 0; - - /** - * @brief Get a circle of laser data \n - * @param[in] nodebuffer Laser data - * @param[in] count one circle of laser points - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Before starting, you must start the start the scan successfully with the ::startScan function - */ - virtual result_t grabScanData(node_info *nodebuffer, size_t &count, - uint32_t timeout = DEFAULT_TIMEOUT) = 0 ; - - /** - * @brief Get lidar scan frequency \n - * @param[in] frequency scanning frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAIL failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t getScanFrequency(scan_frequency &frequency, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Increase the scanning frequency by 1.0 HZ \n - * @param[in] frequency scanning frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t setScanFrequencyAdd(scan_frequency &frequency, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Reduce the scanning frequency by 1.0 HZ \n - * @param[in] frequency scanning frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t setScanFrequencyDis(scan_frequency &frequency, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Increase the scanning frequency by 0.1 HZ \n - * @param[in] frequency scanning frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t setScanFrequencyAddMic(scan_frequency &frequency, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Reduce the scanning frequency by 0.1 HZ \n - * @param[in] frequency scanning frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t setScanFrequencyDisMic(scan_frequency &frequency, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Get lidar sampling frequency \n - * @param[in] frequency sampling frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t getSamplingRate(sampling_rate &rate, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief Set the lidar sampling frequency \n - * @param[in] rate 　　　sampling frequency - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t setSamplingRate(sampling_rate &rate, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief get lidar zero offset angle \n - * @param[in] angle　　　 zero offset angle - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - virtual result_t getZeroOffsetAngle(offset_angle &angle, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief set lidar heart beat \n - * @param[in] beat heart beat status - * @param[in] timeout timeout - * @return return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - * @note Non-scan state, perform currect operation. - */ - - virtual result_t setScanHeartbeat(scan_heart_beat &beat, - uint32_t timeout = DEFAULT_TIMEOUT) { - return RESULT_FAIL; - } - - /** - * @brief setDriverError - * @param er - */ - virtual void setDriverError(const DriverError &er) { - ScopedLocker l(_error_lock); - m_driverErrno = er; - } - - /** - * @brief getDriverError - * @return - */ - virtual DriverError getDriverError() { - ScopedLocker l(_error_lock); - return m_driverErrno; - } - - /** - * @brief 设置雷达工作模式（目前只针对GS2雷达） - * @param[in] mode 雷达工作模式 - * @param[in] addr 雷达地址 - * @return 成功返回RESULT_OK，否则返回非RESULT_OK - */ - virtual result_t setWorkMode(int mode=0, uint8_t addr=0x00) {return RESULT_FAIL;} - - /** - * @brief 解析点云数据并判断带不带强度信息（目前只针对三角雷达） - * @return 成功返回RESULT_OK，否则返回非RESULT_OK - */ - virtual result_t getIntensityFlag() {return RESULT_OK;} - - public: - enum YDLIDAR_MODLES - { - YDLIDAR_None = 0, - YDLIDAR_F4 = 1, /**< F4 LiDAR Model. */ - YDLIDAR_T1 = 2, /**< T1 LiDAR Model. */ - YDLIDAR_F2 = 3, /**< F2 LiDAR Model. */ - YDLIDAR_S4 = 4, /**< S4 LiDAR Model. */ - YDLIDAR_S2PRO = YDLIDAR_S4, /**< S2PRO LiDAR Model. */ - YDLIDAR_G4 = 5, /**< G4 LiDAR Model. */ - YDLIDAR_X4 = 6, /**< X4 LiDAR Model. */ - YDLIDAR_G4PRO = 7, /**< G4PRO LiDAR Model. */ - YDLIDAR_F4PRO = 8, /**< F4PRO LiDAR Model. */ - YDLIDAR_R2 = 9, /**< R2 LiDAR Model. */ - YDLIDAR_G10 = 10, /**< G10 LiDAR Model. */ - YDLIDAR_S4B = 11, /**< S4B LiDAR Model. */ - YDLIDAR_S2 = 12, /**< S2 LiDAR Model. */ - YDLIDAR_G6 = 13, /**< G6 LiDAR Model. */ - YDLIDAR_G2A = 14, /**< G2A LiDAR Model. */ - YDLIDAR_G2B = 15, /**< G2 LiDAR Model. */ - YDLIDAR_G2C = 16, /**< G2C LiDAR Model. */ - YDLIDAR_G4B = 17, /**< G4B LiDAR Model. */ - YDLIDAR_G4C = 18, /**< G4C LiDAR Model. */ - YDLIDAR_G1 = 19, /**< G1 LiDAR Model. */ - YDLIDAR_G5 = 20, /**< G5 LiDAR Model. */ - YDLIDAR_G7 = 21, /**< G7 LiDAR Model. */ - YDLIDAR_SCL = 22, // SCL雷达 - - YDLIDAR_GS2 = 51, // GS2雷达 - YDLIDAR_GS1 = 52, // GS1雷达 - YDLIDAR_GS5 = 53, // GS5雷达 - YDLIDAR_GS6 = 54, // GS6雷达 - - YDLIDAR_TG15 = 100, /**< TG15 LiDAR Model. */ - YDLIDAR_TG30 = 101, /**< T30 LiDAR Model. */ - YDLIDAR_TG50 = 102, /**< TG50 LiDAR Model. */ - - YDLIDAR_TSA = 130, /**< TSA LiDAR Model. */ - YDLIDAR_Tmini = 140, /**< Tmini LiDAR Model. */ - YDLIDAR_TminiPRO = 150, /**< Tmini PRO LiDAR Model. */ - - YDLIDAR_SDM15 = 160, // SDM15单点雷达 - - YDLIDAR_T15 = 200, /**< T15 LiDAR Model. */ - - YDLIDAR_Tail, - }; - - enum YDLIDAR_RATE - { - YDLIDAR_RATE_4K = 0, /**< 4K sample rate code */ - YDLIDAR_RATE_8K = 1, /**< 8K sample rate code */ - YDLIDAR_RATE_9K = 2, /**< 9K sample rate code */ - YDLIDAR_RATE_10K = 3, /**< 10K sample rate code */ - }; - - public: - enum { - DEFAULT_TIMEOUT = 2000, /**< Default timeout. */ - DEFAULT_HEART_BEAT = 1000, /**< Default heartbeat timeout. */ - MAX_SCAN_NODES = 7200, /**< Default Max Scan Count. */ - DEFAULT_TIMEOUT_COUNT = 1, /**< Default Timeout Count. */ - }; - -protected: - /* Variable for LIDAR compatibility */ - /// LiDAR Scanning state - bool m_isScanning = false; - /// LiDAR connected state - bool m_isConnected = false; - /// Scan Data Event - Event _dataEvent; - /// Data Locker（不支持嵌套） - Locker _lock; - /// Parse Data thread - Thread _thread; - /// command locker（不支持嵌套） - Locker _cmd_lock; - /// driver error locker（不支持嵌套） - Locker _error_lock; - - /// LiDAR com port or IP Address - std::string serial_port; - /// baudrate or IP port - uint32_t m_baudrate; - /// LiDAR intensity - bool m_intensities; - /// LiDAR intensity bit - int m_intensityBit; - - /// LiDAR Point pointer - node_info *scan_node_buf; - /// LiDAR scan count - size_t scan_node_count; // &dis, + uint32_t timeout = DEFAULT_TIMEOUT / 2) + { + device_info di; + result_t ret = getDeviceInfo(di, timeout); + if (IS_OK(ret)) + { + device_info_ex die; + memcpy(&die.di, &di, DEVICEINFOSIZE); + dis.push_back(die); + } + return ret; + } + + // 获取设备信息 + virtual bool getDeviceInfoEx(device_info &di, int type=EPT_Module) + { + UNUSED(di); + return false; + } + + /** + * @brief Turn on scanning \n + * @param[in] force Scan mode + * @param[in] timeout timeout + * @return result status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Just turn it on once + */ + virtual result_t startScan(bool force = false, + uint32_t timeout = DEFAULT_TIMEOUT) = 0; + + /** + * @brief turn off scanning \n + * @return result status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + */ + virtual result_t stop() = 0; + + /** + * @brief Get a circle of laser data \n + * @param[in] nodebuffer Laser data + * @param[in] count one circle of laser points + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Before starting, you must start the start the scan successfully with the ::startScan function + */ + virtual result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT) = 0; + + /** + * @brief Get lidar scan frequency \n + * @param[in] frequency scanning frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAIL failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t getScanFrequency(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Increase the scanning frequency by 1.0 HZ \n + * @param[in] frequency scanning frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t setScanFrequencyAdd(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Reduce the scanning frequency by 1.0 HZ \n + * @param[in] frequency scanning frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t setScanFrequencyDis(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Increase the scanning frequency by 0.1 HZ \n + * @param[in] frequency scanning frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t setScanFrequencyAddMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Reduce the scanning frequency by 0.1 HZ \n + * @param[in] frequency scanning frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t setScanFrequencyDisMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Get lidar sampling frequency \n + * @param[in] frequency sampling frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t getSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief Set the lidar sampling frequency \n + * @param[in] rate 　　　sampling frequency + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t setSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief get lidar zero offset angle \n + * @param[in] angle　　　 zero offset angle + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + virtual result_t getZeroOffsetAngle(offset_angle &angle, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief set lidar heart beat \n + * @param[in] beat heart beat status + * @param[in] timeout timeout + * @return return status + * @retval RESULT_OK success + * @retval RESULT_FAILE failed + * @note Non-scan state, perform currect operation. + */ + + virtual result_t setScanHeartbeat(scan_heart_beat &beat, + uint32_t timeout = DEFAULT_TIMEOUT) + { + return RESULT_FAIL; + } + + /** + * @brief setDriverError + * @param er + */ + virtual void setDriverError(const DriverError &er) + { + ScopedLocker l(_error_lock); + m_driverErrno = er; + } + + /** + * @brief getDriverError + * @return + */ + virtual DriverError getDriverError() + { + ScopedLocker l(_error_lock); + return m_driverErrno; + } + + /** + * @brief 设置雷达工作模式（目前只针对GS2雷达） + * @param[in] mode 雷达工作模式 + * @param[in] addr 雷达地址 + * @return 成功返回RESULT_OK，否则返回非RESULT_OK + */ + virtual result_t setWorkMode(int mode = 0, uint8_t addr = 0x00) { return RESULT_FAIL; } + + /** + * @brief 解析点云数据并判断带不带强度信息（目前只针对三角雷达） + * @return 成功返回RESULT_OK，否则返回非RESULT_OK + */ + virtual result_t getIntensityFlag() { return RESULT_OK; } + + public: + enum YDLIDAR_MODLES + { + YDLIDAR_None = 0, + YDLIDAR_F4 = 1, /**< F4 LiDAR Model. */ + YDLIDAR_T1 = 2, /**< T1 LiDAR Model. */ + YDLIDAR_F2 = 3, /**< F2 LiDAR Model. */ + YDLIDAR_S4 = 4, /**< S4 LiDAR Model. */ + YDLIDAR_S2PRO = YDLIDAR_S4, /**< S2PRO LiDAR Model. */ + YDLIDAR_G4 = 5, /**< G4 LiDAR Model. */ + YDLIDAR_X4 = 6, /**< X4 LiDAR Model. */ + YDLIDAR_G4PRO = 7, /**< G4PRO LiDAR Model. */ + YDLIDAR_F4PRO = 8, /**< F4PRO LiDAR Model. */ + YDLIDAR_R2 = 9, /**< R2 LiDAR Model. */ + YDLIDAR_G10 = 10, /**< G10 LiDAR Model. */ + YDLIDAR_S4B = 11, /**< S4B LiDAR Model. */ + YDLIDAR_S2 = 12, /**< S2 LiDAR Model. */ + YDLIDAR_G6 = 13, /**< G6 LiDAR Model. */ + YDLIDAR_G2A = 14, /**< G2A LiDAR Model. */ + YDLIDAR_G2B = 15, /**< G2 LiDAR Model. */ + YDLIDAR_G2C = 16, /**< G2C LiDAR Model. */ + YDLIDAR_G4B = 17, /**< G4B LiDAR Model. */ + YDLIDAR_G4C = 18, /**< G4C LiDAR Model. */ + YDLIDAR_G1 = 19, /**< G1 LiDAR Model. */ + YDLIDAR_G5 = 20, /**< G5 LiDAR Model. */ + YDLIDAR_G7 = 21, /**< G7 LiDAR Model. */ + YDLIDAR_SCL = 22, // SCL雷达 + + YDLIDAR_GS2 = 51, // GS2雷达 + YDLIDAR_GS1 = 52, // GS1雷达 + YDLIDAR_GS5 = 53, // GS5雷达 + YDLIDAR_GS6 = 54, // GS6雷达 + + YDLIDAR_TG15 = 100, /**< TG15 LiDAR Model. */ + YDLIDAR_TG30 = 101, /**< T30 LiDAR Model. */ + YDLIDAR_TG50 = 102, /**< TG50 LiDAR Model. */ + + YDLIDAR_TEA = 110, //TEA雷达 + + YDLIDAR_TSA = 130, /**< TSA LiDAR Model. */ + YDLIDAR_Tmini = 140, /**< Tmini LiDAR Model. */ + YDLIDAR_TminiPRO = 150, /**< Tmini PRO LiDAR Model. */ + + YDLIDAR_SDM15 = 160, // SDM15单点雷达 + + YDLIDAR_T15 = 200, /**< T15 LiDAR Model. */ + + YDLIDAR_Tail, + }; + + enum YDLIDAR_RATE + { + YDLIDAR_RATE_4K = 0, /**< 4K sample rate code */ + YDLIDAR_RATE_8K = 1, /**< 8K sample rate code */ + YDLIDAR_RATE_9K = 2, /**< 9K sample rate code */ + YDLIDAR_RATE_10K = 3, /**< 10K sample rate code */ + }; + + public: + enum + { + DEFAULT_TIMEOUT = 2000, /**< Default timeout. */ + DEFAULT_HEART_BEAT = 1000, /**< Default heartbeat timeout. */ + MAX_SCAN_NODES = 7200, /**< Default Max Scan Count. */ + DEFAULT_TIMEOUT_COUNT = 1, /**< Default Timeout Count. */ + }; + + protected: + /* Variable for LIDAR compatibility */ + /// LiDAR Scanning state + bool m_isScanning = false; + /// LiDAR connected state + bool m_isConnected = false; + /// Scan Data Event + Event _dataEvent; + /// Data Locker（不支持嵌套） + Locker _lock; + /// Parse Data thread + Thread _thread; + /// command locker（不支持嵌套） + Locker _cmd_lock; + /// driver error locker（不支持嵌套） + Locker _error_lock; + + /// LiDAR com port or IP Address + std::string serial_port; + /// baudrate or IP port + uint32_t m_baudrate; + /// LiDAR intensity + bool m_intensities; + /// LiDAR intensity bit + int m_intensityBit; + + /// LiDAR Point pointer + node_info *scan_node_buf; + /// LiDAR scan count + size_t scan_node_count; //getDeviceInfoEx(di)); + return lidarPtr->getDeviceInfoEx(di, type); return false; } @@ -1395,7 +1399,7 @@ bool CYdLidar::getDeviceInfo() lidarPtr->setIntensityBit(m_IntensityBit); ret = true; - if (printfDeviceInfo(di, EPT_Baseplate)) + if (printfDeviceInfo(di, EPT_Base)) { Major = (uint8_t)(di.firmware_version >> 8); Minjor = (uint8_t)(di.firmware_version & 0xff); @@ -1430,7 +1434,7 @@ bool CYdLidar::getDeviceInfo() } // printf("LIDAR get device info finished, Elapsed time %u ms\n", getms() - t); - + //检查转速 if (hasScanFrequencyCtrl(di.model) || ((isTOFLidar(m_LidarType)) && !m_SingleChannel) || isNetTOFLidar(m_LidarType)) diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 0864432..0c73f1f 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -173,7 +173,7 @@ class YDLIDAR_API CYdLidar { //设置是否优先获取底板设备信息 void setBottomPriority(bool yes=true); //获取设备信息 - bool getDeviceInfo(device_info& di); + bool getDeviceInfo(device_info& di, int type); //获取级联设备信息 bool getDeviceInfo(std::vector& dis); diff --git a/src/SDMLidarDriver.h b/src/SDMLidarDriver.h index 359ca3e..b9d5388 100644 --- a/src/SDMLidarDriver.h +++ b/src/SDMLidarDriver.h @@ -443,7 +443,6 @@ class SDMLidarDriver : public DriverInterface private: serial::Serial *_serial = nullptr; //串口 std::vector recvBuff; //一包数据缓存 - device_info info_; device_health health_; }; diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp index ab882cc..111fe5f 100644 --- a/src/YDlidarDriver.cpp +++ b/src/YDlidarDriver.cpp @@ -79,7 +79,6 @@ YDlidarDriver::YDlidarDriver(uint8_t type) : asyncRecvPos = 0; async_size = 0; headerBuffer = reinterpret_cast(&header_); - infoBuffer = reinterpret_cast(&info_); healthBuffer = reinterpret_cast(&health_); nodeIndex = 0; globalRecvBuffer = new uint8_t[sizeof(tri_node_package)]; @@ -720,214 +719,10 @@ int YDlidarDriver::cacheScanData() return RESULT_OK; } -result_t YDlidarDriver::checkDeviceInfo( - uint8_t *recvBuffer, - uint8_t byte, - int recvPos, - int recvSize, - int pos) -{ - if (asyncRecvPos == sizeof(lidar_ans_header)) - { - if ((((pos < recvSize - 1) && byte == LIDAR_ANS_SYNC_BYTE1) || - (last_device_byte == LIDAR_ANS_SYNC_BYTE1 && byte == LIDAR_ANS_SYNC_BYTE2)) && - recvPos == 0) - { - if ((last_device_byte == LIDAR_ANS_SYNC_BYTE1 && - byte == LIDAR_ANS_SYNC_BYTE2)) - { - asyncRecvPos = 0; - async_size = 0; - headerBuffer[asyncRecvPos] = last_device_byte; - asyncRecvPos++; - headerBuffer[asyncRecvPos] = byte; - asyncRecvPos++; - last_device_byte = byte; - return RESULT_OK; - } - else - { - if (pos < recvSize - 1) - { - if (recvBuffer[pos + 1] == LIDAR_ANS_SYNC_BYTE2) - { - asyncRecvPos = 0; - async_size = 0; - headerBuffer[asyncRecvPos] = byte; - asyncRecvPos++; - last_device_byte = byte; - return RESULT_OK; - } - } - } - } - - last_device_byte = byte; - - if (header_.type == LIDAR_ANS_TYPE_DEVINFO || - header_.type == LIDAR_ANS_TYPE_DEVHEALTH) - { - if (header_.size < 1) - { - asyncRecvPos = 0; - async_size = 0; - } - else - { - - if (header_.type == LIDAR_ANS_TYPE_DEVHEALTH) - { - if (async_size < sizeof(health_)) - { - healthBuffer[async_size] = byte; - async_size++; - - if (async_size == sizeof(health_)) - { - asyncRecvPos = 0; - async_size = 0; - get_device_health_success = true; - last_device_byte = byte; - return RESULT_OK; - } - } - else - { - asyncRecvPos = 0; - async_size = 0; - } - } - else - { - - if (async_size < sizeof(info_)) - { - infoBuffer[async_size] = byte; - async_size++; - - if (async_size == sizeof(info_)) - { - asyncRecvPos = 0; - async_size = 0; - m_HasDeviceInfo |= EPT_Module; - - last_device_byte = byte; - return RESULT_OK; - } - } - else - { - asyncRecvPos = 0; - async_size = 0; - } - } - } - } - else if (header_.type == LIDAR_ANS_TYPE_MEASUREMENT) - { - asyncRecvPos = 0; - async_size = 0; - } - } else { - - switch (asyncRecvPos) { - case 0: - if (byte == LIDAR_ANS_SYNC_BYTE1 && recvPos == 0) { - headerBuffer[asyncRecvPos] = byte; - last_device_byte = byte; - asyncRecvPos++; - } - - break; - - case 1: - if (byte == LIDAR_ANS_SYNC_BYTE2 && recvPos == 0) { - headerBuffer[asyncRecvPos] = byte; - asyncRecvPos++; - last_device_byte = byte; - return RESULT_OK; - } else { - asyncRecvPos = 0; - } - - break; - - default: - break; - } - - if (asyncRecvPos >= 2) { - if (((pos < recvSize - 1 && byte == LIDAR_ANS_SYNC_BYTE1) || - (last_device_byte == LIDAR_ANS_SYNC_BYTE1 && byte == LIDAR_ANS_SYNC_BYTE2)) && - recvPos == 0) { - if ((last_device_byte == LIDAR_ANS_SYNC_BYTE1 && - byte == LIDAR_ANS_SYNC_BYTE2)) { - asyncRecvPos = 0; - async_size = 0; - headerBuffer[asyncRecvPos] = last_device_byte; - asyncRecvPos++; - } else { - if (pos < recvSize - 2) { - if (recvBuffer[pos + 1] == LIDAR_ANS_SYNC_BYTE2) { - asyncRecvPos = 0; - } - } - } - } - - headerBuffer[asyncRecvPos] = byte; - asyncRecvPos++; - last_device_byte = byte; - return RESULT_OK; - } - } - - return RESULT_FAIL; -} - //获取雷达模组抛出的设备信息、健康信息 result_t YDlidarDriver::waitDevicePackage(uint32_t timeout) { result_t ret = RESULT_FAIL; - // int recvPos = 0; - // asyncRecvPos = 0; - // uint32_t startTs = getms(); - // uint32_t waitTime = 0; - - // while ((waitTime = getms() - startTs) <= timeout) - // { - // size_t remainSize = PackagePaidBytes - recvPos; - // size_t recvSize = 0; - // ret = waitForData(remainSize, timeout - waitTime, &recvSize); - // if (!IS_OK(ret)) - // return ret; - - // ret = RESULT_FAIL; - - // if (recvSize > remainSize) - // { - // recvSize = remainSize; - // } - - // getData(globalRecvBuffer, recvSize); - - // for (size_t pos = 0; pos < recvSize; ++pos) - // { - // uint8_t currentByte = globalRecvBuffer[pos]; - - // if (checkDeviceInfo(globalRecvBuffer, currentByte, recvPos, recvSize, - // pos) == RESULT_OK) - // { - // continue; - // } - // } - - // if (m_HasDeviceInfo & EPT_Module) - // { - // ret = RESULT_OK; - // break; - // } - // } uint32_t st = getms(); uint32_t wt = 0; @@ -950,7 +745,7 @@ result_t YDlidarDriver::waitDevicePackage(uint32_t timeout) getData(reinterpret_cast(&di), DEVICEINFOSIZE); model = di.model; m_HasDeviceInfo |= EPT_Module; - info_ = di; + m_ModuleDevInfo = di; printfDeviceInfo(di, EPT_Module); break; } @@ -1773,7 +1568,7 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) //获取启动时抛出的设备信息或每帧数据中的设备信息 if (m_HasDeviceInfo & EPT_Module) { - info = this->info_; + info = m_BaseDevInfo; return RESULT_OK; } else @@ -1805,18 +1600,25 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) getData(reinterpret_cast(&info), sizeof(info)); model = info.model; - m_HasDeviceInfo |= EPT_Baseplate; - info_ = info; + m_HasDeviceInfo |= EPT_Base; + m_BaseDevInfo = info; } return ans; } } -bool YDlidarDriver::getDeviceInfoEx(device_info &info) +bool YDlidarDriver::getDeviceInfoEx(device_info &info, int type) { - if (m_HasDeviceInfo & EPT_All) + if ((type & EPT_Module) && + (m_HasDeviceInfo & EPT_Module)) + { + info = m_ModuleDevInfo; + return true; + } + else if ((type & EPT_Base) && + (m_HasDeviceInfo & EPT_Base)) { - info = info_; + info = m_BaseDevInfo; return true; } diff --git a/src/YDlidarDriver.h b/src/YDlidarDriver.h index 7f9caec..d3b9183 100644 --- a/src/YDlidarDriver.h +++ b/src/YDlidarDriver.h @@ -173,7 +173,7 @@ class YDlidarDriver : public DriverInterface { uint32_t timeout = DEFAULT_TIMEOUT); //获取设备信息 - virtual bool getDeviceInfoEx(device_info &info); + virtual bool getDeviceInfoEx(device_info &info, int type=EPT_Module); /** * @brief Turn on scanning \n @@ -393,20 +393,6 @@ class YDlidarDriver : public DriverInterface { */ result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT); - /** - * @brief check single-channel lidar device information - * @param recvBuffer LiDAR Data buffer - * @param byte current byte - * @param recvPos current recived pos - * @param recvSize Buffer size - * @param pos Device Buffer pos - * @return status - * @retval RESULT_OK success - * @retval RESULT_FAILE failed - */ - result_t checkDeviceInfo(uint8_t *recvBuffer, uint8_t byte, int recvPos, - int recvSize, int pos); - /** * @brief waiting device information * @param timeout timeout @@ -641,12 +627,9 @@ class YDlidarDriver : public DriverInterface { int asyncRecvPos; uint16_t async_size; - //singleChannel - device_info info_; device_health health_; lidar_ans_header header_; uint8_t *headerBuffer; - uint8_t *infoBuffer; uint8_t *healthBuffer; bool get_device_health_success;