From 2243f288bb6f413905ff15c6b4cba6c9cbc460a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=8F=A0=E8=90=9D=E6=9C=89=E7=82=B9=E9=85=B8?= Date: Fri, 18 Oct 2024 06:22:35 +0000 Subject: [PATCH] update bb_fix/ydlidar-sdk/files/0003-GS1.patch. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 菠萝有点酸 --- bb_fix/ydlidar-sdk/files/0003-GS1.patch | 5028 +++++++++++------------ 1 file changed, 2514 insertions(+), 2514 deletions(-) diff --git a/bb_fix/ydlidar-sdk/files/0003-GS1.patch b/bb_fix/ydlidar-sdk/files/0003-GS1.patch index 26a643f..df54304 100644 --- a/bb_fix/ydlidar-sdk/files/0003-GS1.patch +++ b/bb_fix/ydlidar-sdk/files/0003-GS1.patch @@ -1,57 +1,57 @@ -From 5c1a1aa92d3296405771b9ef05be1008b9b7148e Mon Sep 17 00:00:00 2001 -From: zhanyiaini -Date: Wed, 28 Sep 2022 19:01:33 +0800 -Subject: [PATCH 3/5] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=94=AF=E6=8C=81GS1?= - =?UTF-8?q?=E9=9B=B7=E8=BE=BE?= -MIME-Version: 1.0 -Content-Type: text/plain; charset=UTF-8 -Content-Transfer-Encoding: 8bit - ---- - core/common/DriverInterface.h | 1 + - core/common/ydlidar_def.h | 1 + - core/common/ydlidar_help.h | 33 +- - core/common/ydlidar_protocol.h | 35 +- - core/serial/impl/unix/list_ports_linux.cpp | 9 +- - samples/gs_test.cpp | 2 +- - samples/tri_test.cpp | 169 +- - src/CYdLidar.cpp | 15 +- - src/GS1LidarDriver.cpp | 1797 ++++++++++++++++++++ - src/GS1LidarDriver.h | 589 +++++++ - src/YDlidarDriver.cpp | 4 + - 11 files changed, 2561 insertions(+), 94 deletions(-) - create mode 100644 src/GS1LidarDriver.cpp - create mode 100644 src/GS1LidarDriver.h - -diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h -index b22b098..e1e1e31 100644 ---- a/core/common/DriverInterface.h -+++ b/core/common/DriverInterface.h -@@ -473,6 +473,7 @@ class DriverInterface { - YDLIDAR_G7 = 21,/**< G7 LiDAR Model. */ - - YDLIDAR_GS2 = 51, //GS2雷达 -+ YDLIDAR_GS1 = 52, //GS1雷达 - - YDLIDAR_TG15 = 100,/**< TG15 LiDAR Model. */ - YDLIDAR_TG30 = 101,/**< T30 LiDAR Model. */ -diff --git a/core/common/ydlidar_def.h b/core/common/ydlidar_def.h -index 136dd46..f8bf961 100644 ---- a/core/common/ydlidar_def.h -+++ b/core/common/ydlidar_def.h -@@ -46,6 +46,7 @@ typedef enum { - TYPE_TRIANGLE = 1,/**< G4. G6. G2 LiDAR.*/ - TYPE_TOF_NET = 2,/**< T15 LiDAR.*/ - TYPE_GS = 3, //GS系列雷达（目前只有GS2） -+ TYPE_GS1 = 4, //GS1雷达 - TYPE_Tail, - } LidarTypeID; - -diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h -index c96b2ef..35ac3ee 100644 ---- a/core/common/ydlidar_help.h -+++ b/core/common/ydlidar_help.h -@@ -168,6 +168,9 @@ inline std::string lidarModelToString(int model) { +From 5c1a1aa92d3296405771b9ef05be1008b9b7148e Mon Sep 17 00:00:00 2001 +From: zhanyiaini +Date: Wed, 28 Sep 2022 19:01:33 +0800 +Subject: [PATCH 3/5] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=94=AF=E6=8C=81GS1?= + =?UTF-8?q?=E9=9B=B7=E8=BE=BE?= +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +--- + core/common/DriverInterface.h | 1 + + core/common/ydlidar_def.h | 1 + + core/common/ydlidar_help.h | 33 +- + core/common/ydlidar_protocol.h | 35 +- + core/serial/impl/unix/list_ports_linux.cpp | 9 +- + samples/gs_test.cpp | 2 +- + samples/tri_test.cpp | 169 +- + src/CYdLidar.cpp | 15 +- + src/GS1LidarDriver.cpp | 1797 ++++++++++++++++++++ + src/GS1LidarDriver.h | 589 +++++++ + src/YDlidarDriver.cpp | 4 + + 11 files changed, 2561 insertions(+), 94 deletions(-) + create mode 100644 src/GS1LidarDriver.cpp + create mode 100644 src/GS1LidarDriver.h + +diff --git a/core/common/DriverInterface.h b/core/common/DriverInterface.h +index b22b098..e1e1e31 100644 +--- a/core/common/DriverInterface.h ++++ b/core/common/DriverInterface.h +@@ -473,6 +473,7 @@ class DriverInterface { + YDLIDAR_G7 = 21,/**< G7 LiDAR Model. */ + + YDLIDAR_GS2 = 51, //GS2雷达 ++ YDLIDAR_GS1 = 52, //GS1雷达 + + YDLIDAR_TG15 = 100,/**< TG15 LiDAR Model. */ + YDLIDAR_TG30 = 101,/**< T30 LiDAR Model. */ +diff --git a/core/common/ydlidar_def.h b/core/common/ydlidar_def.h +index 136dd46..f8bf961 100644 +--- a/core/common/ydlidar_def.h ++++ b/core/common/ydlidar_def.h +@@ -46,6 +46,7 @@ typedef enum { + TYPE_TRIANGLE = 1,/**< G4. G6. G2 LiDAR.*/ + TYPE_TOF_NET = 2,/**< T15 LiDAR.*/ + TYPE_GS = 3, //GS系列雷达（目前只有GS2） ++ TYPE_GS1 = 4, //GS1雷达 + TYPE_Tail, + } LidarTypeID; + +diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h +index c96b2ef..35ac3ee 100644 +--- a/core/common/ydlidar_help.h ++++ b/core/common/ydlidar_help.h +@@ -168,6 +168,9 @@ inline std::string lidarModelToString(int model) { name = "G7"; break; @@ -61,7 +61,7 @@ index c96b2ef..35ac3ee 100644 case DriverInterface::YDLIDAR_GS2: name = "GS2"; break; -@@ -368,6 +371,7 @@ inline bool hasScanFrequencyCtrl(int model) { +@@ -368,6 +371,7 @@ inline bool hasScanFrequencyCtrl(int model) { model == DriverInterface::YDLIDAR_S4B || model == DriverInterface::YDLIDAR_S2 || model == DriverInterface::YDLIDAR_X4 || @@ -69,7 +69,7 @@ index c96b2ef..35ac3ee 100644 model == DriverInterface::YDLIDAR_GS2) { ret = false; } -@@ -405,6 +409,7 @@ inline bool hasIntensity(int model) { +@@ -405,6 +409,7 @@ inline bool hasIntensity(int model) { if (model == DriverInterface::YDLIDAR_G2B || model == DriverInterface::YDLIDAR_G4B || model == DriverInterface::YDLIDAR_S4B || @@ -77,7 +77,7 @@ index c96b2ef..35ac3ee 100644 model == DriverInterface::YDLIDAR_GS2) { ret = true; } -@@ -540,14 +545,30 @@ inline bool isTriangleLidar(int type) { +@@ -540,14 +545,30 @@ inline bool isTriangleLidar(int type) { * @param type LiDAR type * @return true if it is a Triangle type, otherwise false. */ @@ -114,11 +114,11 @@ index c96b2ef..35ac3ee 100644 } /** -diff --git a/core/common/ydlidar_protocol.h b/core/common/ydlidar_protocol.h -index d25ab73..68f21f4 100644 ---- a/core/common/ydlidar_protocol.h -+++ b/core/common/ydlidar_protocol.h -@@ -140,13 +140,14 @@ +diff --git a/core/common/ydlidar_protocol.h b/core/common/ydlidar_protocol.h +index d25ab73..68f21f4 100644 +--- a/core/common/ydlidar_protocol.h ++++ b/core/common/ydlidar_protocol.h +@@ -140,13 +140,14 @@ #define Angle_Py 5.315 #define Angle_PAngle 22.5 #define PackageMaxModuleNums 0x03 @@ -135,7 +135,7 @@ index d25ab73..68f21f4 100644 /// CT Package Type typedef enum { -@@ -326,18 +327,36 @@ struct lidar_ans_header { +@@ -326,18 +327,36 @@ struct lidar_ans_header { uint8_t type; } __attribute__((packed)); @@ -176,7 +176,7 @@ index d25ab73..68f21f4 100644 } __attribute__((packed)); struct gs2_node_package { -@@ -346,9 +365,9 @@ struct gs2_node_package { +@@ -346,9 +365,9 @@ struct gs2_node_package { uint8_t package_CT; uint16_t size; uint16_t BackgroudLight; @@ -188,11 +188,11 @@ index d25ab73..68f21f4 100644 struct gs_lidar_ans_header { uint8_t syncByte0; -diff --git a/core/serial/impl/unix/list_ports_linux.cpp b/core/serial/impl/unix/list_ports_linux.cpp -index ce571d2..d0877ed 100644 ---- a/core/serial/impl/unix/list_ports_linux.cpp -+++ b/core/serial/impl/unix/list_ports_linux.cpp -@@ -284,7 +284,8 @@ usb_sysfs_hw_string(const string &sysfs_path) { +diff --git a/core/serial/impl/unix/list_ports_linux.cpp b/core/serial/impl/unix/list_ports_linux.cpp +index ce571d2..d0877ed 100644 +--- a/core/serial/impl/unix/list_ports_linux.cpp ++++ b/core/serial/impl/unix/list_ports_linux.cpp +@@ -284,7 +284,8 @@ usb_sysfs_hw_string(const string &sysfs_path) { } vector @@ -202,7 +202,7 @@ index ce571d2..d0877ed 100644 vector results; vector search_globs; -@@ -312,8 +313,8 @@ serial::list_ports() { +@@ -312,8 +313,8 @@ serial::list_ports() { std::size_t found = hardware_id.find("10c4:ea60"); std::size_t found1 = hardware_id.find("0483:5740"); @@ -213,7 +213,7 @@ index ce571d2..d0877ed 100644 PortInfo device_entry; device_entry.port = device; device_entry.description = friendly_name; -@@ -322,8 +323,6 @@ serial::list_ports() { +@@ -322,8 +323,6 @@ serial::list_ports() { results.push_back(device_entry); } @@ -222,11 +222,11 @@ index ce571d2..d0877ed 100644 } return results; -diff --git a/samples/gs_test.cpp b/samples/gs_test.cpp -index 3143666..2bb7671 100644 ---- a/samples/gs_test.cpp -+++ b/samples/gs_test.cpp -@@ -153,7 +153,7 @@ int main(int argc, char *argv[]) +diff --git a/samples/gs_test.cpp b/samples/gs_test.cpp +index 3143666..2bb7671 100644 +--- a/samples/gs_test.cpp ++++ b/samples/gs_test.cpp +@@ -153,7 +153,7 @@ int main(int argc, char *argv[]) /// lidar baudrate laser.setlidaropt(LidarPropSerialBaudrate, &baudrate, sizeof(int)); /// gs lidar @@ -235,11 +235,11 @@ index 3143666..2bb7671 100644 laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); /// device type optval = YDLIDAR_TYPE_SERIAL; -diff --git a/samples/tri_test.cpp b/samples/tri_test.cpp -index 3deb862..4e5f80c 100644 ---- a/samples/tri_test.cpp -+++ b/samples/tri_test.cpp -@@ -1,36 +1,36 @@ +diff --git a/samples/tri_test.cpp b/samples/tri_test.cpp +index 3deb862..4e5f80c 100644 +--- a/samples/tri_test.cpp ++++ b/samples/tri_test.cpp +@@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* @@ -308,7 +308,7 @@ index 3deb862..4e5f80c 100644 #include "CYdLidar.h" #include #include -@@ -58,7 +58,8 @@ using namespace ydlidar; +@@ -58,7 +58,8 @@ using namespace ydlidar; * Step7: Uninitialize the SDK and Disconnect the LiDAR.(::CYdLidar::disconnecting)\n */ @@ -318,7 +318,7 @@ index 3deb862..4e5f80c 100644 printf("__ ______ _ ___ ____ _ ____ \n"); printf("\\ \\ / / _ \\| | |_ _| _ \\ / \\ | _ \\ \n"); printf(" \\ V /| | | | | | || | | |/ _ \\ | |_) | \n"); -@@ -70,36 +71,46 @@ int main(int argc, char *argv[]) { +@@ -70,36 +71,46 @@ int main(int argc, char *argv[]) { ydlidar::os_init(); std::map ports = @@ -374,7 +374,7 @@ index 3deb862..4e5f80c 100644 id--; it++; } -@@ -122,16 +133,19 @@ int main(int argc, char *argv[]) { +@@ -122,16 +133,19 @@ int main(int argc, char *argv[]) { printf("Baudrate:\n"); for (std::map::iterator it = baudrateList.begin(); @@ -397,7 +397,7 @@ index 3deb862..4e5f80c 100644 continue; } -@@ -139,7 +153,8 @@ int main(int argc, char *argv[]) { +@@ -139,7 +153,8 @@ int main(int argc, char *argv[]) { break; } @@ -407,7 +407,7 @@ index 3deb862..4e5f80c 100644 return 0; } -@@ -149,15 +164,18 @@ int main(int argc, char *argv[]) { +@@ -149,15 +164,18 @@ int main(int argc, char *argv[]) { std::cin >> input_channel; std::transform(input_channel.begin(), input_channel.end(), input_channel.begin(), @@ -431,7 +431,7 @@ index 3deb862..4e5f80c 100644 return 0; } -@@ -165,12 +183,14 @@ int main(int argc, char *argv[]) { +@@ -165,12 +183,14 @@ int main(int argc, char *argv[]) { float frequency = 8.0; @@ -448,7 +448,7 @@ index 3deb862..4e5f80c 100644 break; } -@@ -178,7 +198,8 @@ int main(int argc, char *argv[]) { +@@ -178,7 +198,8 @@ int main(int argc, char *argv[]) { "Invalid scan frequency,The scanning frequency range is 5 to 12 HZ, Please re-enter.\n"); } @@ -458,7 +458,7 @@ index 3deb862..4e5f80c 100644 return 0; } -@@ -252,14 +273,16 @@ int main(int argc, char *argv[]) { +@@ -252,14 +273,16 @@ int main(int argc, char *argv[]) { laser.enableSunNoise(false); bool ret = laser.initialize(); @@ -477,7 +477,7 @@ index 3deb862..4e5f80c 100644 fprintf(stderr, "Fail to start %s\n", laser.DescribeError()); fflush(stderr); return -1; -@@ -278,23 +301,23 @@ int main(int argc, char *argv[]) { +@@ -278,23 +301,23 @@ int main(int argc, char *argv[]) { LaserScan scan; while (ydlidar::os_isOk()) { @@ -518,11 +518,11 @@ index 3deb862..4e5f80c 100644 } laser.turnOff(); -diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp -index ec89bbf..668e24f 100644 ---- a/src/CYdLidar.cpp -+++ b/src/CYdLidar.cpp -@@ -33,6 +33,7 @@ +diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp +index ec89bbf..668e24f 100644 +--- a/src/CYdLidar.cpp ++++ b/src/CYdLidar.cpp +@@ -33,6 +33,7 @@ #include "core/common/ydlidar_help.h" #include "YDlidarDriver.h" #include "ETLidarDriver.h" @@ -530,7 +530,7 @@ index ec89bbf..668e24f 100644 #include "GS2LidarDriver.h" using namespace std; -@@ -654,12 +655,16 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) +@@ -654,12 +655,16 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) float angle = 0.0; debug.MaxDebugIndex = 0; @@ -547,7 +547,7 @@ index ec89bbf..668e24f 100644 if (isNetTOFLidar(m_LidarType)) { angle = static_cast(global_nodes[i].angle_q6_checkbit / 100.0f) + -@@ -737,6 +742,9 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) +@@ -737,6 +742,9 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) range = .0; } @@ -557,7 +557,7 @@ index ec89bbf..668e24f 100644 if (angle >= outscan.config.min_angle && angle <= outscan.config.max_angle) { -@@ -1802,7 +1810,12 @@ bool CYdLidar::checkCOMMs() +@@ -1802,7 +1810,12 @@ bool CYdLidar::checkCOMMs() { lidarPtr = new ydlidar::ETLidarDriver(); // T15 } @@ -571,2419 +571,2419 @@ index ec89bbf..668e24f 100644 { //GS2 lidarPtr = new ydlidar::GS2LidarDriver(); -diff --git a/src/GS1LidarDriver.cpp b/src/GS1LidarDriver.cpp -new file mode 100644 -index 0000000..dcaf0d0 ---- /dev/null -+++ b/src/GS1LidarDriver.cpp -@@ -0,0 +1,1797 @@ -+/********************************************************************* -+ * 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 "GS1LidarDriver.h" -+#include "core/serial/common.h" -+#include "ydlidar_config.h" -+ -+using namespace impl; -+ -+namespace ydlidar -+{ -+ -+ GS1LidarDriver::GS1LidarDriver() : _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(gs1_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; -+ } -+ -+ GS1LidarDriver::~GS1LidarDriver() -+ { -+ 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 GS1LidarDriver::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 GS1LidarDriver::setDTR() -+ { -+ if (!m_isConnected) -+ { -+ return; -+ } -+ -+ if (_serial) -+ { -+ _serial->setDTR(1); -+ } -+ } -+ -+ void GS1LidarDriver::clearDTR() -+ { -+ if (!m_isConnected) -+ { -+ return; -+ } -+ -+ if (_serial) -+ { -+ _serial->setDTR(0); -+ } -+ } -+ void GS1LidarDriver::flushSerial() -+ { -+ if (!m_isConnected) -+ { -+ return; -+ } -+ -+ size_t len = _serial->available(); -+ if (len) -+ { -+ _serial->readSize(len); -+ } -+ -+ delay(20); -+ } -+ -+ void GS1LidarDriver::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 GS1LidarDriver::disableDataGrabbing() -+ { -+ { -+ if (m_isScanning) -+ { -+ m_isScanning = false; -+ _dataEvent.set(); -+ } -+ } -+ _thread.join(); -+ } -+ -+ bool GS1LidarDriver::isscanning() const -+ { -+ return m_isScanning; -+ } -+ bool GS1LidarDriver::isconnected() const -+ { -+ return m_isConnected; -+ } -+ -+ result_t GS1LidarDriver::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 GS1LidarDriver::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 += addr; -+ 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 GS1LidarDriver::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 GS1LidarDriver::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) -+ { -+ return RESULT_FAIL; -+ } -+ -+ // printf("recv: "); -+ // printHex(data, r); -+ -+ size -= r; -+ data += r; -+ } -+ -+ return RESULT_OK; -+ } -+ -+ result_t GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 > 50) -+ { -+ retryConnect = 50; -+ } -+ -+ 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 GS1LidarDriver::cacheScanData() -+ { -+ node_info local_buf[MaxPointsPerPackge_GS1]; -+ size_t count = MaxPointsPerPackge_GS1; -+ 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 = MaxPointsPerPackge_GS1; -+ 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, "timeout 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) * MaxPointsPerPackge_GS1); -+ break; -+ } -+ } -+ -+ _lock.lock(); // timeout lock, wait resource copys -+ scan_node_buf[0].stamp = local_buf[0].stamp; -+ scan_node_buf[0].scan_frequence = local_buf[0].scan_frequence; -+ scan_node_buf[0].index = 0x03 & moduleNum >> 1; //gs1: 1, 2, 4 -+ scan_node_count = MaxPointsPerPackge_GS1; //一个包固定216个数据 -+ // 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 GS1LidarDriver::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; -+ -+ 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: -+ if (currentByte == LIDAR_ANS_SYNC_BYTE1) //如果出现超过4个包头标识的情况 -+ { -+ recvPos = 4; -+ continue; -+ } -+ 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; -+ moduleNum = 0; -+ has_device_header = false; -+ 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; -+ -+ // printf("env: 0x%04X\n", package.BackgroudLight); -+ // fflush(stdout); -+ } -+ } -+ -+ (*node).stamp = getTime(); -+ -+ if (CheckSumResult) -+ { -+ (*node).index = 0x03 & (moduleNum >> 1); -+ (*node).scan_frequence = scan_frequence; -+ (*node).sync_quality = 0; -+ -+ (*node).distance_q2 = -+ package.packageSample[package_Sample_Index].PakageSampleDistance; -+ -+ if (m_intensities) -+ { -+ (*node).sync_quality = (uint16_t)package.packageSample[package_Sample_Index].PakageSampleQuality; -+ } -+ -+ double sampleAngle = 0; -+ 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; -+ } -+ } -+ -+ //处理环境数据（2个字节分别存储在两个点的is属性中） -+ if (0 == package_Sample_Index) -+ (*node).is = package.BackgroudLight & 0xFF; -+ else if (1 == package_Sample_Index) -+ (*node).is = package.BackgroudLight >> 8; -+ -+ // printf("%s [%d] : Out : %u %u %u\n", __FUNCTION__, __LINE__, package_Sample_Index, node->distance_q2, node->angle_q6_checkbit); -+ } -+ else -+ { -+ (*node).sync_flag = Node_NotSync; -+ (*node).sync_quality = 0; -+ (*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT; -+ (*node).distance_q2 = 0; -+ (*node).scan_frequence = 0; -+ return RESULT_FAIL; -+ } -+ -+ package_Sample_Index ++; -+ (*node).sync_flag = Node_NotSync; -+ -+ if (package_Sample_Index >= MaxPointsPerPackge_GS1) -+ { -+ package_Sample_Index = 0; -+ (*node).sync_flag = Node_Sync; -+ CheckSumResult = false; -+ } -+ -+ return RESULT_OK; -+ } -+ -+ void GS1LidarDriver::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 -+ -+ tempTheta = atan(d_compensateK0[mdNum] * pixelU - d_compensateB0[mdNum]) * 180 / M_PI; -+ Dist = dist / cos(tempTheta * M_PI / 180); -+ theta = tempTheta; -+ -+ if (theta < 0) -+ { -+ theta += 360; -+ } -+ *dstTheta = theta; -+ *dstDist = Dist; -+ } -+ -+ void GS1LidarDriver::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) -+ { -+ GS1_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 GS1LidarDriver::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; -+ -+ if (!package_Sample_Index) -+ { -+ -+ size_t size = _serial->available(); -+ uint64_t delayTime = 0; -+ size_t PackageSize = sizeof(gs1_node_package); -+ -+ 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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::setIntensities(const bool &i) -+ { -+ m_intensities = i; -+ // if (m_intensities != isintensities) -+ // { -+ // if (globalRecvBuffer) -+ // { -+ // delete[] globalRecvBuffer; -+ // globalRecvBuffer = NULL; -+ // } -+ -+ // globalRecvBuffer = new uint8_t[sizeof(gs1_node_package)]; -+ // } -+ -+ // m_intensities = isintensities; -+ -+ // if (m_intensities) -+ // { -+ // PackageSampleBytes = 2; -+ // } -+ // else -+ // { -+ // PackageSampleBytes = 2; -+ // } -+ } -+ /** -+ * @brief 设置雷达异常自动重新连接 \n -+ * @param[in] enable 是否开启自动重连: -+ * true 开启 -+ * false 关闭 -+ */ -+ void GS1LidarDriver::setAutoReconnect(const bool &enable) -+ { -+ isAutoReconnect = enable; -+ } -+ -+ void GS1LidarDriver::checkTransDelay() -+ { -+ //采样率 -+ trans_delay = _serial->getByteTime(); -+ sample_rate = 27 * MaxPointsPerPackge_GS1; -+ m_PointTime = 1e9 / sample_rate; -+ } -+ -+ /************************************************************************/ -+ /* start to scan */ -+ /************************************************************************/ -+ result_t GS1LidarDriver::startScan(bool force, uint32_t timeout) -+ { -+ result_t ans; -+ -+ if (!m_isConnected) -+ { -+ return RESULT_FAIL; -+ } -+ -+ if (m_isScanning) -+ { -+ return RESULT_OK; -+ } -+ -+ stop(); -+ checkTransDelay(); -+ flushSerial(); -+ -+ //配置GS1模组地址（三个模组） -+ // setDeviceAddress(300); -+ -+ //获取GS1参数 -+ gs_device_para gs1_info; -+ // delay(30); -+ getDevicePara(gs1_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 GS1LidarDriver::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); -+ printf("GS1LidarDriver stop lidar successfully\n"); -+ -+ return RESULT_OK; -+ } -+ -+ result_t GS1LidarDriver::createThread() -+ { -+ _thread = CLASS_THREAD(GS1LidarDriver, cacheScanData); -+ -+ if (_thread.getHandle() == 0) -+ { -+ m_isScanning = false; -+ return RESULT_FAIL; -+ } -+ -+ m_isScanning = true; -+ return RESULT_OK; -+ } -+ -+ result_t GS1LidarDriver::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 GS1LidarDriver::stop() -+ { -+ if (isAutoconnting) -+ { -+ isAutoReconnect = false; -+ m_isScanning = false; -+ } -+ -+ disableDataGrabbing(); -+ stopScan(); -+ -+ return RESULT_OK; -+ } -+ -+ /************************************************************************/ -+ /* reset device */ -+ /************************************************************************/ -+ result_t GS1LidarDriver::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 GS1LidarDriver::getSDKVersion() -+ { -+ return YDLIDAR_SDK_VERSION_STR; -+ } -+ -+ std::map GS1LidarDriver::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 *GS1LidarDriver::DescribeError(bool isTCP) -+ { -+ if (_serial) -+ { -+ return _serial->DescribeError(); -+ } -+ return nullptr; -+ } -+ -+ result_t GS1LidarDriver::getHealth(device_health &, uint32_t) -+ { -+ return RESULT_OK; -+ } -+ -+ result_t GS1LidarDriver::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_GS1; -+ 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; -+ } -+ -+ result_t GS1LidarDriver::setWorkMode(int mode, uint8_t addr) -+ { -+ result_t ans; -+ uint32_t timeout = 300; -+ string buf; -+ -+ if (!isconnected()) -+ { -+ return RESULT_FAIL; -+ } -+ -+ //如果已经开启扫描，则先停止扫描 -+ if (isscanning()) -+ { -+ disableDataGrabbing(); -+ delay(10); -+ stopScan(); -+ } -+ flushSerial(); -+ -+ { -+ ScopedLocker l(_lock); -+ uint8_t m = uint8_t(mode); -+ if ((ans = sendCommand(addr, GS_LIDAR_CMD_SET_MODE, &m, 1)) != RESULT_OK) -+ { -+ return ans; -+ } -+ gs_lidar_ans_header response_header; -+ if ((ans = waitResponseHeaderEx(&response_header, GS_LIDAR_CMD_SET_MODE, timeout)) != RESULT_OK) -+ { -+ return ans; -+ } -+ if (response_header.type != GS_LIDAR_CMD_SET_MODE) -+ { -+ return RESULT_FAIL; -+ } -+ } -+ -+ return RESULT_OK; -+ } -+ -+} -diff --git a/src/GS1LidarDriver.h b/src/GS1LidarDriver.h -new file mode 100644 -index 0000000..c49ce81 ---- /dev/null -+++ b/src/GS1LidarDriver.h -@@ -0,0 +1,589 @@ -+/********************************************************************* -+* 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 GS1LidarDriver -+ * GS1LidarDriver API -+ -+
Library GS1LidarDriver -+
File GS1LidarDriver.h -+
Author Tony [code at ydlidar com] -+
Source https://github.com/ydlidar/YDLidar-SDK -+
Version 1.0.0 -+
-+ This GS1LidarDriver 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::GS1LidarDriver @endlink interface documentation. -+*/ -+#ifndef GS1_YDLIDAR_DRIVER_H -+#define GS1_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; -+ -+/*! -+* GS1操控类 -+*/ -+class GS1LidarDriver : 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 -+
GS1 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) -+
GS1 [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. -+ */ -+ GS1LidarDriver(); -+ -+ /*! -+ * A destructor. -+ * A more elaborate description of the destructor. -+ */ -+ virtual ~GS1LidarDriver(); -+ -+ /*! -+ * @brief 连接雷达 \n -+ * 连接成功后，必须使用::disconnect函数关闭 -+ * @param[in] port_path 串口号 -+ * @param[in] baudrate 波特率，YDLIDAR-GS1 雷达波特率：961200 -+ * @return 返回连接状态 -+ * @retval 0 成功 -+ * @retval < 0 失败 -+ * @note连接成功后，必须使用::disconnect函数关闭 -+ * @see 函数::GS1LidarDriver::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 GS1雷达没有健康信息\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); -+ -+ /** -+ * @brief 设置雷达工作模式（目前只针对GS1雷达） -+ * @param[in] mode 雷达工作模式，0为避障模式；1为延边模式 -+ * @param[in] addr 雷达地址，第1个雷达地址为0x01；第2个雷达地址为0x02；第3个雷达地址为0x04； -+ * @return 成功返回RESULT_OK，否则返回非RESULT_OK -+ */ -+ virtual result_t setWorkMode(int mode=0, uint8_t addr=0x00); -+ -+ //未实现的虚函数 -+ 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; ///< -+ -+ gs1_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 // GS1_YDLIDAR_DRIVER_H -diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp -index 7c1be54..d64e9ad 100644 ---- a/src/YDlidarDriver.cpp -+++ b/src/YDlidarDriver.cpp -@@ -1296,6 +1296,10 @@ void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) - (*node).sync_flag = Node_Sync; - package_index = 0; - -+ // printf("start angle %f end angle %f\n", -+ // float(FirstSampleAngle) / 64.0, -+ // float(LastSampleAngle) / 64.0); -+ - if (CheckSumResult) { - has_package_error = false; - (*node).index = package_index; --- -2.34.1 - +diff --git a/src/GS1LidarDriver.cpp b/src/GS1LidarDriver.cpp +new file mode 100644 +index 0000000..dcaf0d0 +--- /dev/null ++++ b/src/GS1LidarDriver.cpp +@@ -0,0 +1,1797 @@ ++/********************************************************************* ++ * 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 "GS1LidarDriver.h" ++#include "core/serial/common.h" ++#include "ydlidar_config.h" ++ ++using namespace impl; ++ ++namespace ydlidar ++{ ++ ++ GS1LidarDriver::GS1LidarDriver() : _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(gs1_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; ++ } ++ ++ GS1LidarDriver::~GS1LidarDriver() ++ { ++ 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 GS1LidarDriver::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 GS1LidarDriver::setDTR() ++ { ++ if (!m_isConnected) ++ { ++ return; ++ } ++ ++ if (_serial) ++ { ++ _serial->setDTR(1); ++ } ++ } ++ ++ void GS1LidarDriver::clearDTR() ++ { ++ if (!m_isConnected) ++ { ++ return; ++ } ++ ++ if (_serial) ++ { ++ _serial->setDTR(0); ++ } ++ } ++ void GS1LidarDriver::flushSerial() ++ { ++ if (!m_isConnected) ++ { ++ return; ++ } ++ ++ size_t len = _serial->available(); ++ if (len) ++ { ++ _serial->readSize(len); ++ } ++ ++ delay(20); ++ } ++ ++ void GS1LidarDriver::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 GS1LidarDriver::disableDataGrabbing() ++ { ++ { ++ if (m_isScanning) ++ { ++ m_isScanning = false; ++ _dataEvent.set(); ++ } ++ } ++ _thread.join(); ++ } ++ ++ bool GS1LidarDriver::isscanning() const ++ { ++ return m_isScanning; ++ } ++ bool GS1LidarDriver::isconnected() const ++ { ++ return m_isConnected; ++ } ++ ++ result_t GS1LidarDriver::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 GS1LidarDriver::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 += addr; ++ 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 GS1LidarDriver::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 GS1LidarDriver::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) ++ { ++ return RESULT_FAIL; ++ } ++ ++ // printf("recv: "); ++ // printHex(data, r); ++ ++ size -= r; ++ data += r; ++ } ++ ++ return RESULT_OK; ++ } ++ ++ result_t GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 > 50) ++ { ++ retryConnect = 50; ++ } ++ ++ 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 GS1LidarDriver::cacheScanData() ++ { ++ node_info local_buf[MaxPointsPerPackge_GS1]; ++ size_t count = MaxPointsPerPackge_GS1; ++ 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 = MaxPointsPerPackge_GS1; ++ 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, "timeout 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) * MaxPointsPerPackge_GS1); ++ break; ++ } ++ } ++ ++ _lock.lock(); // timeout lock, wait resource copys ++ scan_node_buf[0].stamp = local_buf[0].stamp; ++ scan_node_buf[0].scan_frequence = local_buf[0].scan_frequence; ++ scan_node_buf[0].index = 0x03 & moduleNum >> 1; //gs1: 1, 2, 4 ++ scan_node_count = MaxPointsPerPackge_GS1; //一个包固定216个数据 ++ // 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 GS1LidarDriver::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; ++ ++ 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: ++ if (currentByte == LIDAR_ANS_SYNC_BYTE1) //如果出现超过4个包头标识的情况 ++ { ++ recvPos = 4; ++ continue; ++ } ++ 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; ++ moduleNum = 0; ++ has_device_header = false; ++ 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; ++ ++ // printf("env: 0x%04X\n", package.BackgroudLight); ++ // fflush(stdout); ++ } ++ } ++ ++ (*node).stamp = getTime(); ++ ++ if (CheckSumResult) ++ { ++ (*node).index = 0x03 & (moduleNum >> 1); ++ (*node).scan_frequence = scan_frequence; ++ (*node).sync_quality = 0; ++ ++ (*node).distance_q2 = ++ package.packageSample[package_Sample_Index].PakageSampleDistance; ++ ++ if (m_intensities) ++ { ++ (*node).sync_quality = (uint16_t)package.packageSample[package_Sample_Index].PakageSampleQuality; ++ } ++ ++ double sampleAngle = 0; ++ 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; ++ } ++ } ++ ++ //处理环境数据（2个字节分别存储在两个点的is属性中） ++ if (0 == package_Sample_Index) ++ (*node).is = package.BackgroudLight & 0xFF; ++ else if (1 == package_Sample_Index) ++ (*node).is = package.BackgroudLight >> 8; ++ ++ // printf("%s [%d] : Out : %u %u %u\n", __FUNCTION__, __LINE__, package_Sample_Index, node->distance_q2, node->angle_q6_checkbit); ++ } ++ else ++ { ++ (*node).sync_flag = Node_NotSync; ++ (*node).sync_quality = 0; ++ (*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT; ++ (*node).distance_q2 = 0; ++ (*node).scan_frequence = 0; ++ return RESULT_FAIL; ++ } ++ ++ package_Sample_Index ++; ++ (*node).sync_flag = Node_NotSync; ++ ++ if (package_Sample_Index >= MaxPointsPerPackge_GS1) ++ { ++ package_Sample_Index = 0; ++ (*node).sync_flag = Node_Sync; ++ CheckSumResult = false; ++ } ++ ++ return RESULT_OK; ++ } ++ ++ void GS1LidarDriver::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 ++ ++ tempTheta = atan(d_compensateK0[mdNum] * pixelU - d_compensateB0[mdNum]) * 180 / M_PI; ++ Dist = dist / cos(tempTheta * M_PI / 180); ++ theta = tempTheta; ++ ++ if (theta < 0) ++ { ++ theta += 360; ++ } ++ *dstTheta = theta; ++ *dstDist = Dist; ++ } ++ ++ void GS1LidarDriver::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) ++ { ++ GS1_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 GS1LidarDriver::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; ++ ++ if (!package_Sample_Index) ++ { ++ ++ size_t size = _serial->available(); ++ uint64_t delayTime = 0; ++ size_t PackageSize = sizeof(gs1_node_package); ++ ++ 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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::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 GS1LidarDriver::setIntensities(const bool &i) ++ { ++ m_intensities = i; ++ // if (m_intensities != isintensities) ++ // { ++ // if (globalRecvBuffer) ++ // { ++ // delete[] globalRecvBuffer; ++ // globalRecvBuffer = NULL; ++ // } ++ ++ // globalRecvBuffer = new uint8_t[sizeof(gs1_node_package)]; ++ // } ++ ++ // m_intensities = isintensities; ++ ++ // if (m_intensities) ++ // { ++ // PackageSampleBytes = 2; ++ // } ++ // else ++ // { ++ // PackageSampleBytes = 2; ++ // } ++ } ++ /** ++ * @brief 设置雷达异常自动重新连接 \n ++ * @param[in] enable 是否开启自动重连: ++ * true 开启 ++ * false 关闭 ++ */ ++ void GS1LidarDriver::setAutoReconnect(const bool &enable) ++ { ++ isAutoReconnect = enable; ++ } ++ ++ void GS1LidarDriver::checkTransDelay() ++ { ++ //采样率 ++ trans_delay = _serial->getByteTime(); ++ sample_rate = 27 * MaxPointsPerPackge_GS1; ++ m_PointTime = 1e9 / sample_rate; ++ } ++ ++ /************************************************************************/ ++ /* start to scan */ ++ /************************************************************************/ ++ result_t GS1LidarDriver::startScan(bool force, uint32_t timeout) ++ { ++ result_t ans; ++ ++ if (!m_isConnected) ++ { ++ return RESULT_FAIL; ++ } ++ ++ if (m_isScanning) ++ { ++ return RESULT_OK; ++ } ++ ++ stop(); ++ checkTransDelay(); ++ flushSerial(); ++ ++ //配置GS1模组地址（三个模组） ++ // setDeviceAddress(300); ++ ++ //获取GS1参数 ++ gs_device_para gs1_info; ++ // delay(30); ++ getDevicePara(gs1_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 GS1LidarDriver::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); ++ printf("GS1LidarDriver stop lidar successfully\n"); ++ ++ return RESULT_OK; ++ } ++ ++ result_t GS1LidarDriver::createThread() ++ { ++ _thread = CLASS_THREAD(GS1LidarDriver, cacheScanData); ++ ++ if (_thread.getHandle() == 0) ++ { ++ m_isScanning = false; ++ return RESULT_FAIL; ++ } ++ ++ m_isScanning = true; ++ return RESULT_OK; ++ } ++ ++ result_t GS1LidarDriver::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 GS1LidarDriver::stop() ++ { ++ if (isAutoconnting) ++ { ++ isAutoReconnect = false; ++ m_isScanning = false; ++ } ++ ++ disableDataGrabbing(); ++ stopScan(); ++ ++ return RESULT_OK; ++ } ++ ++ /************************************************************************/ ++ /* reset device */ ++ /************************************************************************/ ++ result_t GS1LidarDriver::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 GS1LidarDriver::getSDKVersion() ++ { ++ return YDLIDAR_SDK_VERSION_STR; ++ } ++ ++ std::map GS1LidarDriver::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 *GS1LidarDriver::DescribeError(bool isTCP) ++ { ++ if (_serial) ++ { ++ return _serial->DescribeError(); ++ } ++ return nullptr; ++ } ++ ++ result_t GS1LidarDriver::getHealth(device_health &, uint32_t) ++ { ++ return RESULT_OK; ++ } ++ ++ result_t GS1LidarDriver::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_GS1; ++ 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; ++ } ++ ++ result_t GS1LidarDriver::setWorkMode(int mode, uint8_t addr) ++ { ++ result_t ans; ++ uint32_t timeout = 300; ++ string buf; ++ ++ if (!isconnected()) ++ { ++ return RESULT_FAIL; ++ } ++ ++ //如果已经开启扫描，则先停止扫描 ++ if (isscanning()) ++ { ++ disableDataGrabbing(); ++ delay(10); ++ stopScan(); ++ } ++ flushSerial(); ++ ++ { ++ ScopedLocker l(_lock); ++ uint8_t m = uint8_t(mode); ++ if ((ans = sendCommand(addr, GS_LIDAR_CMD_SET_MODE, &m, 1)) != RESULT_OK) ++ { ++ return ans; ++ } ++ gs_lidar_ans_header response_header; ++ if ((ans = waitResponseHeaderEx(&response_header, GS_LIDAR_CMD_SET_MODE, timeout)) != RESULT_OK) ++ { ++ return ans; ++ } ++ if (response_header.type != GS_LIDAR_CMD_SET_MODE) ++ { ++ return RESULT_FAIL; ++ } ++ } ++ ++ return RESULT_OK; ++ } ++ ++} +diff --git a/src/GS1LidarDriver.h b/src/GS1LidarDriver.h +new file mode 100644 +index 0000000..c49ce81 +--- /dev/null ++++ b/src/GS1LidarDriver.h +@@ -0,0 +1,589 @@ ++/********************************************************************* ++* 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 GS1LidarDriver ++ * GS1LidarDriver API ++ ++
Library GS1LidarDriver ++
File GS1LidarDriver.h ++
Author Tony [code at ydlidar com] ++
Source https://github.com/ydlidar/YDLidar-SDK ++
Version 1.0.0 ++
++ This GS1LidarDriver 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::GS1LidarDriver @endlink interface documentation. ++*/ ++#ifndef GS1_YDLIDAR_DRIVER_H ++#define GS1_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; ++ ++/*! ++* GS1操控类 ++*/ ++class GS1LidarDriver : 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 Failed 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 ++
GS1 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) ++
GS1 [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. ++ */ ++ GS1LidarDriver(); ++ ++ /*! ++ * A destructor. ++ * A more elaborate description of the destructor. ++ */ ++ virtual ~GS1LidarDriver(); ++ ++ /*! ++ * @brief 连接雷达 \n ++ * 连接成功后，必须使用::disconnect函数关闭 ++ * @param[in] port_path 串口号 ++ * @param[in] baudrate 波特率，YDLIDAR-GS1 雷达波特率：961200 ++ * @return 返回连接状态 ++ * @retval 0 成功 ++ * @retval < 0 失败 ++ * @note连接成功后，必须使用::disconnect函数关闭 ++ * @see 函数::GS1LidarDriver::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 GS1雷达没有健康信息\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); ++ ++ /** ++ * @brief 设置雷达工作模式（目前只针对GS1雷达） ++ * @param[in] mode 雷达工作模式，0为避障模式；1为延边模式 ++ * @param[in] addr 雷达地址，第1个雷达地址为0x01；第2个雷达地址为0x02；第3个雷达地址为0x04； ++ * @return 成功返回RESULT_OK，否则返回非RESULT_OK ++ */ ++ virtual result_t setWorkMode(int mode=0, uint8_t addr=0x00); ++ ++ //未实现的虚函数 ++ 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; ///< ++ ++ gs1_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 // GS1_YDLIDAR_DRIVER_H +diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp +index 7c1be54..d64e9ad 100644 +--- a/src/YDlidarDriver.cpp ++++ b/src/YDlidarDriver.cpp +@@ -1296,6 +1296,10 @@ void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) + (*node).sync_flag = Node_Sync; + package_index = 0; + ++ // printf("start angle %f end angle %f\n", ++ // float(FirstSampleAngle) / 64.0, ++ // float(LastSampleAngle) / 64.0); ++ + if (CheckSumResult) { + has_package_error = false; + (*node).index = package_index; +-- +2.34.1 + -- Gitee