diff --git a/CMakeLists.txt b/CMakeLists.txt index 2585d3d..681e373 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ project(ydlidar_sdk C CXX) ######################################################### # version set(YDLIDAR_SDK_VERSION_MAJOR 1) -set(YDLIDAR_SDK_VERSION_MINOR 0) -set(YDLIDAR_SDK_VERSION_PATCH 6) +set(YDLIDAR_SDK_VERSION_MINOR 1) +set(YDLIDAR_SDK_VERSION_PATCH 0) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/README.md b/README.md index 2c85c14..7343a8e 100644 --- a/README.md +++ b/README.md @@ -2,15 +2,16 @@ # Table of Contents -1. [Introduction](#introduction) +1. [Table of Contents](#table-of-contents) +2. [Introduction](#introduction) - [Prerequisites](#prerequisites) - [Supported Languages](#supported-languages) -2. [YDLidar SDK Communication Protocol](#ydlidar-sdk-communication-protocol) -3. [Architecture](#architecture) -4. [Installation](#installation) -5. [Documents](#documents) -6. [Support](#support) -7. [Contact EAI](#contact-eai) +3. [YDLidar SDK Communication Protocol](#ydlidar-sdk-communication-protocol) +4. [Architecture](#architecture) +5. [Installation](#installation) +6. [Documents](#documents) +7. [Support](#support) +8. [Contact EAI](#contact-eai) # Introduction diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h index 0d737f2..cc9ef7f 100644 --- a/core/common/DriverInterface.h +++ b/core/common/DriverInterface.h @@ -435,7 +435,7 @@ class DriverInterface { } public: - enum { + enum YDLIDAR_MODLES { YDLIDAR_F4 = 1,/**< F4 LiDAR Model. */ YDLIDAR_T1 = 2,/**< T1 LiDAR Model. */ YDLIDAR_F2 = 3,/**< F2 LiDAR Model. */ @@ -458,6 +458,8 @@ class DriverInterface { YDLIDAR_G5 = 20,/**< G5 LiDAR Model. */ YDLIDAR_G7 = 21,/**< G7 LiDAR Model. */ + YDLIDAR_GS2 = 51, //GS2雷达 + YDLIDAR_TG15 = 100,/**< TG15 LiDAR Model. */ YDLIDAR_TG30 = 101,/**< T30 LiDAR Model. */ YDLIDAR_TG50 = 102,/**< TG50 LiDAR Model. */ @@ -470,7 +472,7 @@ class DriverInterface { YDLIDAR_Tail, }; - enum { + 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 */ @@ -488,9 +490,9 @@ class DriverInterface { protected: /* Variable for LIDAR compatibility */ /// LiDAR Scanning state - bool m_isScanning; + bool m_isScanning = false; /// LiDAR connected state - bool m_isConnected; + bool m_isConnected = false; /// Scan Data Event Event _dataEvent; /// Data Locker diff --git a/core/common/ydlidar_datatype.h b/core/common/ydlidar_datatype.h index 3788bc4..e12c13b 100644 --- a/core/common/ydlidar_datatype.h +++ b/core/common/ydlidar_datatype.h @@ -110,4 +110,5 @@ typedef struct { std::vector points; /// Configuration of scan LaserConfig config; + int moduleNum; } LaserScan; diff --git a/core/common/ydlidar_def.h b/core/common/ydlidar_def.h index deaaa24..f7f3dff 100644 --- a/core/common/ydlidar_def.h +++ b/core/common/ydlidar_def.h @@ -45,6 +45,7 @@ typedef enum { TYPE_TOF = 0,/**< TG TX LiDAR.*/ TYPE_TRIANGLE = 1,/**< G4. G6. G2 LiDAR.*/ TYPE_TOF_NET = 2,/**< T15 LiDAR.*/ + TYPE_GS = 3, //GS2 TYPE_Tail, } LidarTypeID; @@ -191,7 +192,7 @@ typedef struct { /// System time when first range was measured in nanoseconds uint64_t stamp;///< ns /// Array of lidar points - uint32_t npoints; + uint32_t npoints; LaserPoint *points; /// Configuration of scan LaserConfig config; diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h index bba7c4d..0b5b09a 100644 --- a/core/common/ydlidar_help.h +++ b/core/common/ydlidar_help.h @@ -168,6 +168,10 @@ inline std::string lidarModelToString(int model) { name = "G7"; break; + case DriverInterface::YDLIDAR_GS2: + name = "GS2"; + break; + case DriverInterface::YDLIDAR_TG15: name = "TG15"; break; @@ -359,7 +363,8 @@ inline bool hasScanFrequencyCtrl(int model) { if (model == DriverInterface::YDLIDAR_S4 || model == DriverInterface::YDLIDAR_S4B || model == DriverInterface::YDLIDAR_S2 || - model == DriverInterface::YDLIDAR_X4) { + model == DriverInterface::YDLIDAR_X4 || + model == DriverInterface::YDLIDAR_GS2) { ret = false; } @@ -373,16 +378,16 @@ inline bool hasScanFrequencyCtrl(int model) { */ inline bool isSupportLidar(int model) { - if (model < DriverInterface::YDLIDAR_F4 || - (model > DriverInterface::YDLIDAR_G7 && - model < DriverInterface::YDLIDAR_TG15) || - (model > DriverInterface::YDLIDAR_Tmini && - model < DriverInterface::YDLIDAR_T15)) - { - return false; - } + if (model < DriverInterface::YDLIDAR_F4 || + (model > DriverInterface::YDLIDAR_G7 && + model < DriverInterface::YDLIDAR_GS2) || + (model > DriverInterface::YDLIDAR_Tmini && + model < DriverInterface::YDLIDAR_T15)) + { + return false; + } - return true; + return true; } /*! @@ -395,7 +400,8 @@ inline bool hasIntensity(int model) { if (model == DriverInterface::YDLIDAR_G2B || model == DriverInterface::YDLIDAR_G4B || - model == DriverInterface::YDLIDAR_S4B) { + model == DriverInterface::YDLIDAR_S4B || + model == DriverInterface::YDLIDAR_GS2) { ret = true; } @@ -525,6 +531,21 @@ inline bool isTriangleLidar(int type) { return ret; } +/** + * @brief Whether it is a GS type LiDAR + * @param type LiDAR type + * @return true if it is a Triangle type, otherwise false. + */ +inline bool isGSLidar(int type) { + bool ret = false; + + if (type == TYPE_GS) { + ret = true; + } + + return ret; +} + /** * @brief Whether it is Old Version protocol TOF LiDAR * @param model lidar model @@ -976,6 +997,16 @@ inline bool isV1Protocol(uint8_t protocol) { return false; } +//以16进制打印数据 +inline void printHex(const uint8_t *data, int size) +{ + if (!data) + return; + for (int i=0; i +#include +#include +#include +using namespace std; +using namespace ydlidar; + +#if defined(_MSC_VER) +#pragma comment(lib, "ydlidar_sdk.lib") +#endif + +/** + * @brief gs test + * @param argc + * @param argv + * @return + * @par Flow chart + * Step1: instance CYdLidar.\n + * Step2: set paramters.\n + * Step3: initialize SDK and LiDAR.(::CYdLidar::initialize)\n + * Step4: Start the device scanning routine which runs on a separate thread and enable motor.(::CYdLidar::turnOn)\n + * Step5: Get the LiDAR Scan Data.(::CYdLidar::doProcessSimple)\n + * Step6: Stop the device scanning thread and disable motor.(::CYdLidar::turnOff)\n + * Step7: Uninitialize the SDK and Disconnect the LiDAR.(::CYdLidar::disconnecting)\n + */ + +int main(int argc, char *argv[]) +{ + printf("__ ______ _ ___ ____ _ ____ \n"); + printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n"); + printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n"); + printf(" | | | |_| | |___ | || |_| / ___ \\| _ < \n"); + printf(" |_| |____/|_____|___|____/_/ \\_\\_| \\_\\ \n"); + printf("\n"); + fflush(stdout); + std::string port; + ydlidar::os_init(); + + std::map ports = ydlidar::lidarPortList(); + std::map::iterator it; + + if (ports.size() == 1) { + port = ports.begin()->second; + } else { + int id = 0; + + for (it = ports.begin(); it != ports.end(); it++) { + printf("%d. %s\n", id, it->first.c_str()); + id++; + } + + if (ports.empty()) { + printf("Not Lidar was detected. Please enter the lidar serial port:"); + std::cin >> port; + } else { + while (ydlidar::os_isOk()) { + printf("Please select the lidar port:"); + std::string number; + std::cin >> number; + + if ((size_t)atoi(number.c_str()) >= ports.size()) { + continue; + } + + it = ports.begin(); + id = atoi(number.c_str()); + + while (id) { + id--; + it++; + } + + port = it->second; + break; + } + } + } + + int baudrate = 921600; + std::map baudrateList; + baudrateList[0] = 921600; + printf("Baudrate:\n"); + for (std::map::iterator it = baudrateList.begin(); + it != baudrateList.end(); it++) { + printf("%d. %d\n", it->first, it->second); + } + + while (ydlidar::os_isOk()) + { + printf("Please select the lidar baudrate:"); + std::string number; + std::cin >> number; + + if ((size_t)atoi(number.c_str()) > baudrateList.size()) { + continue; + } + + baudrate = baudrateList[atoi(number.c_str())]; + break; + } + + if (!ydlidar::os_isOk()) { + return 0; + } + + bool isSingleChannel = false; + float frequency = 8.0; + + CYdLidar laser; + //////////////////////string property///////////////// + /// lidar port + laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size()); + /// ignore array + std::string ignore_array; + ignore_array.clear(); + laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(), + ignore_array.size()); + //////////////////////int property///////////////// + /// lidar baudrate + laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int)); + /// gs lidar + int optval = TYPE_GS; + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_SERIAL; + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = isSingleChannel ? 3 : 4; + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + /// Intenstiy bit count + optval = 8; + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = false; + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool)); + /// intensity + b_optvalue = true; + laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + /// Motor DTR + b_optvalue = true; + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + /// HeartBeat + b_optvalue = false; + laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool)); + + //////////////////////float property///////////////// + /// unit: ° + float f_optvalue = 180.0f; + laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); + f_optvalue = -180.0f; + laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); + /// unit: m + f_optvalue = 1.f; + laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); + f_optvalue = 0.025f; + laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); + /// unit: Hz + laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float)); + + bool ret = laser.initialize(); + + if (ret) { + ret = laser.turnOn(); + } else { + fprintf(stderr, "%s\n", laser.DescribeError()); + fflush(stderr); + } + + LaserScan scan; + + while (ret && ydlidar::os_isOk()) + { + if (laser.doProcessSimple(scan)) + { + printf("Scan received [%lu] points module num [%d]\n", + scan.points.size(), + scan.moduleNum); + + for (size_t i = 0; i < scan.points.size(); ++i) + { + const LaserPoint &p = scan.points.at(i); + printf("%lu a %.01f r %.01f\n", i, p.angle * 180.0f / M_PI, p.range * 1000.0f); + } + fflush(stdout); + } + else + { + fprintf(stderr, "Failed to get Lidar Data\n"); + fflush(stderr); + } + } + + laser.turnOff(); + laser.disconnecting(); + + return 0; +} diff --git a/samples/tof_test.cpp b/samples/tof_test.cpp index 5feab06..83646cd 100644 --- a/samples/tof_test.cpp +++ b/samples/tof_test.cpp @@ -266,7 +266,7 @@ int main(int argc, char *argv[]) { /// Turn On success and loop if (laser.doProcessSimple(scan)) { - fprintf(stdout, "Scan received at [%llu] %u points is [%f]s\n", + fprintf(stdout, "Scan received at [%lu] %u points is [%f]s\n", scan.stamp / 100000, (unsigned int)scan.points.size(), scan.config.scan_time); diff --git a/samples/ydlidar_test.cpp b/samples/ydlidar_test.cpp index 6cfccd2..a21c305 100644 --- a/samples/ydlidar_test.cpp +++ b/samples/ydlidar_test.cpp @@ -265,7 +265,7 @@ int main(int argc, char *argv[]) { { if (laser.doProcessSimple(scan)) { - printf("Scan received at [%llu] %u points is [%f]Hz\n", + printf("Scan received at [%lu] %u points is [%f]Hz\n", scan.stamp / 1000000, (unsigned int)scan.points.size(), 1.0 / scan.config.scan_time); diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index ceaeffc..54465ab 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -21,18 +21,19 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. // -#include "CYdLidar.h" -#include #include -#include #include #include -#include "ydlidar_driver.h" #include #include -#include -#include +#include "CYdLidar.h" +#include "core/math/angles.h" +#include "core/serial/common.h" +#include "core/common/DriverInterface.h" +#include "core/common/ydlidar_help.h" +#include "YDlidarDriver.h" #include "ETLidarDriver.h" +#include "GS2LidarDriver.h" using namespace std; using namespace impl; @@ -624,6 +625,8 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) outscan.config.time_increment = outscan.config.scan_time / (double)(count - 1); outscan.config.min_range = m_MinRange; outscan.config.max_range = m_MaxRange; + //模组编号 + outscan.moduleNum = global_nodes[0].index; //将一圈中第一个点采集时间作为该圈数据采集时间 if (global_nodes[0].stamp > 0) outscan.stamp = global_nodes[0].stamp; @@ -668,7 +671,9 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) } else { - if (isTOFLidar(m_LidarType) || isNetTOFLidar(m_LidarType)) + if (isTOFLidar(m_LidarType) || + isNetTOFLidar(m_LidarType) || + isGSLidar(m_LidarType)) { range = static_cast(global_nodes[i].distance_q2 / 1000.f); } @@ -678,9 +683,9 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) } } - intensity = static_cast(global_nodes[i].sync_quality); + // printf("%d %.03f\n", i, range); - // printf("intensity: %f\n", intensity); + intensity = static_cast(global_nodes[i].sync_quality); angle = math::from_degrees(angle); @@ -731,20 +736,15 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) point.range = range; point.intensity = intensity; - // if (outscan.points.empty()) { - // outscan.stamp = tim_scan_start + i * m_PointTime; - // } - outscan.points.push_back(point); } parsePackageNode(global_nodes[i], debug); - if (global_nodes[i].error_package) { debug.MaxDebugIndex = 255; } - } + } //end for (int i = 0; i < count; i++) if (m_FixedResolution) { @@ -1346,8 +1346,10 @@ bool CYdLidar::getDeviceHealth() } lidarPtr->stop(); + result_t op_result; device_health healthinfo; + memset(&healthinfo, 0, sizeof(device_health)); op_result = lidarPtr->getHealth(healthinfo, DriverInterface::DEFAULT_TIMEOUT / 2); @@ -1402,29 +1404,28 @@ bool CYdLidar::getDeviceInfo() return false; } + // check Lidar Type Config + if (isTOFLidarByModel(devinfo.model)) { - // check Lidar Type Config - if (isTOFLidarByModel(devinfo.model)) + if (!isTOFLidar(m_LidarType)) { - if (!isTOFLidar(m_LidarType)) - { - fprintf(stderr, "Incorrect Lidar Type setting...\n"); - m_LidarType = TYPE_TOF; - lidarPtr->setLidarType(m_LidarType); - } - } - else - { - if (!isTriangleLidar(m_LidarType) && !isNetTOFLidarByModel(devinfo.model) && - !m_SingleChannel) - { - fprintf(stderr, "Incorrect Lidar Type setting, Reset Type to %d...\n", - TYPE_TRIANGLE); - m_LidarType = TYPE_TRIANGLE; - lidarPtr->setLidarType(m_LidarType); - } + fprintf(stderr, "Incorrect Lidar Type setting...\n"); + m_LidarType = TYPE_TOF; + lidarPtr->setLidarType(m_LidarType); } } + else + { + // if (!isTriangleLidar(m_LidarType) && + // !isNetTOFLidarByModel(devinfo.model) && + // !m_SingleChannel) + // { + // fprintf(stderr, "Incorrect Lidar Type setting, Reset Type to %d...\n", + // TYPE_TRIANGLE); + // m_LidarType = TYPE_TRIANGLE; + // lidarPtr->setLidarType(m_LidarType); + // } + } frequencyOffset = 0.4; lidar_model = devinfo.model; @@ -1474,7 +1475,9 @@ bool CYdLidar::getDeviceInfo() // 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(); } @@ -1749,8 +1752,14 @@ bool CYdLidar::checkCOMMs() { lidarPtr = new ydlidar::ETLidarDriver(); // T15 } + else if (isGSLidar(m_LidarType)) + { + //GS2 + lidarPtr = new ydlidar::GS2LidarDriver(); + } else - { // TG30 G4 + { + //通用雷达 lidarPtr = new ydlidar::YDlidarDriver(m_DeviceType); } @@ -1819,11 +1828,6 @@ bool CYdLidar::checkCOMMs() -------------------------------------------------------------*/ bool CYdLidar::checkStatus() { - if (!checkCOMMs()) - { - return false; - } - getDeviceHealth(); if (!getDeviceInfo()) diff --git a/src/GS2LidarDriver.cpp b/src/GS2LidarDriver.cpp new file mode 100644 index 0000000..8274794 --- /dev/null +++ b/src/GS2LidarDriver.cpp @@ -0,0 +1,1645 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2018, EAIBOT, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#include +#include "GS2LidarDriver.h" +#include "core/serial/common.h" +#include "ydlidar_config.h" + +using namespace impl; + +namespace ydlidar { + +GS2LidarDriver::GS2LidarDriver(): + _serial(NULL) { + //串口配置参数 + m_intensities = false; + isAutoReconnect = true; + isAutoconnting = false; + m_baudrate = 230400; + isSupportMotorDtrCtrl = true; + scan_node_count = 0; + sample_rate = 5000; + m_PointTime = 1e9 / 5000; + trans_delay = 0; + scan_frequence = 0; + m_sampling_rate = -1; + model = -1; + retryCount = 0; + has_device_header = false; + m_SingleChannel = false; + m_LidarType = TYPE_TOF; + + //解析参数 + PackageSampleBytes = 2; + IntervalSampleAngle = 0.0; + CheckSum = 0; + CheckSumCal = 0; + SampleNumlAndCTCal = 0; + LastSampleAngleCal = 0; + CheckSumResult = true; + Valu8Tou16 = 0; + package_Sample_Num = 0; + moduleNum = 0; + frameNum = 0; + isPrepareToSend = false; + multi_package.resize(MaximumNumberOfPackages); + + last_device_byte = 0x00; + asyncRecvPos = 0; + async_size = 0; + headerBuffer = reinterpret_cast(&header_); + infoBuffer = reinterpret_cast(&info_); + healthBuffer = reinterpret_cast(&health_); + get_device_health_success = false; + get_device_info_success = false; + + package_Sample_Index = 0; + IntervalSampleAngle_LastPackage = 0.0; + globalRecvBuffer = new uint8_t[sizeof(gs2_node_package)]; + scan_node_buf = new node_info[MAX_SCAN_NODES]; + package_index = 0; + has_package_error = false; + isValidPoint = true; + bias[0] = 0; + bias[1] = 0; + bias[2] = 0; +} + +GS2LidarDriver::~GS2LidarDriver() +{ + m_isScanning = false; + + isAutoReconnect = false; + _thread.join(); + + ScopedLocker lk(_serial_lock); + + if (_serial) { + if (_serial->isOpen()) { + _serial->flush(); + _serial->closePort(); + } + } + + if (_serial) { + delete _serial; + _serial = NULL; + } + + if (globalRecvBuffer) { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + + if (scan_node_buf) { + delete[] scan_node_buf; + scan_node_buf = NULL; + } +} + +result_t GS2LidarDriver::connect(const char *port_path, uint32_t baudrate) +{ + ScopedLocker lk(_serial_lock); + m_baudrate = baudrate; + serial_port = string(port_path); + + if (!_serial) { + _serial = new serial::Serial(port_path, m_baudrate, + serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT)); + } + + { + ScopedLocker l(_lock); + + if (!_serial->open()) { + return RESULT_FAIL; + } + + m_isConnected = true; + } + + stopScan(); + delay(100); + clearDTR(); + + return RESULT_OK; +} + + +void GS2LidarDriver::setDTR() { + if (!m_isConnected) { + return ; + } + + if (_serial) { + _serial->setDTR(1); + } + +} + +void GS2LidarDriver::clearDTR() { + if (!m_isConnected) { + return ; + } + + if (_serial) { + _serial->setDTR(0); + } +} +void GS2LidarDriver::flushSerial() { + if (!m_isConnected) { + return; + } + + size_t len = _serial->available(); + if (len) { + _serial->readSize(len); + } + + delay(20); +} + + +void GS2LidarDriver::disconnect() { + isAutoReconnect = false; + + if (!m_isConnected) { + return ; + } + + stop(); + delay(10); + ScopedLocker l(_serial_lock); + + if (_serial) { + if (_serial->isOpen()) { + _serial->closePort(); + } + } + + m_isConnected = false; +} + + +void GS2LidarDriver::disableDataGrabbing() { + { + if (m_isScanning) { + m_isScanning = false; + _dataEvent.set(); + } + } + _thread.join(); +} + +bool GS2LidarDriver::isscanning() const { + return m_isScanning; +} +bool GS2LidarDriver::isconnected() const { + return m_isConnected; +} + +result_t GS2LidarDriver::sendCommand(uint8_t cmd, + const void *payload, + size_t payloadsize) +{ + return sendCommand(0x00, cmd, payload, payloadsize); +// uint8_t pkt_header[12]; +// cmd_packet_gs *header = reinterpret_cast(pkt_header); +// uint8_t checksum = 0; + +// if (!m_isConnected) { +// return RESULT_FAIL; +// } + +// header->syncByte0 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte1 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte2 = LIDAR_CMD_SYNC_BYTE; +// header->syncByte3 = LIDAR_CMD_SYNC_BYTE; +// header->address = 0x00; +// header->cmd_flag = cmd; +// header->size = 0xffff&payloadsize; +// sendData(pkt_header, 8) ; +// checksum += cmd; +// checksum += 0xff&header->size; +// checksum += 0xff&(header->size>>8); + +// if (payloadsize && payload) { +// for (size_t pos = 0; pos < payloadsize; ++pos) { +// checksum += ((uint8_t *)payload)[pos]; +// } +// uint8_t sizebyte = (uint8_t)(payloadsize); +// sendData((const uint8_t *)payload, sizebyte); +// } + +// sendData(&checksum, 1); + +// return RESULT_OK; +} + +result_t GS2LidarDriver::sendCommand(uint8_t addr, + uint8_t cmd, + const void *payload, + size_t payloadsize) +{ + uint8_t pkt_header[12]; + cmd_packet_gs *header = reinterpret_cast(pkt_header); + uint8_t checksum = 0; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + header->syncByte0 = LIDAR_CMD_SYNC_BYTE; + header->syncByte1 = LIDAR_CMD_SYNC_BYTE; + header->syncByte2 = LIDAR_CMD_SYNC_BYTE; + header->syncByte3 = LIDAR_CMD_SYNC_BYTE; + header->address = addr; + header->cmd_flag = cmd; + header->size = 0xffff&payloadsize; + sendData(pkt_header, 8) ; + checksum += cmd; + checksum += 0xff&header->size; + checksum += 0xff&(header->size>>8); + + if (payloadsize && payload) { + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum += ((uint8_t *)payload)[pos]; + } + uint8_t sizebyte = (uint8_t)(payloadsize); + sendData((const uint8_t *)payload, sizebyte); + } + + sendData(&checksum, 1); + + return RESULT_OK; +} + +result_t GS2LidarDriver::sendData(const uint8_t *data, size_t size) { + if (!_serial || !_serial->isOpen()) { + return RESULT_FAIL; + } + + if (data == NULL || size == 0) { + return RESULT_FAIL; + } + + size_t r; + + while (size) + { + r = _serial->writeData(data, size); + + if (!r) { + return RESULT_FAIL; + } + + printf("send: "); + printHex(data, r); + + size -= r; + data += r; + } + + return RESULT_OK; +} + +result_t GS2LidarDriver::getData(uint8_t *data, size_t size) { + if (!_serial || !_serial->isOpen()) { + return RESULT_FAIL; + } + + size_t r; + + while (size) { + r = _serial->readData(data, size); + + if (r < 1) { + return RESULT_FAIL; + } + +// printf("recv: "); +// printHex(data, r); + + size -= r; + data += r; + } + + return RESULT_OK; +} + +result_t GS2LidarDriver::waitResponseHeader(gs_lidar_ans_header *header, + uint32_t timeout) { + int recvPos = 0; + uint32_t startTs = getms(); + uint8_t recvBuffer[sizeof(gs_lidar_ans_header)]; + uint8_t *headerBuffer = reinterpret_cast(header); + uint32_t waitTime = 0; + has_device_header = false; + last_device_byte = 0x00; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(gs_lidar_ans_header) - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) { + return ans; + } + + if (recvSize > remainSize) { + recvSize = remainSize; + } + + ans = getData(recvBuffer, recvSize); + + if (IS_FAIL(ans)) { + return RESULT_FAIL; + } + + for (size_t pos = 0; pos < recvSize; ++pos) { + uint8_t currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 2: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 3: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + has_device_header = true; + break; + + default: + break; + } + + headerBuffer[recvPos++] = currentByte; + last_device_byte = currentByte; + + if (has_device_header && recvPos == sizeof(gs_lidar_ans_header)) { + return RESULT_OK; + } + } + } + + return RESULT_FAIL; +} + +result_t GS2LidarDriver::waitResponseHeaderEx(gs_lidar_ans_header *header, uint8_t cmd, uint32_t timeout) +{ + int recvPos = 0; + uint32_t startTs = getms(); + uint8_t recvBuffer[sizeof(gs_lidar_ans_header)]; + uint8_t *headerBuffer = reinterpret_cast(header); + uint32_t waitTime = 0; + has_device_header = false; + last_device_byte = 0x00; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = sizeof(gs_lidar_ans_header) - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) { + return ans; + } + + if (recvSize > remainSize) { + recvSize = remainSize; + } + + ans = getData(recvBuffer, recvSize); + + if (IS_FAIL(ans)) { + return RESULT_FAIL; + } + + for (size_t pos = 0; pos < recvSize; ++pos) { + uint8_t currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 2: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + + case 3: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) { + recvPos = 0; + continue; + } + break; + case 5: + if (currentByte != cmd) { + recvPos = 0; + continue; + } + has_device_header = true; + break; + + default: + break; + } + + headerBuffer[recvPos++] = currentByte; + last_device_byte = currentByte; + + if (has_device_header && recvPos == sizeof(gs_lidar_ans_header)) { + return RESULT_OK; + } + } + } + + return RESULT_FAIL; +} + +result_t GS2LidarDriver::waitForData(size_t data_count, uint32_t timeout, + size_t *returned_size) { + size_t length = 0; + + if (returned_size == NULL) { + returned_size = (size_t *)&length; + } + + return (result_t)_serial->waitfordata(data_count, timeout, returned_size); +} + +result_t GS2LidarDriver::checkAutoConnecting() { + result_t ans = RESULT_FAIL; + isAutoconnting = true; + + while (isAutoReconnect && isAutoconnting) { + { + ScopedLocker l(_serial_lock); + + if (_serial) { + if (_serial->isOpen() || m_isConnected) { + m_isConnected = false; + _serial->closePort(); + delete _serial; + _serial = NULL; + } + } + } + retryCount++; + + if (retryCount > 100) { + retryCount = 100; + } + + delay(100 * retryCount); + int retryConnect = 0; + + while (isAutoReconnect && + connect(serial_port.c_str(), m_baudrate) != RESULT_OK) { + retryConnect++; + + if (retryConnect > 25) { + retryConnect = 25; + } + + delay(200 * retryConnect); + } + + if (!isAutoReconnect) { + m_isScanning = false; + return RESULT_FAIL; + } + + if (isconnected()) { + delay(100); + { + ScopedLocker l(_serial_lock); + ans = startAutoScan(); + + if (!IS_OK(ans)) { + ans = startAutoScan(); + } + } + + if (IS_OK(ans)) { + isAutoconnting = false; + return ans; + } + } + } + + return RESULT_FAIL; + +} + +int GS2LidarDriver::cacheScanData() { + node_info local_buf[200]; + size_t count = 200; + node_info local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + result_t ans = RESULT_FAIL; + memset(local_scan, 0, sizeof(local_scan)); + + flushSerial(); + waitScanData(local_buf, count); + + int timeout_count = 0; + retryCount = 0; + + while (m_isScanning) + { + count = 160; + ans = waitScanData(local_buf, count); + + if (!IS_OK(ans)) { + if (IS_FAIL(ans) || timeout_count > DEFAULT_TIMEOUT_COUNT) { + if (!isAutoReconnect) { + fprintf(stderr, "exit scanning thread!!\n"); + fflush(stderr); + { + m_isScanning = false; + } + return RESULT_FAIL; + } else { + ans = checkAutoConnecting(); + + if (IS_OK(ans)) { + timeout_count = 0; + local_scan[0].sync_flag = Node_NotSync; + } else { + m_isScanning = false; + return RESULT_FAIL; + } + } + } else { + timeout_count++; + local_scan[0].sync_flag = Node_NotSync; + fprintf(stderr, "timout count: %d\n", timeout_count); + fflush(stderr); + } + } else { + timeout_count = 0; + retryCount = 0; + } + + printf("sync:%d,index:%d,moduleNum:%d\n",package_type,frameNum,moduleNum); + fflush(stdout); + + if(!isPrepareToSend){ + continue; + } + + size_t size = multi_package.size(); + for(size_t i = 0;i < size; i++){ + if(multi_package[i].frameNum == frameNum && multi_package[i].moduleNum == moduleNum){ + memcpy(scan_node_buf,multi_package[i].all_points,sizeof (node_info) * 160); + break; + } + } + + _lock.lock();//timeout lock, wait resource copys + scan_node_buf[0].stamp = local_buf[count - 1].stamp; + scan_node_buf[0].scan_frequence = local_buf[count - 1].scan_frequence; + scan_node_buf[0].index = moduleNum >> 1;//gs2: 1, 2, 4 + scan_node_count = 160; //一个包固定160个数据 + printf("send frameNum: %d,moduleNum: %d\n",frameNum,moduleNum); + fflush(stdout); + _dataEvent.set(); + _lock.unlock(); + scan_count = 0; + isPrepareToSend = false; + } + + m_isScanning = false; + + return RESULT_OK; +} + +result_t GS2LidarDriver::waitPackage(node_info *node, uint32_t timeout) +{ + int recvPos = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + uint8_t *packageBuffer = (uint8_t *)&package; + isValidPoint = true; + int package_recvPos = 0; + uint16_t sample_lens = 0; + has_device_header = false; + uint16_t package_Sample_Num = 0; + + (*node).index = 255; + (*node).scan_frequence = 0; + + if (package_Sample_Index == 0) + { + recvPos = 0; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = PackagePaidBytes_GS - recvPos; + size_t recvSize = 0; + CheckSumCal = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) + { + return ans; + } + + if (recvSize > remainSize) + { + recvSize = remainSize; + } + + getData(globalRecvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) + { + uint8_t currentByte = globalRecvBuffer[pos]; + switch (recvPos) + { + case 0: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) + { + recvPos = 0; + continue; + } + break; + + case 1: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) + { + recvPos = 0; + continue; + } + break; + + case 2: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) + { + recvPos = 0; + continue; + } + break; + + case 3: + if (currentByte != LIDAR_ANS_SYNC_BYTE1) + { + recvPos = 0; + continue; + } + has_device_header = true; + break; + + case 4: + moduleNum = currentByte; + CheckSumCal += currentByte; + break; + + case 5: + if (currentByte != GS_LIDAR_ANS_SCAN) + { + recvPos = 0; + CheckSumCal = 0; + moduleNum = 0; + has_device_header = false; + continue; + } + CheckSumCal += currentByte; + break; + + case 6: + sample_lens |= 0x00ff & currentByte; + CheckSumCal += currentByte; + break; + + case 7: + sample_lens |= (0x00ff & currentByte) << 8; + CheckSumCal += currentByte; + break; + + default: + break; + } + + packageBuffer[recvPos++] = currentByte; + } + + if (has_device_header && + recvPos == PackagePaidBytes_GS) + { + if (!sample_lens) + { + recvPos = 0; + continue; + } + package_Sample_Num = sample_lens + 1; //环境2Bytes + 点云320Bytes + CRC + package_recvPos = recvPos; + printf("sample num %d\n", (package_Sample_Num - 3) / 2); + break; + } + else + { + recvPos = 0; + printf("invalid data\n"); + continue; + } + } + + if (PackagePaidBytes_GS == recvPos) + { + startTs = getms(); + recvPos = 0; + + while ((waitTime = getms() - startTs) <= timeout) + { + size_t remainSize = package_Sample_Num - recvPos; + size_t recvSize = 0; + result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + + if (!IS_OK(ans)) + { + return ans; + } + + if (recvSize > remainSize) + { + recvSize = remainSize; + } + + getData(globalRecvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize - 1; pos++) + { + CheckSumCal += globalRecvBuffer[pos]; + packageBuffer[package_recvPos + recvPos] = globalRecvBuffer[pos]; + recvPos++; + } + CheckSum = globalRecvBuffer[recvSize - 1]; // crc + packageBuffer[package_recvPos + recvPos] = CheckSum; // crc + recvPos += 1; + + if (package_Sample_Num == recvPos) + { + package_recvPos += recvPos; + break; + } + } + + if (package_Sample_Num != recvPos) + { + return RESULT_FAIL; + } + } + else + { + return RESULT_FAIL; + } + + if (CheckSumCal != CheckSum) + { + CheckSumResult = false; + has_package_error = true; + } + else + { + CheckSumResult = true; + } + } + + if (!has_package_error) + { + if (package_Sample_Index == 0) + { + package_index++; + (*node).index = package_index; + } + } + else + { + (*node).index = 255; + package_index = 0xff; + } + + if (CheckSumResult) + { + (*node).index = package_index; + (*node).scan_frequence = scan_frequence; + } + + (*node).sync_quality = Node_Default_Quality; + (*node).stamp = 0; + (*node).scan_frequence = 0; + + double sampleAngle = 0; + if (CheckSumResult) + { + (*node).distance_q2 = + package.packageSample[package_Sample_Index].PakageSampleDistance; + + if (m_intensities) + { + (*node).sync_quality = (uint16_t)package.packageSample[package_Sample_Index].PakageSampleQuality; + } + + if (node->distance_q2 > 0) + { + angTransform((*node).distance_q2, package_Sample_Index, &sampleAngle, &(*node).distance_q2); + } + + // printf("%lf ", sampleAngle); + if (sampleAngle < 0) + { + (*node).angle_q6_checkbit = (((uint16_t)(sampleAngle * 64 + 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } + else + { + if ((sampleAngle * 64) > 23040) + { + (*node).angle_q6_checkbit = (((uint16_t)(sampleAngle * 64 - 23040)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } + else + { + (*node).angle_q6_checkbit = (((uint16_t)(sampleAngle * 64)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + + LIDAR_RESP_MEASUREMENT_CHECKBIT; + } + } + + //过滤左右相机超过0°的点 + if (package_Sample_Index < 80) + { // CT_RingStart CT_Normal + if ((*node).angle_q6_checkbit <= 23041) + { + (*node).distance_q2 = 0; + isValidPoint = false; + } + } + else + { + if ((*node).angle_q6_checkbit > 23041) + { + (*node).distance_q2 = 0; + isValidPoint = false; + } + } + + // printf("%u %u %u\n", package_Sample_Index, node->angle_q6_checkbit, node->distance_q2); + } + else + { + (*node).sync_flag = Node_NotSync; + (*node).sync_quality = Node_Default_Quality; + (*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT; + (*node).distance_q2 = 0; + (*node).scan_frequence = 0; + } + + uint8_t nowPackageNum = 160; + + package_Sample_Index++; + (*node).sync_flag = Node_NotSync; + + if (package_Sample_Index >= nowPackageNum) + { + package_Sample_Index = 0; + (*node).sync_flag = Node_Sync; + CheckSumResult = false; + } + + return RESULT_OK; +} + +void GS2LidarDriver::angTransform(uint16_t dist, int n, double *dstTheta, uint16_t *dstDist) +{ + double pixelU = n, Dist, theta, tempTheta, tempDist, tempX, tempY; + uint8_t mdNum = 0x03 & (moduleNum >> 1);//1,2,4 + if (n < 80) + { + pixelU = 80 - pixelU; + if (d_compensateB0[mdNum] > 1) { + tempTheta = d_compensateK0[mdNum] * pixelU - d_compensateB0[mdNum]; + } + else + { + tempTheta = atan(d_compensateK0[mdNum] * pixelU - d_compensateB0[mdNum]) * 180 / M_PI; + } + tempDist = (dist - Angle_Px) / cos(((Angle_PAngle + bias[mdNum]) - (tempTheta)) * M_PI / 180); + tempTheta = tempTheta * M_PI / 180; + tempX = cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * + sin(tempTheta)); + tempY = -sin((Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + cos((Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * + sin(tempTheta)); + tempX = tempX + Angle_Px; + tempY = tempY - Angle_Py; //5.315 + Dist = sqrt(tempX * tempX + tempY * tempY); + theta = atan(tempY / tempX) * 180 / M_PI; + } + else + { + pixelU = 160 - pixelU; + if (d_compensateB1[mdNum] > 1) + { + tempTheta = d_compensateK1[mdNum] * pixelU - d_compensateB1[mdNum]; + } + else + { + tempTheta = atan(d_compensateK1[mdNum] * pixelU - d_compensateB1[mdNum]) * 180 / M_PI; + } + tempDist = (dist - Angle_Px) / cos(((Angle_PAngle + bias[mdNum]) + (tempTheta)) * M_PI / 180); + tempTheta = tempTheta * M_PI / 180; + tempX = cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * + sin(tempTheta)); + tempY = -sin(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * tempDist * cos(tempTheta) + cos(-(Angle_PAngle + bias[mdNum]) * M_PI / 180) * (tempDist * + sin(tempTheta)); + tempX = tempX + Angle_Px; + tempY = tempY + Angle_Py; //5.315 + Dist = sqrt(tempX * tempX + tempY * tempY); + theta = atan(tempY / tempX) * 180 / M_PI; + } + if (theta < 0) + { + theta += 360; + } + *dstTheta = theta; + *dstDist = Dist; +} + +void GS2LidarDriver::addPointsToVec(node_info *nodebuffer, size_t &count){ + size_t size = multi_package.size(); + bool isFound = false; + for(size_t i =0;i < size; i++){ + if(multi_package[i].frameNum == frameNum && multi_package[i].moduleNum == moduleNum){ + isFound = true; + memcpy(multi_package[i].all_points,nodebuffer,sizeof (node_info) * count); + isPrepareToSend = true; + if(frameNum > 0){ + int lastFrame = frameNum - 1; + for(size_t j =0;j < size; j++){ + if(multi_package[j].frameNum == lastFrame && multi_package[j].moduleNum == moduleNum){ + break; + } + } + } + break; + } + } + if(!isFound){ + GS2_Multi_Package package; + package.frameNum = frameNum; + package.moduleNum = moduleNum; + multi_package.push_back(package); + } + // printf("add points, [sync:%d] [%u]\n",package_type,frameNum); + // fflush(stdout); +} + +result_t GS2LidarDriver::waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout) { + if (!m_isConnected) { + count = 0; + return RESULT_FAIL; + } + + size_t recvNodeCount = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_FAIL; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) + { + node_info node; + memset(&node, 0, sizeof(node_info)); + ans = waitPackage(&node, timeout - waitTime); + + if (!IS_OK(ans)) { + count = recvNodeCount; + return ans; + } + nodebuffer[recvNodeCount++] = node; +// printf("%d ", node.distance_q2); + + if (!package_Sample_Index) + { +// printf("\n"); + + size_t size = _serial->available(); + uint64_t delayTime = 0; + size_t PackageSize = NORMAL_PACKAGE_SIZE; + + if (size > PackagePaidBytes_GS) { + size_t packageNum = size / PackageSize; + size_t Number = size % PackageSize; + delayTime = packageNum * m_PointTime * PackageSize / 2; + + if (Number > PackagePaidBytes_GS) { + delayTime += m_PointTime * ((Number - PackagePaidBytes_GS) / 2); + } + + size = Number; + + if (packageNum > 0 && Number == 0) { + size = PackageSize; + } + } + addPointsToVec(nodebuffer,recvNodeCount); + + nodebuffer[recvNodeCount - 1].stamp = size * trans_delay + delayTime; + nodebuffer[recvNodeCount - 1].scan_frequence = node.scan_frequence; + count = recvNodeCount; + return RESULT_OK; + } + + if (recvNodeCount == count) { + return RESULT_OK; + } + } + + count = recvNodeCount; + return RESULT_FAIL; +} + + +result_t GS2LidarDriver::grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout) { + switch (_dataEvent.wait(timeout)) { + case Event::EVENT_TIMEOUT: + count = 0; + return RESULT_TIMEOUT; + + case Event::EVENT_OK: { + if (scan_node_count == 0) { + return RESULT_FAIL; + } + + ScopedLocker l(_lock); + size_t size_to_copy = min(count, scan_node_count); + memcpy(nodebuffer, scan_node_buf, size_to_copy * sizeof(node_info)); + count = size_to_copy; + scan_node_count = 0; + } + + return RESULT_OK; + + default: + count = 0; + return RESULT_FAIL; + } + +} + + +result_t GS2LidarDriver::ascendScanData(node_info *nodebuffer, size_t count) { + float inc_origin_angle = (float)360.0 / count; + int i = 0; + + for (i = 0; i < (int)count; i++) { + if (nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while (i != 0) { + i--; + float expect_angle = (nodebuffer[i + 1].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / + 64.0f - inc_origin_angle; + + if (expect_angle < 0.0f) { + expect_angle = 0.0f; + } + + uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & + LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + if (i == (int)count) { + return RESULT_FAIL; + } + + for (i = (int)count - 1; i >= 0; i--) { + if (nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while (i != ((int)count - 1)) { + i++; + float expect_angle = (nodebuffer[i - 1].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / + 64.0f + inc_origin_angle; + + if (expect_angle > 360.0f) { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & + LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + + break; + } + } + + float frontAngle = (nodebuffer[0].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f; + + for (i = 1; i < (int)count; i++) { + if (nodebuffer[i].distance_q2 == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + + if (expect_angle > 360.0f) { + expect_angle -= 360.0f; + } + + uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & + LIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + } + + size_t zero_pos = 0; + float pre_degree = (nodebuffer[0].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f; + + for (i = 1; i < (int)count ; ++i) { + float degree = (nodebuffer[i].angle_q6_checkbit >> + LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f; + + if (zero_pos == 0 && (pre_degree - degree > 180)) { + zero_pos = i; + break; + } + + pre_degree = degree; + } + + node_info *tmpbuffer = new node_info[count]; + + for (i = (int)zero_pos; i < (int)count; i++) { + tmpbuffer[i - zero_pos] = nodebuffer[i]; + } + + for (i = 0; i < (int)zero_pos; i++) { + tmpbuffer[i + (int)count - zero_pos] = nodebuffer[i]; + } + + memcpy(nodebuffer, tmpbuffer, count * sizeof(node_info)); + delete[] tmpbuffer; + + return RESULT_OK; +} + +/************************************************************************/ +/* get device parameters of gs lidar */ +/************************************************************************/ +result_t GS2LidarDriver::getDevicePara(gs_device_para &info, uint32_t timeout) { + result_t ans; + uint8_t crcSum, mdNum; + uint8_t *pInfo = reinterpret_cast(&info); + + if (!m_isConnected) { + return RESULT_FAIL; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_lock); + + if ((ans = sendCommand(GS_LIDAR_CMD_GET_PARAMETER)) != RESULT_OK) { + return ans; + } + gs_lidar_ans_header response_header; + for(int i = 0; i < PackageMaxModuleNums; i++) + { + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) { + return ans; + } + if (response_header.type != GS_LIDAR_CMD_GET_PARAMETER) { + return RESULT_FAIL; + } + if (response_header.size < (sizeof(gs_device_para) - 1)) { + return RESULT_FAIL; + } + if (waitForData(response_header.size+1, timeout) != RESULT_OK) { + return RESULT_FAIL; + } + getData(reinterpret_cast(&info), sizeof(info)); + + crcSum = 0; + crcSum += response_header.address; + crcSum += response_header.type; + crcSum += 0xff & response_header.size; + crcSum += 0xff & (response_header.size >> 8); + for(int j = 0; j < response_header.size; j++) { + crcSum += pInfo[j]; + } + if(crcSum != info.crc) { + return RESULT_FAIL; + } + + mdNum = response_header.address >> 1; // 1,2,4 + if( mdNum > 2) { + return RESULT_FAIL; + } + u_compensateK0[mdNum] = info.u_compensateK0; + u_compensateK1[mdNum] = info.u_compensateK1; + u_compensateB0[mdNum] = info.u_compensateB0; + u_compensateB1[mdNum] = info.u_compensateB1; + d_compensateK0[mdNum] = info.u_compensateK0 / 10000.00; + d_compensateK1[mdNum] = info.u_compensateK1 / 10000.00; + d_compensateB0[mdNum] = info.u_compensateB0 / 10000.00; + d_compensateB1[mdNum] = info.u_compensateB1 / 10000.00; + bias[mdNum] = double(info.bias) * 0.1; + delay(5); + } + } + + return RESULT_OK; +} + +result_t GS2LidarDriver::setDeviceAddress(uint32_t timeout) +{ + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + if (m_SingleChannel) { + return RESULT_OK; + } + + disableDataGrabbing(); + flushSerial(); + { + ScopedLocker l(_lock); + + if ((ans = sendCommand(GS_LIDAR_CMD_GET_ADDRESS)) != RESULT_OK) { + return ans; + } + + gs_lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) { + return ans; + } + + if (response_header.type != GS_LIDAR_CMD_GET_ADDRESS) { + return RESULT_FAIL; + } + + printf("[YDLIDAR] Lidar module count %d", (response_header.address << 1) + 1); + } + + return RESULT_OK; +} + +/************************************************************************/ +/* the set to signal quality */ +/************************************************************************/ +void GS2LidarDriver::setIntensities(const bool &isintensities) { + if (m_intensities != isintensities) { + if (globalRecvBuffer) { + delete[] globalRecvBuffer; + globalRecvBuffer = NULL; + } + + globalRecvBuffer = new uint8_t[sizeof(gs2_node_package)]; + } + + m_intensities = isintensities; + + if (m_intensities) { + PackageSampleBytes = 2; + } else { + PackageSampleBytes = 2; + } +} +/** +* @brief 设置雷达异常自动重新连接 \n +* @param[in] enable 是否开启自动重连: +* true 开启 +* false 关闭 +*/ +void GS2LidarDriver::setAutoReconnect(const bool &enable) { + isAutoReconnect = enable; +} + +void GS2LidarDriver::checkTransDelay() +{ + //采样率 + trans_delay = _serial->getByteTime(); + sample_rate = 27 * 160; + m_PointTime = 1e9 / sample_rate; +} + +/************************************************************************/ +/* start to scan */ +/************************************************************************/ +result_t GS2LidarDriver::startScan(bool force, uint32_t timeout) { + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + if (m_isScanning) { + return RESULT_OK; + } + + stop(); + checkTransDelay(); + flushSerial(); + + //配置GS2模组地址（三个模组） + setDeviceAddress(300); + + //获取GS2参数 + gs_device_para gs2_info; +// delay(30); + getDevicePara(gs2_info, 300); +// delay(30); + { + flushSerial(); + + ScopedLocker l(_lock); + if ((ans = sendCommand(force ? LIDAR_CMD_FORCE_SCAN : GS_LIDAR_CMD_SCAN)) != + RESULT_OK) { + return ans; + } + + if (!m_SingleChannel) + { + gs_lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) { + return ans; + } + + if (response_header.type != GS_LIDAR_ANS_SCAN) { + printf("[CYdLidar] Response to start scan type error!\n"); + return RESULT_FAIL; + } + } + + ans = this->createThread(); + } + + return ans; +} + + +result_t GS2LidarDriver::stopScan(uint32_t timeout) { + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + ScopedLocker l(_lock); + + if ((ans = sendCommand(GS_LIDAR_CMD_STOP)) != RESULT_OK) { + return ans; + } + gs_lidar_ans_header response_header; + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) { + return ans; + } + if (response_header.type != GS_LIDAR_CMD_STOP) { + return RESULT_FAIL; + } + delay(10); + + return RESULT_OK; +} + +result_t GS2LidarDriver::createThread() { + _thread = CLASS_THREAD(GS2LidarDriver, cacheScanData); + + if (_thread.getHandle() == 0) { + m_isScanning = false; + return RESULT_FAIL; + } + + m_isScanning = true; + return RESULT_OK; +} + + +result_t GS2LidarDriver::startAutoScan(bool force, uint32_t timeout) { + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + flushSerial(); + delay(10); + { + + ScopedLocker l(_lock); + + if ((ans = sendCommand(force ? LIDAR_CMD_FORCE_SCAN : GS_LIDAR_CMD_SCAN)) != + RESULT_OK) { + return ans; + } + + if (!m_SingleChannel) { + gs_lidar_ans_header response_header; + + if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) { + return ans; + } + + if (response_header.type != GS_LIDAR_CMD_SCAN) { + return RESULT_FAIL; + } + } + + } + + return RESULT_OK; +} + +/************************************************************************/ +/* stop scan */ +/************************************************************************/ +result_t GS2LidarDriver::stop() { + if (isAutoconnting) { + isAutoReconnect = false; + m_isScanning = false; + } + + disableDataGrabbing(); + stopScan(); + + return RESULT_OK; +} + +/************************************************************************/ +/* reset device */ +/************************************************************************/ +result_t GS2LidarDriver::reset(uint8_t addr, uint32_t timeout) { + UNUSED(timeout); + result_t ans; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + ScopedLocker l(_lock); + + if ((ans = sendCommand(addr, GS_LIDAR_CMD_RESET)) != RESULT_OK) { + return ans; + } + + return RESULT_OK; +} + +std::string GS2LidarDriver::getSDKVersion() { + return YDLIDAR_SDK_VERSION_STR; +} + +std::map GS2LidarDriver::lidarPortList() { + std::vector lst = list_ports(); + std::map ports; + + for (std::vector::iterator it = lst.begin(); it != lst.end(); it++) { + std::string port = "ydlidar" + (*it).device_id; + ports[port] = (*it).port; + } + + return ports; +} + +const char *GS2LidarDriver::DescribeError(bool isTCP) +{ + if (_serial) { + return _serial->DescribeError(); + } + return nullptr; +} + +result_t GS2LidarDriver::getHealth(device_health &, uint32_t) +{ + return RESULT_OK; +} + +result_t GS2LidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) +{ + result_t ret = RESULT_OK; + + if (!m_isConnected) { + return RESULT_FAIL; + } + + disableDataGrabbing(); +// flushSerial(); + { + ScopedLocker l(_lock); + + if ((ret = sendCommand(GS_LIDAR_CMD_GET_VERSION)) != RESULT_OK) { + return ret; + } + + gs_lidar_ans_header head; + if ((ret = waitResponseHeaderEx(&head, GS_LIDAR_CMD_GET_VERSION, timeout)) != RESULT_OK) { + return ret; + } + if (head.type != GS_LIDAR_CMD_GET_VERSION) { + return RESULT_FAIL; + } + if (head.size < sizeof(gs_device_info)) { + return RESULT_FAIL; + } + + if (waitForData(head.size + 1, timeout) != RESULT_OK) { + return RESULT_FAIL; + } + gs_device_info di = {0}; + getData(reinterpret_cast(&di), sizeof(di)); + model = YDLIDAR_GS2; + info.model = model; + info.hardware_version = di.hardware_version; + info.firmware_version = uint16_t((di.firmware_version & 0xFF) << 8) + + uint16_t(di.firmware_version >> 8); + memcpy(info.serialnum, di.serialnum, 16); + // head.address; //雷达序号 + } + + return RESULT_OK; +} + +} diff --git a/src/GS2LidarDriver.h b/src/GS2LidarDriver.h new file mode 100644 index 0000000..66aa816 --- /dev/null +++ b/src/GS2LidarDriver.h @@ -0,0 +1,581 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2018, EAIBOT, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/** @page GS2LidarDriver + * GS2LidarDriver API + +
Library GS2LidarDriver +
File GS2LidarDriver.h +
Author Tony [code at ydlidar com] +
Source https://github.com/ydlidar/YDLidar-SDK +
Version 1.0.0 +
+ This GS2LidarDriver support [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) and [TYPE_TOF](\ref LidarTypeID::TYPE_TOF) LiDAR + +* @copyright Copyright (c) 2018-2020 EAIBOT + Jump to the @link ::ydlidar::GS2LidarDriver @endlink interface documentation. +*/ +#ifndef GS2_YDLIDAR_DRIVER_H +#define GS2_YDLIDAR_DRIVER_H + +#include +#include +#include +#include "core/serial/serial.h" +#include "core/base/locker.h" +#include "core/base/thread.h" +#include "core/common/ydlidar_protocol.h" +#include "core/common/ydlidar_help.h" + +#if !defined(__cplusplus) +#ifndef __cplusplus +#error "The YDLIDAR SDK requires a C++ compiler to be built" +#endif +#endif + + +using namespace std; + +namespace ydlidar { + +using namespace core; +using namespace core::serial; +using namespace core::base; + +/*! +* GS2操控类 +*/ +class GS2LidarDriver : public 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/G4B/G4PRO/G6/F4/F4PRO false +
S4/S4B/X4/R2/G4C false +
S2/X2/X2L false +
TG15/TG30/TG50 false +
TX8/TX20 false +
T5/T15 false +
GS2 true +
+ * @see DriverInterface::setSingleChannel and DriverInterface::getSingleChannel + */ + PropertyBuilderByName(bool, SingleChannel, private); + /** + * @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/G4B/G4C/G4PRO [TYPE_TRIANGLE](\ref LidarTypeID::TYPE_TRIANGLE) +
G6/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) +
GS2 [TYPE_GS](\ref LidarTypeID::TYPE_GS) +
+ * @see [LidarTypeID](\ref LidarTypeID) + * @see DriverInterface::setLidarType and DriverInterface::getLidarType + */ + PropertyBuilderByName(int, LidarType, private); + /** + * @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,private); + /*! + * A constructor. + * A more elaborate description of the constructor. + */ + GS2LidarDriver(); + + /*! + * A destructor. + * A more elaborate description of the destructor. + */ + virtual ~GS2LidarDriver(); + + /*! + * @brief 连接雷达 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] port_path 串口号 + * @param[in] baudrate 波特率，YDLIDAR-GS2 雷达波特率：961200 + * @return 返回连接状态 + * @retval 0 成功 + * @retval < 0 失败 + * @note连接成功后，必须使用::disconnect函数关闭 + * @see 函数::GS2LidarDriver::disconnect (“::”是指定有连接功能,可以看文档里的disconnect变成绿,点击它可以跳转到disconnect.) + */ + result_t connect(const char *port_path, uint32_t baudrate); + + /*! + * @brief 断开雷达连接 + */ + void disconnect(); + + /*! + * @brief 获取当前SDK版本号 \n + * 静态函数 + * @return 返回当前SKD 版本号 + */ + virtual std::string getSDKVersion(); + + /*! + * @brief lidarPortList 获取雷达端口 + * @return 在线雷达列表 + */ + static std::map lidarPortList(); + + /*! + * @brief 扫图状态 \n + * @return 返回当前雷达扫图状态 + * @retval true 正在扫图 + * @retval false 扫图关闭 + */ + bool isscanning() const; + + /*! + * @brief 连接雷达状态 \n + * @return 返回连接状态 + * @retval true 成功 + * @retval false 失败 + */ + bool isconnected() const; + + /*! + * @brief 设置雷达是否带信号质量 \n + * 连接成功后，必须使用::disconnect函数关闭 + * @param[in] isintensities 是否带信号质量: + * true 带信号质量 + * false 无信号质量 + * @note只有S4B(波特率是153600)雷达支持带信号质量, 别的型号雷达暂不支持 + */ + void setIntensities(const bool &isintensities); + + /*! + * @brief 设置雷达异常自动重新连接 \n + * @param[in] enable 是否开启自动重连: + * true 开启 + * false 关闭 + */ + void setAutoReconnect(const bool &enable); + + /*! + * @brief 获取雷达设备信息 \n + * @param[in] parameters 设备信息 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE or RESULT_TIMEOUT 获取失败 + */ + result_t getDevicePara(gs_device_para &info, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 配置雷达地址 \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 配置成功 + * @retval RESULT_FAILE or RESULT_TIMEOUT 配置超时 + */ + result_t setDeviceAddress(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note 只用开启一次成功即可 + */ + result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 关闭扫描 \n + * @return 返回执行结果 + * @retval RESULT_OK 关闭成功 + * @retval RESULT_FAILE 关闭失败 + */ + result_t stop(); + + /*! + * @brief 获取激光数据 \n + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + * @note 获取之前，必须使用::startScan函数开启扫描 + */ + result_t grabScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT) ; + + + /*! + * @brief 补偿激光角度 \n + * 把角度限制在0到360度之间 + * @param[in] nodebuffer 激光点信息 + * @param[in] count 一圈激光点数 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 补偿之前，必须使用::grabScanData函数获取激光数据成功 + */ + result_t ascendScanData(node_info *nodebuffer, size_t count); + + /*! + * @brief 重置激光雷达 \n + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + * @note 停止扫描后再执行当前操作, 如果在扫描中调用::stop函数停止扫描 + */ + result_t reset(uint8_t addr, uint32_t timeout = DEFAULT_TIMEOUT); + + protected: + + /*! + * @brief 创建解析雷达数据线程 \n + * @note 创建解析雷达数据线程之前，必须使用::startScan函数开启扫图成功 + */ + result_t createThread(); + + + /*! + * @brief 重新连接开启扫描 \n + * @param[in] force 扫描模式 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 开启成功 + * @retval RESULT_FAILE 开启失败 + * @note sdk 自动重新连接调用 + */ + result_t startAutoScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ; + + /*! + * @brief stopScan + * @param timeout + * @return + */ + result_t stopScan(uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief waitDevicePackage + * @param timeout + * @return + */ + result_t waitDevicePackage(uint32_t timeout = DEFAULT_TIMEOUT); + /*! + * @brief 解包激光数据 \n + * @param[in] node 解包后激光点信息 + * @param[in] timeout 超时时间 + */ + result_t waitPackage(node_info *node, uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] nodebuffer 激光信息指针 + * @param[in] count 激光点数大小 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 失败 + */ + result_t waitScanData(node_info *nodebuffer, size_t &count, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 激光数据解析线程 \n + */ + int cacheScanData(); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] cmd 命名码 + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + */ + result_t sendCommand(uint8_t cmd, + const void *payload = NULL, + size_t payloadsize = 0); + + /*! + * @brief 发送数据到雷达 \n + * @param[in] addr 模组地址 + * @param[in] cmd 命名码 + * @param[in] payload payload + * @param[in] payloadsize payloadsize + * @return 返回执行结果 + * @retval RESULT_OK 成功 + * @retval RESULT_FAILE 失败 + */ + result_t sendCommand(uint8_t addr, + uint8_t cmd, + const void *payload = NULL, + size_t payloadsize = 0); + + /*! + * @brief 等待激光数据包头 \n + * @param[in] header 包头 + * @param[in] timeout 超时时间 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitResponseHeader(gs_lidar_ans_header *header, + uint32_t timeout = DEFAULT_TIMEOUT); + result_t waitResponseHeaderEx(gs_lidar_ans_header *header, + uint8_t cmd, + uint32_t timeout = DEFAULT_TIMEOUT); + + /*! + * @brief 等待固定数量串口数据 \n + * @param[in] data_count 等待数据大小 + * @param[in] timeout 等待时间 + * @param[in] returned_size 实际数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_TIMEOUT 等待超时 + * @retval RESULT_FAILE 获取失败 + * @note 当timeout = -1 时, 将一直等待 + */ + result_t waitForData(size_t data_count, uint32_t timeout = DEFAULT_TIMEOUT, + size_t *returned_size = NULL); + + /*! + * @brief 获取串口数据 \n + * @param[in] data 数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 获取成功 + * @retval RESULT_FAILE 获取失败 + */ + result_t getData(uint8_t *data, size_t size); + + /*! + * @brief 串口发送数据 \n + * @param[in] data 发送数据指针 + * @param[in] size 数据大小 + * @return 返回执行结果 + * @retval RESULT_OK 发送成功 + * @retval RESULT_FAILE 发送失败 + */ + result_t sendData(const uint8_t *data, size_t size); + + + /*! + * @brief checkTransDelay + */ + void checkTransDelay(); + + /*! + * @brief 关闭数据获取通道 \n + */ + void disableDataGrabbing(); + + /*! + * @brief 设置串口DTR \n + */ + void setDTR(); + + /*! + * @brief 清除串口DTR \n + */ + void clearDTR(); + + /*! + * @brief flushSerial + */ + void flushSerial(); + + /*! + * @brief checkAutoConnecting + */ + result_t checkAutoConnecting(); + + /*! + * @brief 换算得出点的距离和角度 + */ + void angTransform(uint16_t dist, int n, double *dstTheta, uint16_t *dstDist); + + void addPointsToVec(node_info *nodebuffer, size_t &count); + + /** + * @brief 串口错误信息 + * @param isTCP TCP or UDP + * @return error information + */ + virtual const char *DescribeError(bool isTCP = false); + + /** + * @brief GS2雷达没有健康信息\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); + + /** + * @brief get Device information \n + * @param[in] info 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 &info, uint32_t timeout = DEFAULT_TIMEOUT); + + //未实现的虚函数 + virtual result_t getScanFrequency(scan_frequency &frequency, uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyDis(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyAdd(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyAddMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanFrequencyDisMic(scan_frequency &frequency, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t getSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setSamplingRate(sampling_rate &rate, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t getZeroOffsetAngle(offset_angle &angle, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + virtual result_t setScanHeartbeat(scan_heart_beat &beat, + uint32_t timeout = DEFAULT_TIMEOUT) { return RESULT_OK; } + +public: + enum { + DEFAULT_TIMEOUT = 2000, /**< 默认超时时间. */ + DEFAULT_HEART_BEAT = 1000, /**< 默认检测掉电功能时间. */ + MAX_SCAN_NODES = 3600, /**< 最大扫描点数. */ + DEFAULT_TIMEOUT_COUNT = 1, + }; + + node_info *scan_node_buf; ///< 激光点信息 + size_t scan_node_count; ///< 激光点数 + Event _dataEvent; ///< 数据同步事件 + Locker _lock; ///< 线程锁 + Locker _serial_lock; ///< 串口锁 + Thread _thread; ///< 线程id + + private: + int PackageSampleBytes; ///< 一个包包含的激光点数 + serial::Serial *_serial; ///< 串口 + bool m_intensities; ///< 信号质量状体 + uint32_t m_baudrate; ///< 波特率 + bool isSupportMotorDtrCtrl; ///< 是否支持电机控制 + uint32_t trans_delay; ///< 串口传输一个byte时间 + int m_sampling_rate; ///< 采样频率 + int model; ///< 雷达型号 + int sample_rate; ///< + + gs2_node_package package; ///< 带信号质量协议包 + + uint16_t package_Sample_Index; ///< 包采样点索引 + float IntervalSampleAngle; + float IntervalSampleAngle_LastPackage; + uint8_t CheckSum; ///< 校验和 + uint8_t scan_frequence; ///< 协议中雷达转速 + + uint8_t CheckSumCal; + uint16_t SampleNumlAndCTCal; + uint16_t LastSampleAngleCal; + bool CheckSumResult; + uint16_t Valu8Tou16; + + std::string serial_port;///< 雷达端口 + uint8_t *globalRecvBuffer; + int retryCount; + bool has_device_header; + uint8_t last_device_byte; + int asyncRecvPos; + uint16_t async_size; + + //singleChannel + device_info info_; + device_health health_; + gs_lidar_ans_header header_; + uint8_t *headerBuffer; + uint8_t *infoBuffer; + uint8_t *healthBuffer; + bool get_device_info_success; + bool get_device_health_success; + + int package_index; + uint8_t package_type; + bool has_package_error; + + double d_compensateK0[PackageMaxModuleNums]; + double d_compensateK1[PackageMaxModuleNums]; + double d_compensateB0[PackageMaxModuleNums]; + double d_compensateB1[PackageMaxModuleNums]; + uint16_t u_compensateK0[PackageMaxModuleNums]; + uint16_t u_compensateK1[PackageMaxModuleNums]; + uint16_t u_compensateB0[PackageMaxModuleNums]; + uint16_t u_compensateB1[PackageMaxModuleNums]; + double bias[PackageMaxModuleNums]; + bool isValidPoint; + uint8_t package_Sample_Num; + + uint8_t frameNum; //帧序号 + uint8_t moduleNum; //模块编号 + bool isPrepareToSend; //是否准备好发送 + + std::vector multi_package; +}; + +}// namespace ydlidar + +#endif // GS2_YDLIDAR_DRIVER_H diff --git a/src/ydlidar_driver.cpp b/src/YDlidarDriver.cpp similarity index 99% rename from src/ydlidar_driver.cpp rename to src/YDlidarDriver.cpp index a9c7bc3..b836939 100644 --- a/src/ydlidar_driver.cpp +++ b/src/YDlidarDriver.cpp @@ -23,7 +23,7 @@ // #include #include -#include "ydlidar_driver.h" +#include "YDlidarDriver.h" #include #include #include diff --git a/src/ydlidar_driver.h b/src/YDlidarDriver.h similarity index 100% rename from src/ydlidar_driver.h rename to src/YDlidarDriver.h