diff --git a/CMakeLists.txt b/CMakeLists.txt index eef49aa..c1b0375 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 0) -set(YDLIDAR_SDK_VERSION_PATCH 5) +set(YDLIDAR_SDK_VERSION_PATCH 6) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h index 5b2a7d6..bba7c4d 100644 --- a/core/common/ydlidar_help.h +++ b/core/common/ydlidar_help.h @@ -204,109 +204,81 @@ inline std::string lidarModelToString(int model) { * @param model lidar model. * @return lidar sampling rate. */ -inline int lidarModelDefaultSampleRate(int model) { - int sample_rate = 4; +inline std::vector getDefaultSampleRate(int model) +{ + std::vector srs; - switch (model) { + switch (model) + { case DriverInterface::YDLIDAR_F4: - break; - case DriverInterface::YDLIDAR_T1: - break; - case DriverInterface::YDLIDAR_F2: - break; - case DriverInterface::YDLIDAR_S4: + srs.push_back(4); break; - case DriverInterface::YDLIDAR_G4: - sample_rate = 9; + srs.push_back(9); break; - case DriverInterface::YDLIDAR_X4: - sample_rate = 5; + srs.push_back(5); break; - case DriverInterface::YDLIDAR_G4PRO: - sample_rate = 9; + srs.push_back(9); break; - case DriverInterface::YDLIDAR_F4PRO: - sample_rate = 4; + srs.push_back(4); break; - case DriverInterface::YDLIDAR_R2: - sample_rate = 5; + srs.push_back(5); break; - case DriverInterface::YDLIDAR_G10: - sample_rate = 10; + srs.push_back(10); break; - case DriverInterface::YDLIDAR_S4B: - sample_rate = 4; + srs.push_back(4); break; - case DriverInterface::YDLIDAR_S2: - sample_rate = 3; + srs.push_back(3); break; - case DriverInterface::YDLIDAR_G6: - sample_rate = 18; + srs.push_back(18); break; - case DriverInterface::YDLIDAR_G2A: - sample_rate = 5; - break; - case DriverInterface::YDLIDAR_G2B: - sample_rate = 5; + srs.push_back(5); break; - case DriverInterface::YDLIDAR_G2C: - sample_rate = 4; - break; - case DriverInterface::YDLIDAR_G4B: - break; - case DriverInterface::YDLIDAR_G4C: + srs.push_back(4); break; - case DriverInterface::YDLIDAR_G1: - sample_rate = 9; - break; - case DriverInterface::YDLIDAR_G5: - sample_rate = 9; + srs.push_back(9); break; - case DriverInterface::YDLIDAR_G7: - sample_rate = 18; + srs.push_back(18); break; - case DriverInterface::YDLIDAR_TG15: - sample_rate = 20; + srs.push_back(20); break; - case DriverInterface::YDLIDAR_TG30: - sample_rate = 20; + srs.push_back(10); + srs.push_back(20); break; - case DriverInterface::YDLIDAR_TG50: - sample_rate = 20; + srs.push_back(20); break; - case DriverInterface::YDLIDAR_T15: - sample_rate = 20; + srs.push_back(20); break; default: + srs.push_back(4); break; } - return sample_rate ; + return srs; } /*! @@ -708,58 +680,56 @@ inline int ConvertUserToLidarSmaple(int model, int m_SampleRate, * @param rate LiDAR sampling rate code * @return user sampling code */ -inline int ConvertLidarToUserSmaple(int model, int rate) { +inline int ConvertLidarToUserSmaple(int model, int rate) +{ int _samp_rate = 9; - switch (rate) { + if (!isOctaveLidar(model) && + !isTOFLidarByModel(model)) + { + switch (rate) + { + case DriverInterface::YDLIDAR_RATE_4K: + _samp_rate = 4; + break; + case DriverInterface::YDLIDAR_RATE_8K: + _samp_rate = 8; + if (model == DriverInterface::YDLIDAR_F4PRO) + _samp_rate = 6; + break; + case DriverInterface::YDLIDAR_RATE_9K: + _samp_rate = 9; + break; + case DriverInterface::YDLIDAR_RATE_10K: + _samp_rate = 10; + break; + default: + //修改默认为当前获取到采样率值 + _samp_rate = rate; + break; + } + } + else + { + switch (rate) + { case DriverInterface::YDLIDAR_RATE_4K: _samp_rate = 10; - - if (!isOctaveLidar(model)) { - _samp_rate = 4; - } - break; - case DriverInterface::YDLIDAR_RATE_8K: _samp_rate = 16; - - if (!isOctaveLidar(model)) { - _samp_rate = 8; - - if (model == DriverInterface::YDLIDAR_F4PRO) { - _samp_rate = 6; - } - } - break; - case DriverInterface::YDLIDAR_RATE_9K: _samp_rate = 18; - - if (!isOctaveLidar(model)) { - _samp_rate = 9; - } - break; - case DriverInterface::YDLIDAR_RATE_10K: _samp_rate = 20; - - if (!isOctaveLidar(model)) { - _samp_rate = 10; - } - break; - default: - _samp_rate = 9; - - if (isOctaveLidar(model)) { - _samp_rate = 18; - } - + //修改默认为当前获取到采样率值 + _samp_rate = rate; break; + } } return _samp_rate; diff --git a/samples/tof_test.cpp b/samples/tof_test.cpp index 6cbee4c..5feab06 100644 --- a/samples/tof_test.cpp +++ b/samples/tof_test.cpp @@ -225,7 +225,7 @@ int main(int argc, char *argv[]) { b_optvalue = isSingleChannel; laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); /// intensity - b_optvalue = true; + b_optvalue = false; laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); /// Motor DTR laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); @@ -272,8 +272,7 @@ int main(int argc, char *argv[]) { scan.config.scan_time); //使用拖尾滤波器 - filter.filter(scan, 0, 0, outScan); - + // filter.filter(scan, 0, 0, outScan); // for (size_t i=0; i(1e9 / 5000); - m_AngleOffset = 0.0f; - lidar_model = DriverInterface::YDLIDAR_G2B; - m_Intensity = false; + m_IgnoreString = ""; + m_PointTime = static_cast(1e9 / 5000); + m_AngleOffset = 0.0f; + lidar_model = DriverInterface::YDLIDAR_G2B; + m_Intensity = false; m_IntensityBit = 10; - last_node_time = getTime(); - global_nodes = new node_info[DriverInterface::MAX_SCAN_NODES]; - last_frequency = 0; - m_FristNodeTime = getTime(); - m_AllNode = 0; - m_DeviceType = YDLIDAR_TYPE_SERIAL; + last_node_time = getTime(); + global_nodes = new node_info[DriverInterface::MAX_SCAN_NODES]; + last_frequency = 0; + m_FristNodeTime = getTime(); + m_AllNode = 0; + m_DeviceType = YDLIDAR_TYPE_SERIAL; m_SupportMotorDtrCtrl = true; - m_SupportHearBeat = false; - m_parsingCompleted = false; + m_SupportHearBeat = false; + m_parsingCompleted = false; m_isAngleOffsetCorrected = false; - m_field_of_view = 360.f; + m_field_of_view = 360.f; memset(&m_LidarVersion, 0, sizeof(LidarVersion)); zero_offset_angle_scale = 4.f; } @@ -91,17 +91,21 @@ CYdLidar::CYdLidar(): lidarPtr(nullptr) { /*------------------------------------------------------------- ~CYdLidar -------------------------------------------------------------*/ -CYdLidar::~CYdLidar() { +CYdLidar::~CYdLidar() +{ disconnecting(); - if (global_nodes) { + if (global_nodes) + { delete[] global_nodes; global_nodes = NULL; } } -bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { - if (optval == NULL) { +bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) +{ + if (optval == NULL) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -110,8 +114,10 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { return false; } - if (optname >= LidarPropFixedResolution) { - if (optlen != sizeof(bool)) { + if (optname >= LidarPropFixedResolution) + { + if (optlen != sizeof(bool)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -119,9 +125,11 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { #endif return false; } - - } else if (optname >= LidarPropMaxRange) { - if (optlen != sizeof(float)) { + } + else if (optname >= LidarPropMaxRange) + { + if (optlen != sizeof(float)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -129,8 +137,11 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { #endif return false; } - } else if (optname >= LidarPropSerialBaudrate) { - if (optlen != sizeof(int)) { + } + else if (optname >= LidarPropSerialBaudrate) + { + if (optlen != sizeof(int)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -138,114 +149,118 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { #endif return false; } - } else { - } - + else + { + } bool ret = true; - switch (optname) { - case LidarPropSerialPort: - m_SerialPort = (const char *)optval; - break; + switch (optname) + { + case LidarPropSerialPort: + m_SerialPort = (const char *)optval; + break; - case LidarPropIgnoreArray: - m_IgnoreString = (const char *)optval; - m_IgnoreArray = ydlidar::split(m_IgnoreString, ','); + case LidarPropIgnoreArray: + m_IgnoreString = (const char *)optval; + m_IgnoreArray = ydlidar::split(m_IgnoreString, ','); - if (m_IgnoreArray.size() % 2 != 0) { - m_IgnoreArray.clear(); - ret = false; - } + if (m_IgnoreArray.size() % 2 != 0) + { + m_IgnoreArray.clear(); + ret = false; + } - break; + break; - case LidarPropFixedResolution: - m_FixedResolution = *(bool *)(optval); - break; + case LidarPropFixedResolution: + m_FixedResolution = *(bool *)(optval); + break; - case LidarPropReversion: - m_Reversion = *(bool *)(optval); - break; + case LidarPropReversion: + m_Reversion = *(bool *)(optval); + break; - case LidarPropInverted: - m_Inverted = *(bool *)(optval); - break; + case LidarPropInverted: + m_Inverted = *(bool *)(optval); + break; - case LidarPropAutoReconnect: - m_AutoReconnect = *(bool *)(optval); - break; + case LidarPropAutoReconnect: + m_AutoReconnect = *(bool *)(optval); + break; - case LidarPropSingleChannel: - m_SingleChannel = *(bool *)(optval); - break; + case LidarPropSingleChannel: + m_SingleChannel = *(bool *)(optval); + break; - case LidarPropIntenstiy: - m_Intensity = *(bool *)(optval); - break; - case LidarPropIntenstiyBit: - m_IntensityBit = *(int *)(optval); - break; + case LidarPropIntenstiy: + m_Intensity = *(bool *)(optval); + break; + case LidarPropIntenstiyBit: + m_IntensityBit = *(int *)(optval); + break; - case LidarPropSupportMotorDtrCtrl: - m_SupportMotorDtrCtrl = *(bool *)(optval); - break; + case LidarPropSupportMotorDtrCtrl: + m_SupportMotorDtrCtrl = *(bool *)(optval); + break; - case LidarPropSupportHeartBeat: - m_SupportHearBeat = *(bool *)(optval); - break; + case LidarPropSupportHeartBeat: + m_SupportHearBeat = *(bool *)(optval); + break; - case LidarPropMaxRange: - m_MaxRange = *(float *)(optval); - break; + case LidarPropMaxRange: + m_MaxRange = *(float *)(optval); + break; - case LidarPropMinRange: - m_MinRange = *(float *)(optval); - break; + case LidarPropMinRange: + m_MinRange = *(float *)(optval); + break; - case LidarPropMaxAngle: - m_MaxAngle = *(float *)(optval); - break; + case LidarPropMaxAngle: + m_MaxAngle = *(float *)(optval); + break; - case LidarPropMinAngle: - m_MinAngle = *(float *)(optval); - break; + case LidarPropMinAngle: + m_MinAngle = *(float *)(optval); + break; - case LidarPropScanFrequency: - m_ScanFrequency = *(float *)(optval); - break; + case LidarPropScanFrequency: + m_ScanFrequency = *(float *)(optval); + break; - case LidarPropSerialBaudrate: - m_SerialBaudrate = *(int *)(optval); - break; + case LidarPropSerialBaudrate: + m_SerialBaudrate = *(int *)(optval); + break; - case LidarPropLidarType: - m_LidarType = *(int *)(optval); - break; + case LidarPropLidarType: + m_LidarType = *(int *)(optval); + break; - case LidarPropDeviceType: - m_DeviceType = *(int *)(optval); - break; + case LidarPropDeviceType: + m_DeviceType = *(int *)(optval); + break; - case LidarPropSampleRate: - m_SampleRate = *(int *)(optval); - break; + case LidarPropSampleRate: + m_SampleRate = *(int *)(optval); + break; - case LidarPropAbnormalCheckCount: - m_AbnormalCheckCount = *(int *)(optval); - break; + case LidarPropAbnormalCheckCount: + m_AbnormalCheckCount = *(int *)(optval); + break; - default : - ret = false; - break; + default: + ret = false; + break; } return ret; } -bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { - if (optval == NULL) { +bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) +{ + if (optval == NULL) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -254,8 +269,10 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { return false; } - if (optname >= LidarPropFixedResolution) { - if (optlen != sizeof(bool)) { + if (optname >= LidarPropFixedResolution) + { + if (optlen != sizeof(bool)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -263,9 +280,11 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { #endif return false; } - - } else if (optname >= LidarPropMaxRange) { - if (optlen != sizeof(float)) { + } + else if (optname >= LidarPropMaxRange) + { + if (optlen != sizeof(float)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -273,8 +292,11 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { #endif return false; } - } else if (optname >= LidarPropSerialBaudrate) { - if (optlen != sizeof(int)) { + } + else if (optname >= LidarPropSerialBaudrate) + { + if (optlen != sizeof(int)) + { #if defined(_WIN32) SetLastError(EINVAL); #else @@ -282,103 +304,104 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) { #endif return false; } - } else { - + } + else + { } bool ret = true; - switch (optname) { - case LidarPropSerialPort: - memcpy(optval, m_SerialPort.c_str(), optlen); - break; - - case LidarPropIgnoreArray: - memcpy(optval, m_IgnoreString.c_str(), optlen); - break; - - case LidarPropFixedResolution: - memcpy(optval, &m_FixedResolution, optlen); - break; - - case LidarPropReversion: - memcpy(optval, &m_Reversion, optlen); - break; - - case LidarPropInverted: - memcpy(optval, &m_Inverted, optlen); - break; - - case LidarPropAutoReconnect: - memcpy(optval, &m_AutoReconnect, optlen); - break; - - case LidarPropSingleChannel: - memcpy(optval, &m_SingleChannel, optlen); - break; - - case LidarPropIntenstiy: - memcpy(optval, &m_Intensity, optlen); - break; - case LidarPropIntenstiyBit: - memcpy(optval, &m_IntensityBit, optlen); - break; - - case LidarPropSupportMotorDtrCtrl: - memcpy(optval, &m_SupportMotorDtrCtrl, optlen); - break; - - case LidarPropSupportHeartBeat: - memcpy(optval, &m_SupportHearBeat, optlen); - break; - - case LidarPropMaxRange: - memcpy(optval, &m_MaxRange, optlen); - break; - - case LidarPropMinRange: - memcpy(optval, &m_MinRange, optlen); - break; - - case LidarPropMaxAngle: - memcpy(optval, &m_MaxAngle, optlen); - break; - - case LidarPropMinAngle: - memcpy(optval, &m_MinAngle, optlen); - break; - - case LidarPropScanFrequency: - memcpy(optval, &m_ScanFrequency, optlen); - break; - - case LidarPropSerialBaudrate: - memcpy(optval, &m_SerialBaudrate, optlen); - break; - - case LidarPropLidarType: - memcpy(optval, &m_LidarType, optlen); - break; - - case LidarPropDeviceType: - memcpy(optval, &m_DeviceType, optlen); - break; - - case LidarPropSampleRate: - memcpy(optval, &m_SampleRate, optlen); - break; - - case LidarPropAbnormalCheckCount: - memcpy(optval, &m_AbnormalCheckCount, optlen); - break; - - default : - ret = false; - break; + switch (optname) + { + case LidarPropSerialPort: + memcpy(optval, m_SerialPort.c_str(), optlen); + break; + + case LidarPropIgnoreArray: + memcpy(optval, m_IgnoreString.c_str(), optlen); + break; + + case LidarPropFixedResolution: + memcpy(optval, &m_FixedResolution, optlen); + break; + + case LidarPropReversion: + memcpy(optval, &m_Reversion, optlen); + break; + + case LidarPropInverted: + memcpy(optval, &m_Inverted, optlen); + break; + + case LidarPropAutoReconnect: + memcpy(optval, &m_AutoReconnect, optlen); + break; + + case LidarPropSingleChannel: + memcpy(optval, &m_SingleChannel, optlen); + break; + + case LidarPropIntenstiy: + memcpy(optval, &m_Intensity, optlen); + break; + case LidarPropIntenstiyBit: + memcpy(optval, &m_IntensityBit, optlen); + break; + + case LidarPropSupportMotorDtrCtrl: + memcpy(optval, &m_SupportMotorDtrCtrl, optlen); + break; + + case LidarPropSupportHeartBeat: + memcpy(optval, &m_SupportHearBeat, optlen); + break; + + case LidarPropMaxRange: + memcpy(optval, &m_MaxRange, optlen); + break; + + case LidarPropMinRange: + memcpy(optval, &m_MinRange, optlen); + break; + + case LidarPropMaxAngle: + memcpy(optval, &m_MaxAngle, optlen); + break; + + case LidarPropMinAngle: + memcpy(optval, &m_MinAngle, optlen); + break; + + case LidarPropScanFrequency: + memcpy(optval, &m_ScanFrequency, optlen); + break; + + case LidarPropSerialBaudrate: + memcpy(optval, &m_SerialBaudrate, optlen); + break; + + case LidarPropLidarType: + memcpy(optval, &m_LidarType, optlen); + break; + + case LidarPropDeviceType: + memcpy(optval, &m_DeviceType, optlen); + break; + + case LidarPropSampleRate: + memcpy(optval, &m_SampleRate, optlen); + break; + + case LidarPropAbnormalCheckCount: + memcpy(optval, &m_AbnormalCheckCount, optlen); + break; + + default: + ret = false; + break; } return ret; - } /*------------------------------------------------------------- @@ -413,15 +436,18 @@ bool CYdLidar::initialize() /*------------------------------------------------------------- initialize -------------------------------------------------------------*/ -void CYdLidar::GetLidarVersion(LidarVersion &version) { +void CYdLidar::GetLidarVersion(LidarVersion &version) +{ memcpy(&version, &m_LidarVersion, sizeof(LidarVersion)); } /*------------------------------------------------------------- turnOn -------------------------------------------------------------*/ -bool CYdLidar::turnOn() { - if (isScanning && lidarPtr->isscanning()) { +bool CYdLidar::turnOn() +{ + if (isScanning && lidarPtr->isscanning()) + { return true; } @@ -429,10 +455,12 @@ bool CYdLidar::turnOn() { // start scan... result_t op_result = lidarPtr->startScan(); - if (!IS_OK(op_result)) { + if (!IS_OK(op_result)) + { op_result = lidarPtr->startScan(); - if (!IS_OK(op_result)) { + if (!IS_OK(op_result)) + { lidarPtr->stop(); fprintf(stderr, "[CYdLidar] Failed to start scan mode: %x\n", op_result); isScanning = false; @@ -445,7 +473,8 @@ bool CYdLidar::turnOn() { m_PointTime = lidarPtr->getPointTime(); - if (checkLidarAbnormal()) { + if (checkLidarAbnormal()) + { lidarPtr->stop(); fprintf(stderr, "[CYdLidar] Failed to turn on the Lidar, because the lidar is [%s].\n", @@ -454,30 +483,36 @@ bool CYdLidar::turnOn() { return false; } - if (m_SingleChannel && !isNetTOFLidar(m_LidarType)) { + if (m_SingleChannel && !isNetTOFLidar(m_LidarType)) + { handleSingleChannelDevice(); - } else { - printf("[YDLIDAR INFO] Current Sampling Rate : %dK\n", m_SampleRate); + } + else + { + printf("[YDLIDAR INFO1] Current Sampling Rate : %dK\n", m_SampleRate); } m_field_of_view = 360.f; - if (isNetTOFLidar(m_LidarType)) { + if (isNetTOFLidar(m_LidarType)) + { lidarConfig cfg = lidarPtr->getFinishedScanCfg(); m_field_of_view = cfg.fov_end - cfg.fov_start; - if (cfg.fov_end - 180 < m_MaxAngle) { + if (cfg.fov_end - 180 < m_MaxAngle) + { m_MaxAngle = cfg.fov_end - 180; } - if (cfg.fov_start - 180 > m_MinAngle) { + if (cfg.fov_start - 180 > m_MinAngle) + { m_MinAngle = cfg.fov_start - 180; } } last_frequency = 0; m_FristNodeTime = getTime(); - m_AllNode = 0; + m_AllNode = 0; m_PointTime = lidarPtr->getPointTime(); isScanning = true; lidarPtr->setAutoReconnect(m_AutoReconnect); @@ -487,238 +522,274 @@ bool CYdLidar::turnOn() { } /*------------------------------------------------------------- - doProcessSimple + doProcessSimple -------------------------------------------------------------*/ bool CYdLidar::doProcessSimple(LaserScan &outscan) { - // Boud? - if (!checkHardware()) { - delay(200 / m_ScanFrequency); - m_AllNode = 0; - m_FristNodeTime = getTime(); - return false; - } + // Boud? + if (!checkHardware()) + { + delay(200 / m_ScanFrequency); + m_AllNode = 0; + m_FristNodeTime = getTime(); + return false; + } - size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; - //wait Scan data: - uint64_t tim_scan_start = getTime(); - uint64_t startTs = tim_scan_start; - //从缓存中获取已采集的一圈扫描数据 - result_t op_result = lidarPtr->grabScanData(global_nodes, count); - uint64_t tim_scan_end = getTime(); - uint64_t endTs = tim_scan_end; - uint64_t sys_scan_time = tim_scan_end - tim_scan_start; //获取一圈数据所花费的时间 - outscan.points.clear(); + // wait Scan data: + uint64_t tim_scan_start = getTime(); + uint64_t startTs = tim_scan_start; + //从缓存中获取已采集的一圈扫描数据 + result_t op_result = lidarPtr->grabScanData(global_nodes, count); + uint64_t tim_scan_end = getTime(); + uint64_t endTs = tim_scan_end; + uint64_t sys_scan_time = tim_scan_end - tim_scan_start; //获取一圈数据所花费的时间 + outscan.points.clear(); + + // Fill in scan data: + if (IS_OK(op_result)) + { + int offsetSize = 0; - // Fill in scan data: - if (IS_OK(op_result)) + if (isNetTOFLidar(m_LidarType)) { - int offsetSize = 0; + double echo_angle = static_cast(m_field_of_view * 1.0 / count); - if (isNetTOFLidar(m_LidarType)) { - double echo_angle = static_cast(m_field_of_view * 1.0 / count); + if (echo_angle != 0.0) + { + offsetSize = static_cast((360 - m_field_of_view) / echo_angle); + } + } - if (echo_angle != 0.0) { - offsetSize = static_cast((360 - m_field_of_view) / echo_angle); - } - } + //根据采样频率计算的采样间隔时间计算出来总的扫描时间 + uint64_t scan_time = m_PointTime * (count - 1 + offsetSize); + int timeDiff = static_cast(sys_scan_time - scan_time); - //根据采样频率计算的采样间隔时间计算出来总的扫描时间 - uint64_t scan_time = m_PointTime * (count - 1 + offsetSize); - int timeDiff = static_cast(sys_scan_time - scan_time); + bool HighPayLoad = false; - bool HighPayLoad = false; + if (global_nodes[0].stamp > 0 && + global_nodes[0].stamp < tim_scan_start) + { + tim_scan_end = global_nodes[0].stamp; + HighPayLoad = true; + } - if (global_nodes[0].stamp > 0 && - global_nodes[0].stamp < tim_scan_start) { - tim_scan_end = global_nodes[0].stamp; - HighPayLoad = true; - } + tim_scan_end -= m_PointTime; + tim_scan_end -= global_nodes[0].delay_time; + tim_scan_start = tim_scan_end - scan_time; - tim_scan_end -= m_PointTime; - tim_scan_end -= global_nodes[0].delay_time; - tim_scan_start = tim_scan_end - scan_time; + if (!HighPayLoad && tim_scan_start < startTs) + { + tim_scan_start = startTs; + tim_scan_end = tim_scan_start + scan_time; + } - if (!HighPayLoad && tim_scan_start < startTs) { - tim_scan_start = startTs; - tim_scan_end = tim_scan_start + scan_time; - } + if ((last_node_time + m_PointTime) >= tim_scan_start && + (last_node_time + m_PointTime) < endTs - scan_time) + { + tim_scan_start = last_node_time + m_PointTime; + tim_scan_end = tim_scan_start + scan_time; + } - if ((last_node_time + m_PointTime) >= tim_scan_start && - (last_node_time + m_PointTime) < endTs - scan_time) { - tim_scan_start = last_node_time + m_PointTime; - tim_scan_end = tim_scan_start + scan_time; - } + if (m_AllNode == 0 && abs(timeDiff) < 10 * 1e6) + { + m_FristNodeTime = tim_scan_start; + m_AllNode += (count + offsetSize); + } + else if (m_AllNode != 0) + { + m_AllNode += (count + offsetSize); + } - if (m_AllNode == 0 && abs(timeDiff) < 10 * 1e6) { - m_FristNodeTime = tim_scan_start; - m_AllNode += (count + offsetSize); - } else if (m_AllNode != 0) { - m_AllNode += (count + offsetSize); - } + last_node_time = tim_scan_end; - last_node_time = tim_scan_end; + if (m_MaxAngle < m_MinAngle) + { + float temp = m_MinAngle; + m_MinAngle = m_MaxAngle; + m_MaxAngle = temp; + } - if (m_MaxAngle < m_MinAngle) { - float temp = m_MinAngle; - m_MinAngle = m_MaxAngle; - m_MaxAngle = temp; - } + int all_node_count = count; + LaserDebug debug; + + memset(&debug, 0, sizeof(debug)); + outscan.config.min_angle = math::from_degrees(m_MinAngle); + outscan.config.max_angle = math::from_degrees(m_MaxAngle); + //将首末点采集时间差作为采集时长 + // printf("stamp [%llu]-[%llu]\n", global_nodes[0].stamp, global_nodes[count - 1].stamp); + outscan.config.scan_time = static_cast((global_nodes[count - 1].stamp - global_nodes[0].stamp)) / 1e9; + // outscan.config.scan_time = static_cast(scan_time * 1.0 / 1e9); + outscan.config.time_increment = outscan.config.scan_time / (double)(count - 1); + outscan.config.min_range = m_MinRange; + outscan.config.max_range = m_MaxRange; + //将一圈中第一个点采集时间作为该圈数据采集时间 + if (global_nodes[0].stamp > 0) + outscan.stamp = global_nodes[0].stamp; + else + outscan.stamp = 0; + // outscan.stamp = tim_scan_start; - int all_node_count = count; - LaserDebug debug; - - memset(&debug, 0, sizeof(debug)); - outscan.config.min_angle = math::from_degrees(m_MinAngle); - outscan.config.max_angle = math::from_degrees(m_MaxAngle); - //将首末点采集时间差作为采集时长 -// printf("stamp [%llu]-[%llu]\n", global_nodes[0].stamp, global_nodes[count - 1].stamp); - outscan.config.scan_time = static_cast((global_nodes[count - 1].stamp - global_nodes[0].stamp)) / 1e9; -// outscan.config.scan_time = static_cast(scan_time * 1.0 / 1e9); - outscan.config.time_increment = outscan.config.scan_time / (double)(count - 1); - outscan.config.min_range = m_MinRange; - outscan.config.max_range = m_MaxRange; - //将一圈中第一个点采集时间作为该圈数据采集时间 - if (global_nodes[0].stamp > 0) - outscan.stamp = global_nodes[0].stamp; - else - outscan.stamp = 0; -// outscan.stamp = tim_scan_start; + float scanfrequency = 0.0; - float scanfrequency = 0.0; + if (m_FixedResolution) + { + all_node_count = m_FixedSize; + } - if (m_FixedResolution) { - all_node_count = m_FixedSize; - } + outscan.config.angle_increment = math::from_degrees(m_field_of_view) / + (all_node_count - 1); - outscan.config.angle_increment = math::from_degrees(m_field_of_view) / - (all_node_count - 1); + float range = 0.0; + float intensity = 0.0; + float angle = 0.0; + debug.MaxDebugIndex = 0; - float range = 0.0; - float intensity = 0.0; - float angle = 0.0; - debug.MaxDebugIndex = 0; + for (int i = 0; i < count; i++) + { + if (isNetTOFLidar(m_LidarType)) + { + angle = static_cast(global_nodes[i].angle_q6_checkbit / 100.0f) + + m_AngleOffset; + } + else + { + angle = static_cast((global_nodes[i].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / + 64.0f) + + m_AngleOffset; + } - for (int i = 0; i < count; i++) + if (isOctaveLidar(lidar_model) || + isOldVersionTOFLidar(lidar_model, Major, Minjor)) + { + range = static_cast(global_nodes[i].distance_q2 / 2000.f); + } + else + { + if (isTOFLidar(m_LidarType) || isNetTOFLidar(m_LidarType)) { - if (isNetTOFLidar(m_LidarType)) { - angle = static_cast(global_nodes[i].angle_q6_checkbit / 100.0f) + - m_AngleOffset; - } else { - angle = static_cast((global_nodes[i].angle_q6_checkbit >> - LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f) + m_AngleOffset; - } - - if (isOctaveLidar(lidar_model) || - isOldVersionTOFLidar(lidar_model, Major, Minjor)) { - range = static_cast(global_nodes[i].distance_q2 / 2000.f); - } else { - if (isTOFLidar(m_LidarType) || isNetTOFLidar(m_LidarType)) { - range = static_cast(global_nodes[i].distance_q2 / 1000.f); - } else { - range = static_cast(global_nodes[i].distance_q2 / 4000.f); - } - - } - - intensity = static_cast(global_nodes[i].sync_quality); - - // printf("intensity: %f\n", intensity); + range = static_cast(global_nodes[i].distance_q2 / 1000.f); + } + else + { + range = static_cast(global_nodes[i].distance_q2 / 4000.f); + } + } - angle = math::from_degrees(angle); + intensity = static_cast(global_nodes[i].sync_quality); - if (global_nodes[i].scan_frequence != 0) { - scanfrequency = global_nodes[i].scan_frequence / 10.0; + // printf("intensity: %f\n", intensity); - if (isTOFLidar(m_LidarType)) { - if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) { - scanfrequency = global_nodes[i].scan_frequence / 10.0 + 3.0; - } - } - } + angle = math::from_degrees(angle); - //Rotate 180 degrees or not - if (m_Reversion || isNetTOFLidar(m_LidarType)) { - angle = angle + M_PI; - } + if (global_nodes[i].scan_frequence != 0) + { + scanfrequency = global_nodes[i].scan_frequence / 10.0; - //Is it counter clockwise - if (m_Inverted) { - angle = 2 * M_PI - angle; - } + if (isTOFLidar(m_LidarType)) + { + if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) + { + scanfrequency = global_nodes[i].scan_frequence / 10.0 + 3.0; + } + } + } - angle = math::normalize_angle(angle); + // Rotate 180 degrees or not + if (m_Reversion || isNetTOFLidar(m_LidarType)) + { + angle = angle + M_PI; + } - //ignore angle - if (isRangeIgnore(angle)) { - range = 0.0; - } + // Is it counter clockwise + if (m_Inverted) + { + angle = 2 * M_PI - angle; + } - //valid range - if (!isRangeValid(range)) { - range = 0.0; - } + angle = math::normalize_angle(angle); - if (angle >= outscan.config.min_angle && - angle <= outscan.config.max_angle) { - LaserPoint point; - point.angle = angle; - point.range = range; - point.intensity = intensity; + // ignore angle + if (isRangeIgnore(angle)) + { + range = 0.0; + } -// if (outscan.points.empty()) { -// outscan.stamp = tim_scan_start + i * m_PointTime; -// } + // valid range + if (!isRangeValid(range)) + { + range = 0.0; + } - outscan.points.push_back(point); - } + if (angle >= outscan.config.min_angle && + angle <= outscan.config.max_angle) + { + LaserPoint point; + point.angle = angle; + point.range = range; + point.intensity = intensity; - parsePackageNode(global_nodes[i], debug); + // if (outscan.points.empty()) { + // outscan.stamp = tim_scan_start + i * m_PointTime; + // } - if (global_nodes[i].error_package) { - debug.MaxDebugIndex = 255; - } - } + outscan.points.push_back(point); + } - if (m_FixedResolution) { - outscan.points.resize(all_node_count); - } + parsePackageNode(global_nodes[i], debug); - //parsing version - handleVersionInfoByPackage(debug); - //resample sample rate - resample(scanfrequency, count, tim_scan_end, tim_scan_start); - return true; + if (global_nodes[i].error_package) + { + debug.MaxDebugIndex = 255; + } } - else + + if (m_FixedResolution) { - if (IS_FAIL(op_result)) { - // Error? Retry connection - } + outscan.points.resize(all_node_count); + } - if (lidarPtr->getDriverError() != NoError) { - fprintf(stderr, "[YDLIDAR ERROR]: %s\n", - DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); - fflush(stderr); - } + // parsing version + handleVersionInfoByPackage(debug); + // resample sample rate + resample(scanfrequency, count, tim_scan_end, tim_scan_start); + return true; + } + else + { + if (IS_FAIL(op_result)) + { + // Error? Retry connection + } - m_AllNode = 0; - m_FristNodeTime = tim_scan_start; + if (lidarPtr->getDriverError() != NoError) + { + fprintf(stderr, "[YDLIDAR ERROR]: %s\n", + DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); + fflush(stderr); } - return false; + m_AllNode = 0; + m_FristNodeTime = tim_scan_start; + } + + return false; } /*------------------------------------------------------------- - turnOff + turnOff -------------------------------------------------------------*/ -bool CYdLidar::turnOff() { - if (lidarPtr) { +bool CYdLidar::turnOff() +{ + if (lidarPtr) + { lidarPtr->stop(); } - if (isScanning) { + if (isScanning) + { printf("[YDLIDAR INFO] Now YDLIDAR Scanning has stopped ......\n"); fflush(stdout); } @@ -730,8 +801,10 @@ bool CYdLidar::turnOff() { /*------------------------------------------------------------- disconnecting -------------------------------------------------------------*/ -void CYdLidar::disconnecting() { - if (lidarPtr) { +void CYdLidar::disconnecting() +{ + if (lidarPtr) + { lidarPtr->disconnect(); delete lidarPtr; lidarPtr = nullptr; @@ -743,24 +816,28 @@ void CYdLidar::disconnecting() { /*------------------------------------------------------------- getAngleOffset -------------------------------------------------------------*/ -float CYdLidar::getAngleOffset() const { +float CYdLidar::getAngleOffset() const +{ return m_AngleOffset; } /*------------------------------------------------------------- isAngleOffsetCorrected -------------------------------------------------------------*/ -bool CYdLidar::isAngleOffsetCorrected() const { +bool CYdLidar::isAngleOffsetCorrected() const +{ return m_isAngleOffsetCorrected; } /*------------------------------------------------------------- DescribeError -------------------------------------------------------------*/ -const char *CYdLidar::DescribeError() const { +const char *CYdLidar::DescribeError() const +{ char const *value = ""; - if (lidarPtr) { + if (lidarPtr) + { return lidarPtr->DescribeError(); } @@ -770,10 +847,12 @@ const char *CYdLidar::DescribeError() const { /*------------------------------------------------------------- getDriverError -------------------------------------------------------------*/ -DriverError CYdLidar::getDriverError() const { +DriverError CYdLidar::getDriverError() const +{ DriverError er = UnknownError; - if (lidarPtr) { + if (lidarPtr) + { return lidarPtr->getDriverError(); } @@ -783,8 +862,10 @@ DriverError CYdLidar::getDriverError() const { /*------------------------------------------------------------- isRangeValid -------------------------------------------------------------*/ -bool CYdLidar::isRangeValid(double reading) const { - if (reading >= m_MinRange && reading <= m_MaxRange) { +bool CYdLidar::isRangeValid(double reading) const +{ + if (reading >= m_MinRange && reading <= m_MaxRange) + { return true; } @@ -794,12 +875,15 @@ bool CYdLidar::isRangeValid(double reading) const { /*------------------------------------------------------------- isRangeIgnore -------------------------------------------------------------*/ -bool CYdLidar::isRangeIgnore(double angle) const { +bool CYdLidar::isRangeIgnore(double angle) const +{ bool ret = false; - for (uint16_t j = 0; j < m_IgnoreArray.size(); j = j + 2) { + for (uint16_t j = 0; j < m_IgnoreArray.size(); j = j + 2) + { if ((math::from_degrees(m_IgnoreArray[j]) <= angle) && - (angle <= math::from_degrees(m_IgnoreArray[j + 1]))) { + (angle <= math::from_degrees(m_IgnoreArray[j + 1]))) + { ret = true; break; } @@ -811,15 +895,19 @@ bool CYdLidar::isRangeIgnore(double angle) const { /*------------------------------------------------------------- handleVersionInfoByPackage -------------------------------------------------------------*/ -void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug) { - if (m_parsingCompleted) { +void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug) +{ + if (m_parsingCompleted) + { return; } device_info info; - if (ParseLaserDebugInfo(debug, info)) { - if (printfVersionInfo(info, m_SerialPort, m_SerialBaudrate)) { + if (ParseLaserDebugInfo(debug, info)) + { + if (printfVersionInfo(info, m_SerialPort, m_SerialBaudrate)) + { std::string serial_number; Major = (uint8_t)(info.firmware_version >> 8); Minjor = (uint8_t)(info.firmware_version & 0xff); @@ -829,14 +917,14 @@ void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug) { m_LidarVersion.soft_patch = Minjor % 10; memcpy(&m_LidarVersion.sn[0], &info.serialnum[0], 16); - for (int i = 0; i < 16; i++) { + for (int i = 0; i < 16; i++) + { serial_number += std::to_string(info.serialnum[i] & 0xff); } m_SerialNumber = serial_number; m_parsingCompleted = true; } - } } @@ -844,189 +932,198 @@ void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug) { resample -------------------------------------------------------------*/ void CYdLidar::resample(int frequency, int count, uint64_t tim_scan_end, - uint64_t tim_scan_start) { + uint64_t tim_scan_start) +{ //重新校准采样率 - // if( (lidar_model == DriverInterface::YDLIDAR_TG15) - // || (lidar_model == DriverInterface::YDLIDAR_TG30) - // || (lidar_model == DriverInterface::YDLIDAR_TG50) ) - // { - // m_SampleRate = m_SampleRatebyD1; - // } + // if( (lidar_model == DriverInterface::YDLIDAR_TG15) + // || (lidar_model == DriverInterface::YDLIDAR_TG30) + // || (lidar_model == DriverInterface::YDLIDAR_TG50) ) + // { + // m_SampleRate = m_SampleRatebyD1; + // } if (frequency > 3 && frequency <= 15.7 && - (frequency - last_frequency) < 0.05) { + (frequency - last_frequency) < 0.05) + { int sample = static_cast((frequency * count + 500) / 1000); - if (sample != m_SampleRate) { + if (sample != m_SampleRate) + { } } last_frequency = frequency; int realSampleRate = 0; - if (m_AllNode != 0) { + if (m_AllNode != 0) + { realSampleRate = 1e9 * m_AllNode / (tim_scan_end - m_FristNodeTime); int RateDiff = std::abs(static_cast(realSampleRate - m_SampleRate * 1000)); if (RateDiff > 1000 || (static_cast(tim_scan_end - m_FristNodeTime) > 10 * 1e9 && - RateDiff > 30)) { + RateDiff > 30)) + { m_AllNode = 0; m_FristNodeTime = tim_scan_start; } } } - - /*------------------------------------------------------------- checkLidarAbnormal -------------------------------------------------------------*/ bool CYdLidar::checkLidarAbnormal() { - size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; - int check_abnormal_count = 0; + size_t count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + int check_abnormal_count = 0; + + if (m_AbnormalCheckCount < 2) + { + m_AbnormalCheckCount = 2; + } + + result_t op_result = RESULT_FAIL; + std::vector data; + int buffer_count = 0; - if (m_AbnormalCheckCount < 2) + while (check_abnormal_count < m_AbnormalCheckCount) + { + // Ensure that the voltage is insufficient or the motor resistance is high, causing an abnormality. + if (check_abnormal_count > 0) { - m_AbnormalCheckCount = 2; + delay(check_abnormal_count * 1000); } - result_t op_result = RESULT_FAIL; - std::vector data; - int buffer_count = 0; + float scan_time = 0.0; + uint64_t start_time = 0; + uint64_t end_time = 0; + op_result = RESULT_OK; - while (check_abnormal_count < m_AbnormalCheckCount) + while (buffer_count < 10 && + (scan_time < 0.05 || !lidarPtr->getSingleChannel()) && + IS_OK(op_result)) { - // Ensure that the voltage is insufficient or the motor resistance is high, causing an abnormality. - if (check_abnormal_count > 0) + start_time = getTime(); + count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + op_result = lidarPtr->grabScanData(global_nodes, count); + end_time = getTime(); + scan_time = 1.0 * static_cast(end_time - start_time) / 1e9; + buffer_count++; + + if (IS_OK(op_result)) + { + if (isNetTOFLidar(m_LidarType)) { - delay(check_abnormal_count * 1000); + return !IS_OK(op_result); } - float scan_time = 0.0; - uint64_t start_time = 0; - uint64_t end_time = 0; - op_result = RESULT_OK; - - while (buffer_count < 10 && (scan_time < 0.05 || !lidarPtr->getSingleChannel()) && IS_OK(op_result)) + if (CalculateSampleRate(count, scan_time)) { - start_time = getTime(); - count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; - op_result = lidarPtr->grabScanData(global_nodes, count); - end_time = getTime(); - scan_time = 1.0 * static_cast(end_time - start_time) / 1e9; - buffer_count++; - - if (IS_OK(op_result)) - { - if (isNetTOFLidar(m_LidarType)) - { - return !IS_OK(op_result); - } - - if (!lidarPtr->getSingleChannel()) - { - } - - if (CalculateSampleRate(count, scan_time)) - { - if (!lidarPtr->getSingleChannel()) - { - return !IS_OK(op_result); - } - } - } - else - { - check_abnormal_count++; - } + if (!lidarPtr->getSingleChannel()) + { + return !IS_OK(op_result); + } } + } + else + { + check_abnormal_count++; + } + } - if (IS_OK(op_result) && lidarPtr->getSingleChannel()) - { - data.push_back(count); - int collection = 0; + if (IS_OK(op_result) && lidarPtr->getSingleChannel()) + { + data.push_back(count); + int collection = 0; - while (collection < 5) - { - count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; - start_time = getTime(); - op_result = lidarPtr->grabScanData(global_nodes, count); - end_time = getTime(); - - if (IS_OK(op_result)) - { - if (isNetTOFLidar(m_LidarType)) - { - return !IS_OK(op_result); - } - - if (std::abs(static_cast(data.front() - count)) > 10) - { - data.erase(data.begin()); - } - - scan_time = 1.0 * static_cast(end_time - start_time) / 1e9; - bool ret = CalculateSampleRate(count, scan_time); - - if (scan_time > 0.05 && scan_time < 0.5 && lidarPtr->getSingleChannel()) - { - if (!ret) - { - m_SampleRate = static_cast((count / scan_time + 500) / 1000); - m_PointTime = 1e9 / (m_SampleRate * 1000); - lidarPtr->setPointTime(m_PointTime); - } - } - - data.push_back(count); - - if (ret) - { - break; - } - } - - collection++; - } + while (collection < 5) + { + count = ydlidar::YDlidarDriver::MAX_SCAN_NODES; + start_time = getTime(); + op_result = lidarPtr->grabScanData(global_nodes, count); + end_time = getTime(); - if (data.size() > 1) + if (IS_OK(op_result)) + { + if (isNetTOFLidar(m_LidarType)) + { + return !IS_OK(op_result); + } + + if (std::abs(static_cast(data.front() - count)) > 10) + { + data.erase(data.begin()); + } + + scan_time = 1.0 * static_cast(end_time - start_time) / 1e9; + bool ret = CalculateSampleRate(count, scan_time); + + if (scan_time > 0.05 && scan_time < 0.5 && lidarPtr->getSingleChannel()) + { + if (!ret) { - int total = accumulate(data.begin(), data.end(), 0); - int mean = total / data.size(); // mean value - m_FixedSize = (static_cast((mean + 5) / 10)) * 10; - printf("[YDLIDAR]:Single Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR]:Sample Rate: %dK\n", m_SampleRate); - return false; + m_SampleRate = static_cast((count / scan_time + 500) / 1000); + m_PointTime = 1e9 / (m_SampleRate * 1000); + lidarPtr->setPointTime(m_PointTime); } + } + + data.push_back(count); + + if (ret) + { + break; + } } - check_abnormal_count++; + collection++; + } + + if (data.size() > 1) + { + int total = accumulate(data.begin(), data.end(), 0); + int mean = total / data.size(); // mean value + m_FixedSize = (static_cast((mean + 5) / 10)) * 10; + printf("[YDLIDAR]:Single Fixed Size: %d\n", m_FixedSize); + printf("[YDLIDAR]:Sample Rate: %dK\n", m_SampleRate); + return false; + } } - return !IS_OK(op_result); + check_abnormal_count++; + } + + return !IS_OK(op_result); } /*------------------------------------------------------------- removeExceptionSample -------------------------------------------------------------*/ -inline void removeExceptionSample(std::map &smap) { - if (smap.size() < 2) { +inline void removeExceptionSample(std::map &smap) +{ + if (smap.size() < 2) + { return; } - std::map::iterator last = smap.begin(); - std::map::iterator its = smap.begin(); + std::map::iterator last = smap.begin(); + std::map::iterator its = smap.begin(); - while (its != smap.end()) { - if (last->second > its->second) { + while (its != smap.end()) + { + if (last->second > its->second) + { smap.erase(its++); - } else if (last->second < its->second) { + } + else if (last->second < its->second) + { its = smap.erase(last); last = its; its++; - } else { + } + else + { its++; } } @@ -1037,124 +1134,214 @@ inline void removeExceptionSample(std::map &smap) { -------------------------------------------------------------*/ bool CYdLidar::CalculateSampleRate(int count, double scan_time) { - if (count < 1) + if (count < 1) + return false; + + // 1、如果雷达支持直接获取采样率，则使用获取的采样率，此时将默认采样率值置为该值 + // 2、如果有设置默认采样率，判断当前雷达型号对应的默认采样率值的个数，个数为1， + //直接使用该采样率，个数不为1则根据实时采样率进行匹配 + // 3、如果没有设置默认采样率，则使用实时采样率 + int sr = 0; + bool ret = false; + + if (global_nodes[0].scan_frequence != 0) + { + //如果解析到转速信息，根据转速计算采样率 + double scanfrequency = global_nodes[0].scan_frequence / 10.0; + if (isTOFLidar(m_LidarType) && + !isOldVersionTOFLidar(lidar_model, Major, Minjor)) { - return false; + scanfrequency = global_nodes[0].scan_frequence / 10.0 + 3.0; } - - if (global_nodes[0].scan_frequence != 0) + sr = static_cast((count * scanfrequency + 500) / 1000); + } + else + { + //如果没有解析到转速信息，根据时间计算采样率 + if (scan_time > 0.04 && scan_time < 0.4) { - double scanfrequency; - scanfrequency = global_nodes[0].scan_frequence / 10.0; - - if (isTOFLidar(m_LidarType)) - { - if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) - { - scanfrequency = global_nodes[0].scan_frequence / 10.0 + 3.0; - } - } - - int samplerate = static_cast((count * scanfrequency + 500) / 1000); - int cnt = 0; - - if (SampleRateMap.find(samplerate) != SampleRateMap.end()) - { - cnt = SampleRateMap[samplerate]; - } + sr = static_cast((count / scan_time + 500) / 1000); + } + } - cnt++; - SampleRateMap[samplerate] = cnt; + printf("[YDLIDAR] Calc Sample Rate: %dK\n", sr); - if (isValidSampleRate(SampleRateMap) || defalutSampleRate == samplerate || - m_SampleRate == samplerate) + size_t size = defalutSampleRate.size(); + if (size) + { + if (size == 1) + { + sr = defalutSampleRate.front(); + ret = true; + // printf("[YDLIDAR] Calc Sample Rate1: %dK\n", sr); + } + else + { + float d = .0; + for (size_t i=0; isetPointTime(m_PointTime); - - if (!m_SingleChannel) - { - m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); - } - - printf("[YDLIDAR1]:Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR1]:Sample Rate: %dK\n", m_SampleRate); - return true; + //按权值2:8分 + d = defalutSampleRate.at(i + 1) - defalutSampleRate.at(i); + if (float(sr) <= float(defalutSampleRate.at(i) + 0.2 * d)) + { + sr = defalutSampleRate.at(i); + break; + } } else { - if (SampleRateMap.size() > 1) - { - SampleRateMap.clear(); - } + sr = defalutSampleRate.at(i); } + } + ret = true; + // printf("[YDLIDAR] Calc Sample Rate2: %dK\n", sr); } - else - { - if (scan_time > 0.04 && scan_time < 0.4) - { - int samplerate = static_cast((count / scan_time + 500) / 1000); + } + else + { + if (sr) + SampleRateMap[sr] ++; + if (isValidSampleRate(SampleRateMap)) + ret = true; + // printf("[YDLIDAR] Calc Sample Rate3: %dK\n", sr); + } - if (defalutSampleRate == samplerate || m_SampleRate == samplerate) - { - m_SampleRate = samplerate; - m_PointTime = 1e9 / (m_SampleRate * 1000); - lidarPtr->setPointTime(m_PointTime); - - if (!m_SingleChannel) - { - m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); - } - - printf("[YDLIDAR3]:Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR3]:Sample Rate: %dK\n", m_SampleRate); - return true; - } - else - { - int cnt = 0; - - if (SampleRateMap.find(samplerate) != SampleRateMap.end()) - { - cnt = SampleRateMap[samplerate]; - } - - cnt++; - SampleRateMap[samplerate] = cnt; - - if (SampleRateMap.size() > 1) - { - SampleRateMap.clear(); - } - - if (isValidSampleRate(SampleRateMap)) - { - m_SampleRate = samplerate; - m_PointTime = 1e9 / (m_SampleRate * 1000); - lidarPtr->setPointTime(m_PointTime); - - if (!m_SingleChannel) - { - m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); - } - - printf("[YDLIDAR2]:Fixed Size: %d\n", m_FixedSize); - printf("[YDLIDAR2]:Sample Rate: %dK\n", m_SampleRate); - return true; - } - } - } - } + if (ret) + { + m_SampleRate = sr; + m_PointTime = 1e9 / (m_SampleRate * 1000); + lidarPtr->setPointTime(m_PointTime); + if (!m_SingleChannel) + m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); + printf("[YDLIDAR] Fixed Size: %d\n", m_FixedSize); + printf("[YDLIDAR] Sample Rate: %dK\n", m_SampleRate); + } - return false; + return ret; + + // if (global_nodes[0].scan_frequence != 0) //如果解析到转速信息 + // { + // double scanfrequency; + // scanfrequency = global_nodes[0].scan_frequence / 10.0; + + // if (isTOFLidar(m_LidarType)) + // { + // if (!isOldVersionTOFLidar(lidar_model, Major, Minjor)) + // { + // scanfrequency = global_nodes[0].scan_frequence / 10.0 + 3.0; + // } + // } + + // int samplerate = static_cast((count * scanfrequency + 500) / 1000); + // int cnt = 0; + + // float sr = float(count * scanfrequency + 500) / 1000; + // printf("[CalculateSampleRate] Sample Rate: %.03fK(%dK)\n", sr, samplerate); + // // fflush(stdout); + + // if (SampleRateMap.find(samplerate) != SampleRateMap.end()) + // { + // cnt = SampleRateMap[samplerate]; + // } + + // cnt++; + // SampleRateMap[samplerate] = cnt; + + // if (isValidSampleRate(SampleRateMap) || + // defalutSampleRate == samplerate || + // m_SampleRate == samplerate) + // { + // m_SampleRate = samplerate; + // m_PointTime = 1e9 / (m_SampleRate * 1000); + // lidarPtr->setPointTime(m_PointTime); + + // if (!m_SingleChannel) + // { + // m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); + // } + + // printf("[YDLIDAR1]:Fixed Size: %d\n", m_FixedSize); + // printf("[YDLIDAR1]:Sample Rate: %dK\n", m_SampleRate); + // return true; + // } + // else + // { + // if (SampleRateMap.size() > 1) + // { + // SampleRateMap.clear(); + // } + // } + // } + // else //如果没有解析到转速信息 + // { + // if (scan_time > 0.04 && scan_time < 0.4) + // { + // int samplerate = static_cast((count / scan_time + 500) / 1000); + + // if (defalutSampleRate == samplerate || + // m_SampleRate == samplerate) + // { + // m_SampleRate = samplerate; + // m_PointTime = 1e9 / (m_SampleRate * 1000); + // lidarPtr->setPointTime(m_PointTime); + + // if (!m_SingleChannel) + // { + // m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); + // } + + // printf("[YDLIDAR3]:Fixed Size: %d\n", m_FixedSize); + // printf("[YDLIDAR3]:Sample Rate: %dK\n", m_SampleRate); + // return true; + // } + // else + // { + // int cnt = 0; + + // if (SampleRateMap.find(samplerate) != SampleRateMap.end()) + // { + // cnt = SampleRateMap[samplerate]; + // } + + // cnt++; + // SampleRateMap[samplerate] = cnt; + + // if (SampleRateMap.size() > 1) + // { + // SampleRateMap.clear(); + // } + + // if (isValidSampleRate(SampleRateMap)) + // { + // m_SampleRate = samplerate; + // m_PointTime = 1e9 / (m_SampleRate * 1000); + // lidarPtr->setPointTime(m_PointTime); + + // if (!m_SingleChannel) + // { + // m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); + // } + + // printf("[YDLIDAR2]:Fixed Size: %d\n", m_FixedSize); + // printf("[YDLIDAR2]:Sample Rate: %dK\n", m_SampleRate); + // return true; + // } + // } + // } + // } + + // return false; } /*------------------------------------------------------------- getDeviceHealth -------------------------------------------------------------*/ -bool CYdLidar::getDeviceHealth() { - if (!lidarPtr) { +bool CYdLidar::getDeviceHealth() +{ + if (!lidarPtr) + { return false; } @@ -1164,44 +1351,52 @@ bool CYdLidar::getDeviceHealth() { op_result = lidarPtr->getHealth(healthinfo, DriverInterface::DEFAULT_TIMEOUT / 2); - if (IS_OK(op_result)) { + if (IS_OK(op_result)) + { printf("[YDLIDAR]:Lidar running correctly ! The health status: %s\n", (int)healthinfo.status == 0 ? "good" : "bad"); - if (healthinfo.status == 2) { + if (healthinfo.status == 2) + { fprintf(stderr, "Error, YDLidar internal error detected. Please reboot the device to retry.\n"); return false; - } else { + } + else + { return true; } - - } else { + } + else + { fprintf(stderr, "Error, cannot retrieve YDLidar health code: %x\n", op_result); return false; } - } /*------------------------------------------------------------- getDeviceInfo -------------------------------------------------------------*/ -bool CYdLidar::getDeviceInfo() { - if (!lidarPtr) { +bool CYdLidar::getDeviceInfo() +{ + if (!lidarPtr) + { return false; } bool ret = false; device_info devinfo; result_t op_result = lidarPtr->getDeviceInfo(devinfo, - DriverInterface::DEFAULT_TIMEOUT / 2); + DriverInterface::DEFAULT_TIMEOUT / 2); - if (!IS_OK(op_result)) { + if (!IS_OK(op_result)) + { fprintf(stderr, "get Device Information Error\n"); return false; } - if (!isSupportLidar(devinfo.model)) { + if (!isSupportLidar(devinfo.model)) + { printf("[YDLIDAR INFO] Current SDK does not support current lidar models[%s]\n", lidarModelToString(devinfo.model).c_str()); return false; @@ -1209,15 +1404,20 @@ bool CYdLidar::getDeviceInfo() { { // check Lidar Type Config - if (isTOFLidarByModel(devinfo.model)) { - if (!isTOFLidar(m_LidarType)) { + if (isTOFLidarByModel(devinfo.model)) + { + if (!isTOFLidar(m_LidarType)) + { fprintf(stderr, "Incorrect Lidar Type setting...\n"); m_LidarType = TYPE_TOF; lidarPtr->setLidarType(m_LidarType); } - } else { + } + else + { if (!isTriangleLidar(m_LidarType) && !isNetTOFLidarByModel(devinfo.model) && - !m_SingleChannel) { + !m_SingleChannel) + { fprintf(stderr, "Incorrect Lidar Type setting, Reset Type to %d...\n", TYPE_TRIANGLE); m_LidarType = TYPE_TRIANGLE; @@ -1229,16 +1429,18 @@ bool CYdLidar::getDeviceInfo() { frequencyOffset = 0.4; lidar_model = devinfo.model; bool intensity = hasIntensity(devinfo.model); - defalutSampleRate = lidarModelDefaultSampleRate(devinfo.model); + defalutSampleRate = getDefaultSampleRate(devinfo.model); + // printf("getDefaultSampleRate %d\n", defalutSampleRate.size()); intensity = m_Intensity; std::string serial_number; lidarPtr->setIntensities(intensity); -// printf("Set Lidar Intensity Bit count %d\n", m_IntensityBit); + // printf("Set Lidar Intensity Bit count %d\n", m_IntensityBit); lidarPtr->setIntensityBit(m_IntensityBit); ret = true; - if (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate)) { + if (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate)) + { Major = (uint8_t)(devinfo.firmware_version >> 8); Minjor = (uint8_t)(devinfo.firmware_version & 0xff); m_LidarVersion.hardware = devinfo.hardware_version; @@ -1247,41 +1449,48 @@ bool CYdLidar::getDeviceInfo() { m_LidarVersion.soft_patch = Minjor % 10; memcpy(&m_LidarVersion.sn[0], &devinfo.serialnum[0], 16); - for (int i = 0; i < 16; i++) { + for (int i = 0; i < 16; i++) + { serial_number += std::to_string(devinfo.serialnum[i] & 0xff); } m_SerialNumber = serial_number; m_parsingCompleted = true; zero_offset_angle_scale = lidarZeroOffsetAngleScale(devinfo.model, - devinfo.firmware_version >> 8, devinfo.firmware_version & 0x00ff); + devinfo.firmware_version >> 8, devinfo.firmware_version & 0x00ff); } // uint32_t t = getms(); - if (hasSampleRate(devinfo.model)) { + if (hasSampleRate(devinfo.model)) + { checkSampleRate(); - } else { - m_PointTime = 1e9 / (defalutSampleRate * 1000); + } + else + { + m_PointTime = 1e9 / (defalutSampleRate.front() * 1000); lidarPtr->setPointTime(m_PointTime); } // printf("LiDAR get device info finished, Elapsed time %u ms\n", getms() - t); - if (hasScanFrequencyCtrl(devinfo.model) || ((isTOFLidar(m_LidarType)) && - !m_SingleChannel) || isNetTOFLidar(m_LidarType)) { + if (hasScanFrequencyCtrl(devinfo.model) || ((isTOFLidar(m_LidarType)) && !m_SingleChannel) || isNetTOFLidar(m_LidarType)) + { checkScanFrequency(); } - if (isSupportHeartBeat(devinfo.model)) { + if (isSupportHeartBeat(devinfo.model)) + { ret &= checkHeartBeat(); - if (!ret) { + if (!ret) + { fprintf(stderr, "Failed to Set HeartBeat[%d].\n", m_SupportHearBeat); } } - if (hasZeroAngle(devinfo.model)) { + if (hasZeroAngle(devinfo.model)) + { ret &= checkCalibrationAngle(serial_number); } @@ -1291,19 +1500,23 @@ bool CYdLidar::getDeviceInfo() { /*------------------------------------------------------------- handleSingleChannelDevice -------------------------------------------------------------*/ -void CYdLidar::handleSingleChannelDevice() { - if (!lidarPtr || !lidarPtr->getSingleChannel()) { +void CYdLidar::handleSingleChannelDevice() +{ + if (!lidarPtr || !lidarPtr->getSingleChannel()) + { return; } device_info devinfo; result_t op_result = lidarPtr->getDeviceInfo(devinfo); - if (!IS_OK(op_result)) { + if (!IS_OK(op_result)) + { return; } - if (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate)) { + if (printfVersionInfo(devinfo, m_SerialPort, m_SerialBaudrate)) + { m_parsingCompleted = true; m_LidarVersion.hardware = devinfo.hardware_version; m_LidarVersion.soft_major = Major; @@ -1313,37 +1526,33 @@ void CYdLidar::handleSingleChannelDevice() { } lidar_model = devinfo.model; -// if( (lidar_model == DriverInterface::YDLIDAR_TG15) -// || (lidar_model == DriverInterface::YDLIDAR_TG30) -// || (lidar_model == DriverInterface::YDLIDAR_TG50) ) -// { -// m_SampleRate = m_SampleRatebyD1; -// } - printf("[YDLIDAR INFO] Current Sampling Rate : %dK\n", m_SampleRate); + // defalutSampleRate = getDefaultSampleRate(devinfo.model); + + printf("[YDLIDAR INFO] Single Channel Current Sampling Rate: %dK\n", m_SampleRate); return; } /*------------------------------------------------------------- checkSampleRate -------------------------------------------------------------*/ -void CYdLidar::checkSampleRate() { - sampling_rate _rate; - _rate.rate = 3; - int _samp_rate = 9; +void CYdLidar::checkSampleRate() +{ + sampling_rate _rate = {0}; + int sr = 0; int try_count = 0; m_FixedSize = 1440; result_t ans = lidarPtr->getSamplingRate(_rate); - m_SampleRatebyD1 = _rate.rate; - - if (IS_OK(ans)) + if (IS_OK(ans)) { - _samp_rate = ConvertUserToLidarSmaple(lidar_model, m_SampleRate, _rate.rate); - + printf("[YDLIDAR] Get Origin Sample Rate: %uK\n", _rate.rate); if (!isTOFLidarByModel(lidar_model)) { - //非TOF雷达通过设备信息获取 - while (_samp_rate != _rate.rate) + //非TG系列雷达获取采样率码转成采样率值 + sr = ConvertUserToLidarSmaple(lidar_model, m_SampleRate, _rate.rate); + + //非TG系列雷达通过设备信息获取 + while (sr != _rate.rate) { ans = lidarPtr->setSamplingRate(_rate); try_count++; @@ -1353,52 +1562,68 @@ void CYdLidar::checkSampleRate() { break; } } + + sr = ConvertLidarToUserSmaple(lidar_model, _rate.rate); + } + else + { + //TG系列雷达直接获取采样率值 + sr = ConvertLidarToUserSmaple(lidar_model, _rate.rate); } - _samp_rate = ConvertLidarToUserSmaple(lidar_model, _rate.rate); + m_SampleRate = sr; + defalutSampleRate.clear(); + defalutSampleRate.push_back(m_SampleRate); + printf("[YDLIDAR] Get Sample Rate: %dK\n", m_SampleRate); } - - m_SampleRate = _samp_rate; - defalutSampleRate = m_SampleRate; } - /*------------------------------------------------------------- checkScanFrequency -------------------------------------------------------------*/ -bool CYdLidar::checkScanFrequency() { +bool CYdLidar::checkScanFrequency() +{ float frequency = 7.4f; scan_frequency _scan_frequency; float hz = 0.f; result_t ans = RESULT_FAIL; - if (isSupportScanFrequency(lidar_model, m_ScanFrequency)) { + if (isSupportScanFrequency(lidar_model, m_ScanFrequency)) + { m_ScanFrequency += frequencyOffset; - ans = lidarPtr->getScanFrequency(_scan_frequency) ; + ans = lidarPtr->getScanFrequency(_scan_frequency); - if (IS_OK(ans)) { + if (IS_OK(ans)) + { frequency = _scan_frequency.frequency / 100.f; hz = m_ScanFrequency - frequency; - if (hz > 0) { - while (hz > 0.95) { + if (hz > 0) + { + while (hz > 0.95) + { lidarPtr->setScanFrequencyAdd(_scan_frequency); hz = hz - 1.0; } - while (hz > 0.09) { + while (hz > 0.09) + { lidarPtr->setScanFrequencyAddMic(_scan_frequency); hz = hz - 0.1; } frequency = _scan_frequency.frequency / 100.0f; - } else { - while (hz < -0.95) { + } + else + { + while (hz < -0.95) + { lidarPtr->setScanFrequencyDis(_scan_frequency); hz = hz + 1.0; } - while (hz < -0.09) { + while (hz < -0.09) + { lidarPtr->setScanFrequencyDisMic(_scan_frequency); hz = hz + 0.1; } @@ -1406,7 +1631,9 @@ bool CYdLidar::checkScanFrequency() { frequency = _scan_frequency.frequency / 100.0f; } } - } else { + } + else + { m_ScanFrequency += frequencyOffset; fprintf(stderr, "current scan frequency[%f] is out of range.", m_ScanFrequency - frequencyOffset); @@ -1414,17 +1641,18 @@ bool CYdLidar::checkScanFrequency() { ans = lidarPtr->getScanFrequency(_scan_frequency); - if (IS_OK(ans)) { + if (IS_OK(ans)) + { frequency = _scan_frequency.frequency / 100.0f; m_ScanFrequency = frequency; } -// if( (lidar_model == DriverInterface::YDLIDAR_TG15) -// || (lidar_model == DriverInterface::YDLIDAR_TG30) -// || (lidar_model == DriverInterface::YDLIDAR_TG50) ) -// { -// m_SampleRate = m_SampleRatebyD1; -// } + // if( (lidar_model == DriverInterface::YDLIDAR_TG15) + // || (lidar_model == DriverInterface::YDLIDAR_TG30) + // || (lidar_model == DriverInterface::YDLIDAR_TG50) ) + // { + // m_SampleRate = m_SampleRatebyD1; + // } m_ScanFrequency -= frequencyOffset; m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1); @@ -1432,8 +1660,10 @@ bool CYdLidar::checkScanFrequency() { return true; } -bool CYdLidar::checkHeartBeat() { - if (!m_SupportHearBeat) { +bool CYdLidar::checkHeartBeat() +{ + if (!m_SupportHearBeat) + { lidarPtr->setHeartBeat(false); return true; } @@ -1442,10 +1672,12 @@ bool CYdLidar::checkHeartBeat() { scan_heart_beat beat; int retry = 0; - do { + do + { result_t ans = lidarPtr->setScanHeartbeat(beat); - if (IS_OK(ans) && !beat.enable) { + if (IS_OK(ans) && !beat.enable) + { ret = true; break; } @@ -1460,7 +1692,8 @@ bool CYdLidar::checkHeartBeat() { /*------------------------------------------------------------- checkCalibrationAngle -------------------------------------------------------------*/ -bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) { +bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) +{ bool ret = false; m_AngleOffset = 0.0; result_t ans = RESULT_FAIL; @@ -1468,15 +1701,19 @@ bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) { int retry = 0; m_isAngleOffsetCorrected = false; - while (retry < 2) { + while (retry < 2) + { ans = lidarPtr->getZeroOffsetAngle(angle); - if (IS_OK(ans)) { + if (IS_OK(ans)) + { if (angle.angle > zero_offset_angle_scale * 360 || - angle.angle < -zero_offset_angle_scale * 360) { + angle.angle < -zero_offset_angle_scale * 360) + { ans = lidarPtr->getZeroOffsetAngle(angle); - if (!IS_OK(ans)) { + if (!IS_OK(ans)) + { retry++; continue; } @@ -1485,8 +1722,7 @@ bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) { m_isAngleOffsetCorrected = (angle.angle != 180 * zero_offset_angle_scale); m_AngleOffset = angle.angle / zero_offset_angle_scale; ret = true; - printf("[YDLIDAR INFO] Successfully obtained the %s offset angle[%f] from the lidar[%s]\n" - , m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset, + printf("[YDLIDAR INFO] Successfully obtained the %s offset angle[%f] from the lidar[%s]\n", m_isAngleOffsetCorrected ? "corrected" : "uncorrrected", m_AngleOffset, serialNumber.c_str()); return ret; } @@ -1499,23 +1735,27 @@ bool CYdLidar::checkCalibrationAngle(const std::string &serialNumber) { return ret; } - - /*------------------------------------------------------------- - checkCOMMs + checkCOMMs -------------------------------------------------------------*/ -bool CYdLidar::checkCOMMs() { - if (!lidarPtr) { +bool CYdLidar::checkCOMMs() +{ + if (!lidarPtr) + { printf("YDLidar SDK initializing\n"); // create the driver instance - if (isNetTOFLidar(m_LidarType)) { - lidarPtr = new ydlidar::ETLidarDriver();//T15 - } else {//TG30 G4 + if (isNetTOFLidar(m_LidarType)) + { + lidarPtr = new ydlidar::ETLidarDriver(); // T15 + } + else + { // TG30 G4 lidarPtr = new ydlidar::YDlidarDriver(m_DeviceType); } - if (!lidarPtr) { + if (!lidarPtr) + { fprintf(stderr, "Create Driver fail\n"); return false; } @@ -1526,16 +1766,20 @@ bool CYdLidar::checkCOMMs() { lidarPtr->setSupportMotorDtrCtrl(m_SupportMotorDtrCtrl); } - if (lidarPtr->isconnected()) { + if (lidarPtr->isconnected()) + { return true; } // Is it COMX, X>4? -> "\\.\COMX" - if (m_SerialPort.size() >= 3) { + if (m_SerialPort.size() >= 3) + { if (tolower(m_SerialPort[0]) == 'c' && tolower(m_SerialPort[1]) == 'o' && - tolower(m_SerialPort[2]) == 'm') { + tolower(m_SerialPort[2]) == 'm') + { // Need to add "\\.\"? - if (m_SerialPort.size() > 4 || m_SerialPort[3] > '4') { + if (m_SerialPort.size() > 4 || m_SerialPort[3] > '4') + { m_SerialPort = std::string("\\\\.\\") + m_SerialPort; } } @@ -1544,17 +1788,20 @@ bool CYdLidar::checkCOMMs() { // make connection... result_t op_result = lidarPtr->connect(m_SerialPort.c_str(), m_SerialBaudrate); - if (!IS_OK(op_result)) { - if (isNetTOFLidar(m_LidarType)) { + if (!IS_OK(op_result)) + { + if (isNetTOFLidar(m_LidarType)) + { fprintf(stderr, "[CYdLidar] Error, cannot bind to the specified IP Address[%s]\n", m_SerialPort.c_str()); - } else { + } + else + { fprintf(stderr, "[CYdLidar] Error, cannot bind to the specified %s[%s] and %s[%d]\n", m_DeviceType != YDLIDAR_TYPE_SERIAL ? "IP Adddress" : "serial port", - m_SerialPort.c_str(), m_DeviceType != YDLIDAR_TYPE_SERIAL ? "network port" : - "baudrate", m_SerialBaudrate); + m_SerialPort.c_str(), m_DeviceType != YDLIDAR_TYPE_SERIAL ? "network port" : "baudrate", m_SerialBaudrate); } return false; @@ -1590,35 +1837,41 @@ bool CYdLidar::checkStatus() /*------------------------------------------------------------- checkHardware -------------------------------------------------------------*/ -bool CYdLidar::checkHardware() { - if (!lidarPtr) { +bool CYdLidar::checkHardware() +{ + if (!lidarPtr) + { return false; } - if (isScanning && lidarPtr->isscanning()) { + if (isScanning && lidarPtr->isscanning()) + { return true; } return false; } +namespace ydlidar +{ + void os_init() + { + ydlidar::core::base::init(); + } + bool os_isOk() + { + return ydlidar::core::base::ok(); + } -namespace ydlidar { -void os_init() { - ydlidar::core::base::init(); -} - -bool os_isOk() { - return ydlidar::core::base::ok(); -} - -void os_shutdown() { - ydlidar::core::base::shutdown(); -} + void os_shutdown() + { + ydlidar::core::base::shutdown(); + } -std::map lidarPortList() { - return ydlidar::YDlidarDriver::lidarPortList(); -} + std::map lidarPortList() + { + return ydlidar::YDlidarDriver::lidarPortList(); + } } diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 5889751..68ff67b 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -1056,7 +1056,8 @@ class YDLIDAR_API CYdLidar { uint64_t m_AllNode; ///< Sum of sampling points std::map SampleRateMap; ///< Sample Rate Map std::string m_SerialNumber; ///< LiDAR serial number - int defalutSampleRate; ///< LiDAR Default Sampling Rate + // int defalutSampleRate; ///< LiDAR Default Sampling Rate + std::vector defalutSampleRate; //默认采样率可能是多个值 bool m_parsingCompleted; ///< LiDAR Version Information is successfully parsed float m_field_of_view; ///< LiDAR Field of View Angle. LidarVersion m_LidarVersion; ///< LiDAR Version information diff --git a/src/ydlidar_driver.cpp b/src/ydlidar_driver.cpp index 58949d7..a9c7bc3 100644 --- a/src/ydlidar_driver.cpp +++ b/src/ydlidar_driver.cpp @@ -1578,7 +1578,7 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) { info = this->info_; return RESULT_OK; } - + //未获取到设备信息时，返回一个无效的设备信息 info.model = YDLIDAR_S2; info.firmware_version = 0; info.hardware_version = 0; @@ -1663,7 +1663,7 @@ void YDlidarDriver::setAutoReconnect(const bool &enable) { void YDlidarDriver::checkTransDelay() { //calc stamp trans_delay = _serial->getByteTime(); - sample_rate = lidarModelDefaultSampleRate(model) * 1000; + sample_rate = getDefaultSampleRate(model).front() * 1000; switch (model) { case YDLIDAR_G4://g4