├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── ldlidar_driver │ ├── ldlidar_dataprocess.h │ ├── ldlidar_datatype.h │ ├── ldlidar_driver.h │ ├── ldlidar_driver_linux.h │ ├── ldlidar_driver_win.h │ ├── ldlidar_protocol.h │ ├── log_module.h │ ├── network_socket_interface_linux.h │ ├── serial_interface_linux.h │ ├── serial_interface_win.h │ ├── sl_transform.h │ ├── slbf.h │ └── tofbf.h ├── sample ├── linux │ ├── CMakeLists.txt │ ├── README.md │ ├── README_CN.md │ ├── auto_build.sh │ ├── clean_build.sh │ └── demo.cpp └── windows │ ├── CMakeLists.txt │ ├── README.md │ ├── README_CN.md │ ├── pic │ ├── 1.png │ ├── 2.png │ ├── 3.png │ ├── 4.png │ ├── 5.png │ └── 6.png │ └── windemo.cpp ├── src ├── ldlidar_dataprocess.cpp ├── ldlidar_driver.cpp ├── ldlidar_driver_linux.cpp ├── ldlidar_driver_win.cpp ├── ldlidar_protocol.cpp ├── log_module.cpp ├── network_socket_interface_linux.cpp ├── serial_interface_linux.cpp ├── serial_interface_win.cpp ├── sl_transform.cpp ├── slbf.cpp └── tofbf.cpp └── test ├── CMakeLists.txt ├── auto_build_test.sh ├── clean_build.sh └── test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.json 2 | build/** 3 | sample/windows/build/** -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # sub project 2 | message(STATUS "operation system is ${CMAKE_SYSTEM}") 3 | 4 | if(CMAKE_SYSTEM_NAME MATCHES "Linux") 5 | message(STATUS "current platform: Linux ") 6 | 7 | set(LDLIDAR_DRIVER_SOURCE_LINUX 8 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_driver.cpp 9 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_driver_linux.cpp 10 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_dataprocess.cpp 11 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_protocol.cpp 12 | ${CMAKE_CURRENT_SOURCE_DIR}/src/log_module.cpp 13 | ${CMAKE_CURRENT_SOURCE_DIR}/src/network_socket_interface_linux.cpp 14 | ${CMAKE_CURRENT_SOURCE_DIR}/src/serial_interface_linux.cpp 15 | ${CMAKE_CURRENT_SOURCE_DIR}/src/sl_transform.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slbf.cpp 17 | ${CMAKE_CURRENT_SOURCE_DIR}/src/tofbf.cpp 18 | ) 19 | 20 | add_library(ldlidar_driver STATIC 21 | ${LDLIDAR_DRIVER_SOURCE_LINUX} 22 | ) 23 | 24 | elseif(CMAKE_SYSTEM_NAME MATCHES "Windows") 25 | message(STATUS "current platform: Windows") 26 | 27 | set(LDLIDAR_DRIVER_SOURCE_WIN 28 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_driver.cpp 29 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_driver_win.cpp 30 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_dataprocess.cpp 31 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ldlidar_protocol.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/src/log_module.cpp 33 | ${CMAKE_CURRENT_SOURCE_DIR}/src/serial_interface_win.cpp 34 | ${CMAKE_CURRENT_SOURCE_DIR}/src/sl_transform.cpp 35 | ${CMAKE_CURRENT_SOURCE_DIR}/src/slbf.cpp 36 | ${CMAKE_CURRENT_SOURCE_DIR}/src/tofbf.cpp 37 | ) 38 | 39 | add_library(ldlidar_driver STATIC 40 | ${LDLIDAR_DRIVER_SOURCE_WIN} 41 | ) 42 | 43 | else() 44 | message(STATUS "other platform: ${CMAKE_SYSTEM_NAME}") 45 | endif (CMAKE_SYSTEM_NAME MATCHES "Linux") 46 | 47 | 48 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 SHENZHEN LDROBOT CO., LTD. All rights 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LDLiDAR SDK 2 | This SDK is only applicable to the LiDAR products sold by Shenzhen LDROBOT Co., LTD. 3 | | product models | range types | 4 | | ---- | ---- | 5 | | LDROBOT LiDAR LD06 | 2D DTOF | 6 | | LDROBOT LiDAR LD19 | 2D DTOF | 7 | | LDROBOT LiDAR LD14 | 2D Triangle| 8 | | LDROBOT LiDAR LD14P | 2D Triangle| 9 | 10 | --- 11 | ## Get SDK 12 | - download package file or use git. 13 | ```bash 14 | $ cd ~ 15 | 16 | $ mkdir ldlidar_ws 17 | 18 | $ cd ldlidar_ws 19 | 20 | $ git clone https://github.com/ldrobotSensorTeam/ldlidar_sdk.git 21 | ``` 22 | 23 | --- 24 | ## Introduction to linux application example 25 | - [EN](./sample/linux/README.md) 26 | - [CN](./sample/linux/README_CN.md) 27 | 28 | --- 29 | 30 | ## Introduction to windows application example 31 | - [EN](./sample/windows/README.md) 32 | - [CN](./sample/windows/README_CN.md) 33 | -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_dataprocess.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * David Hu (hmd_hubei_cn@163.com) 5 | * @brief LiDAR data protocol processing App 6 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 7 | * products sold by Shenzhen LDROBOT Co., LTD 8 | * @version 1.0 9 | * @date 2023-03-12 10 | * 11 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 12 | * reserved. 13 | * Licensed under the MIT License (the "License"); 14 | * you may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License in the file LICENSE 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | #ifndef __LDLIDAR_DATAPROCESS_H__ 23 | #define __LDLIDAR_DATAPROCESS_H__ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "ldlidar_driver/slbf.h" 32 | #include "ldlidar_driver/sl_transform.h" 33 | #include "ldlidar_driver/tofbf.h" 34 | #include "ldlidar_driver/ldlidar_protocol.h" 35 | 36 | namespace ldlidar { 37 | 38 | class LdLidarDataProcess { 39 | public: 40 | LdLidarDataProcess(); 41 | 42 | ~LdLidarDataProcess(); 43 | 44 | void SetProductType(LDType typenumber); 45 | 46 | void SetNoiseFilter(bool is_enable); 47 | 48 | void RegisterTimestampGetFunctional(std::function timestamp_handle); 49 | 50 | void CommReadCallback(const char *byte, size_t len); 51 | 52 | /** 53 | * @brief get lidar scan data 54 | */ 55 | bool GetLaserScanData(Points2D& out); 56 | 57 | /** 58 | * @brief get Lidar spin speed (Hz) 59 | */ 60 | double GetSpeed(void); 61 | 62 | LidarStatus GetLidarStatus(void); 63 | 64 | uint8_t GetLidarErrorCode(void); 65 | 66 | bool GetLidarPowerOnCommStatus(void); 67 | 68 | void ClearDataProcessStatus(void) { 69 | is_frame_ready_ = false; 70 | is_poweron_comm_normal_ = false; 71 | lidarstatus_ = LidarStatus::NORMAL; 72 | lidarerrorcode_ = LIDAR_NO_ERROR; 73 | last_pkg_timestamp_ = 0; 74 | lidar_scan_data_vec_.clear(); 75 | tmp_lidar_scan_data_vec_.clear(); 76 | } 77 | 78 | private: 79 | int lidar_measure_freq_; 80 | LDType typenumber_; 81 | LidarStatus lidarstatus_; 82 | uint8_t lidarerrorcode_; 83 | bool is_frame_ready_; 84 | bool is_noise_filter_; 85 | uint16_t timestamp_; 86 | double speed_; 87 | std::function get_timestamp_; 88 | bool is_poweron_comm_normal_; 89 | uint64_t last_pkg_timestamp_; 90 | 91 | LdLidarProtocol* protocol_handle_; 92 | Points2D lidar_scan_data_vec_; 93 | Points2D tmp_lidar_scan_data_vec_; 94 | std::mutex mutex_lock1_; 95 | std::mutex mutex_lock2_; 96 | 97 | void SetLidarStatus(LidarStatus status); 98 | 99 | void SetLidarErrorCode(uint8_t errorcode); 100 | 101 | bool AnalysisOne(uint8_t byte); // parse single packet 102 | 103 | bool Parse(const uint8_t *data, long len); 104 | 105 | bool AssemblePacket(); // combine stantard data into data frames and calibrate 106 | 107 | bool IsFrameReady(void); // get Lidar data frame ready flag 108 | 109 | void ResetFrameReady(void); // reset frame ready flag 110 | 111 | void SetFrameReady(void); // set frame ready flag 112 | 113 | void SetLaserScanData(Points2D& src); 114 | 115 | Points2D GetLaserScanData(void); 116 | }; 117 | 118 | } // namespace ldlidar 119 | 120 | #endif // __LDLIDAR_DATAPROCESS_H__ 121 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 122 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_datatype.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_datatype.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief lidar point data structure 5 | * This code is only applicable to LDROBOT products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef _LDLIDAR_POINT_DATA_H_ 22 | #define _LDLIDAR_POINT_DATA_H_ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #define LDLiDAR_SDK_VERSION_NUMBER "3.3.1" 31 | 32 | #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) 33 | 34 | // lidar error code definition 35 | #define LIDAR_NO_ERROR 0x00 36 | #define LIDAR_ERROR_BLOCKING 0x01 /* 雷达堵转 */ 37 | #define LIDAR_ERROR_OCCLUSION 0x02 /* 雷达遮挡 */ 38 | #define LIDAR_ERROR_BLOCKING_AND_OCCLUSION 0x03 /* 雷达堵转且遮挡 */ 39 | // end lidar error code definition 40 | 41 | namespace ldlidar { 42 | 43 | enum class LDType { 44 | NO_VER, 45 | LD_14, 46 | LD_14P, 47 | LD_06, 48 | LD_19, 49 | STL_06P, 50 | STL_26, 51 | STL_27L, 52 | }; 53 | 54 | enum class LidarStatus { 55 | NORMAL, /* 雷达正常,可获取点云数据 */ 56 | ERROR, /* 表明雷达出现异常错误，可获取雷达反馈的错误码了解具体错误 */ 57 | DATA_TIME_OUT, /* 雷达点云数据包发布超时 */ 58 | DATA_WAIT, /* 雷达点云数据包发布等待 */ 59 | STOP, /* 雷达停止转动、未启动Driver */ 60 | }; 61 | 62 | struct PointData { 63 | // Polar coordinate representation 64 | float angle; // Angle ranges from 0 to 359 degrees 65 | uint16_t distance; // Distance is measured in millimeters 66 | uint8_t intensity; // Intensity is 0 to 255 67 | //! System time when first range was measured in nanoseconds 68 | uint64_t stamp; 69 | // Cartesian coordinate representation 70 | double x; 71 | double y; 72 | PointData(float angle, uint16_t distance, uint8_t intensity, uint64_t stamp = 0, double x = 0, double y = 0) { 73 | this->angle = angle; 74 | this->distance = distance; 75 | this->intensity = intensity; 76 | this->stamp = stamp; 77 | this->x = x; 78 | this->y = y; 79 | } 80 | PointData() {} 81 | }; 82 | 83 | typedef std::vector Points2D; 84 | 85 | struct LaserScan { 86 | //! System time when first range was measured in nanoseconds 87 | uint64_t stamp; 88 | //! Array of laser point 89 | Points2D points; 90 | 91 | LaserScan &operator=(const LaserScan &data) { 92 | this->stamp = data.stamp; 93 | this->points = data.points; 94 | return *this; 95 | } 96 | }; 97 | 98 | 99 | } // namespace ldlidar 100 | 101 | #endif // _LDLIDAR_POINT_DATA_H_ 102 | 103 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 104 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_driver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-05-12 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __LDLIDAR_DRIVER_SDK_INTERFACE_H__ 22 | #define __LDLIDAR_DRIVER_SDK_INTERFACE_H__ 23 | 24 | #include 25 | #include 26 | 27 | #include "ldlidar_driver/ldlidar_datatype.h" 28 | 29 | namespace ldlidar { 30 | 31 | class LDLidarDriver { 32 | public: 33 | LDLidarDriver(); 34 | 35 | virtual ~LDLidarDriver(); 36 | 37 | std::string GetLidarSdkVersionNumber(void); 38 | 39 | virtual void EnablePointCloudDataFilter(bool is_enable) = 0; 40 | 41 | virtual bool WaitLidarComm(int64_t timeout) = 0; 42 | 43 | virtual LidarStatus GetLaserScanData(Points2D& dst, int64_t timeout) = 0; 44 | 45 | virtual LidarStatus GetLaserScanData(LaserScan& dst, int64_t timeout) = 0; 46 | 47 | virtual bool GetLidarScanFreq(double& spin_hz) = 0; 48 | 49 | virtual void RegisterGetTimestampFunctional(std::function get_timestamp_handle) = 0; 50 | 51 | virtual uint8_t GetLidarErrorCode(void) = 0; 52 | 53 | /** 54 | * @brief Start lidar driver node 55 | * @param none 56 | * @retval value is true, start is success; 57 | * value is false, start is failed. 58 | */ 59 | virtual bool Start(void) = 0; 60 | 61 | /** 62 | * @brief Stop lidar driver node 63 | * @param none 64 | * @retval value is true, stop is success; 65 | * value is false, stop is failed. 66 | */ 67 | virtual bool Stop(void) = 0; 68 | 69 | /** 70 | * @brief Get SDK(ldlidar driver) running status. 71 | */ 72 | static bool Ok(); 73 | 74 | /** 75 | * @brief Set SDK(ldlidar driver) running status. 76 | */ 77 | static void SetLidarDriverStatus(bool status); 78 | 79 | protected: 80 | bool is_start_flag_; 81 | bool is_connect_flag_; 82 | 83 | private: 84 | std::string sdk_pack_version_; 85 | static bool is_ok_; 86 | }; 87 | 88 | } // namespace ldlidar 89 | 90 | #endif // __LDLIDAR_DRIVER_NODE_H__ 91 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 92 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_driver_linux.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2023-03 9 | * 10 | * @copyright Copyright (c) 20017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __LDLIDAR_DRIVER_SDK_LINUX_INTERFACE_H__ 22 | #define __LDLIDAR_DRIVER_SDK_LINUX_INTERFACE_H__ 23 | 24 | #include "ldlidar_driver/ldlidar_driver.h" 25 | #include "ldlidar_driver/ldlidar_dataprocess.h" 26 | #include "ldlidar_driver/serial_interface_linux.h" 27 | #include "ldlidar_driver/network_socket_interface_linux.h" 28 | #include "ldlidar_driver/log_module.h" 29 | 30 | namespace ldlidar { 31 | 32 | typedef enum CommunicationMode { 33 | COMM_NO_NULL, 34 | COMM_SERIAL_MODE, /* serial communication */ 35 | COMM_UDP_CLIENT_MODE, /* network communication for UDP client */ 36 | COMM_UDP_SERVER_MODE, /* network communication for UDP server */ 37 | COMM_TCP_CLIENT_MODE, /* network communication for TCP client */ 38 | COMM_TCP_SERVER_MODE /* network communication for TCP server */ 39 | }CommunicationModeType; 40 | 41 | class LDLidarDriverLinuxInterface : public LDLidarDriver { 42 | public: 43 | LDLidarDriverLinuxInterface(); 44 | 45 | ~LDLidarDriverLinuxInterface(); 46 | 47 | /** 48 | * @brief Communcation port open and assert initlization param 49 | * @param product_name 50 | * ldlidar product type: ldlidar::LDType, value: 51 | * - ldlidar::LDType::NOVER 52 | * - ldlidar::LDType::LD_14 53 | * .... 54 | * - else definition in "ldlidar_driver/include/ldlidar_datatype.h" 55 | * @param serial_port_name 56 | * serial device system path, eg: "/dev/ttyUSB0" 57 | * @param serial_baudrate 58 | * serial communication baudrate value(> 0), unit is bit/s. 59 | * @retval value is true, start is success; 60 | * value is false, start is failed. 61 | */ 62 | bool Connect(LDType product_name, 63 | std::string serial_port_name, 64 | uint32_t serial_baudrate = 115200, 65 | CommunicationModeType comm_mode = COMM_SERIAL_MODE); 66 | 67 | bool Connect(LDType product_name, 68 | const char* server_ip, 69 | const char* server_port, 70 | CommunicationModeType comm_mode = COMM_TCP_CLIENT_MODE); 71 | 72 | bool Disconnect(void); 73 | 74 | void EnablePointCloudDataFilter(bool is_enable) override; 75 | 76 | /** 77 | * @brief Whether the connection of the communication channel is normal after the lidar is powered on 78 | * @param[in] 79 | * *@param timeout: Wait timeout, in milliseconds 80 | * @retval if times >= 1000, return false, communication connection is fail; 81 | * if "times < 1000", return ture, communication connection is successful. 82 | */ 83 | bool WaitLidarComm(int64_t timeout = 1000) override; 84 | 85 | /** 86 | * @brief get lidar laser scan point cloud data 87 | * @param [output] 88 | * *@param dst: type is ldlidar::Point2D, value is laser scan point cloud data 89 | * @param [in] 90 | * *@param timeout: Wait timeout, in milliseconds 91 | * @retval value is ldlidar::LidarStatus Enum Type, definition in "include/ldlidar_datatype.h", value is: 92 | * ldlidar::LidarStatus::NORMAL // 雷达正常 93 | * lldlidar::LidarStatus::ERROR // 雷达异常错误 94 | * .... 95 | */ 96 | LidarStatus GetLaserScanData(Points2D& dst, int64_t timeout = 1000) override; 97 | 98 | LidarStatus GetLaserScanData(LaserScan& dst, int64_t timeout = 1000) override; 99 | 100 | /** 101 | * @brief get lidar scan frequence 102 | * @param [output] 103 | * *@param spin_hz: value is lidar scan frequence, unit is Hz 104 | * @retval value is true, get sucess; 105 | */ 106 | bool GetLidarScanFreq(double& spin_hz) override; 107 | 108 | /** 109 | * @brief register get timestamp handle functional. 110 | * @param [input] 111 | * *@param get_timestamp_handle: type is `uint64_t (*)(void)`, value is pointer get timestamp fuction. 112 | * @retval none 113 | */ 114 | void RegisterGetTimestampFunctional(std::function get_timestamp_handle) override; 115 | 116 | /** 117 | * @brief When the lidar is in an error state, get the error code fed back by the lidar 118 | * @param none 119 | * @retval errcode 120 | */ 121 | uint8_t GetLidarErrorCode(void) override; 122 | 123 | /** 124 | * @brief Start lidar driver node 125 | * @param none 126 | * @retval value is true, start is success; 127 | * value is false, start is failed. 128 | */ 129 | bool Start(void) override; 130 | 131 | /** 132 | * @brief Stop lidar driver node 133 | * @param none 134 | * @retval value is true, stop is success; 135 | * value is false, stop is failed. 136 | */ 137 | bool Stop(void) override; 138 | 139 | static LDLidarDriverLinuxInterface* Create(void) { 140 | LDLidarDriverLinuxInterface* pdrv = new LDLidarDriverLinuxInterface(); 141 | return pdrv; 142 | } 143 | 144 | static void Destory(LDLidarDriverLinuxInterface* pdrv) { 145 | if (pdrv != nullptr) { 146 | delete pdrv; 147 | } 148 | } 149 | 150 | private: 151 | LdLidarDataProcess* comm_pkg_; 152 | SerialInterfaceLinux* comm_serial_; 153 | TCPSocketInterfaceLinux* comm_tcp_network_; 154 | UDPSocketInterfaceLinux* comm_udp_network_; 155 | std::function register_get_timestamp_handle_; 156 | std::chrono::_V2::steady_clock::time_point last_pubdata_times_; 157 | }; 158 | 159 | } // namespace ldlidar 160 | 161 | #endif // __LDLIDAR_DRIVER_NODE_H__ 162 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 163 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_driver_win.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2023-03 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __LDLIDAR_DRIVER_SDK_WIN_INTERFACE_H__ 22 | #define __LDLIDAR_DRIVER_SDK_WIN_INTERFACE_H__ 23 | 24 | #include "ldlidar_driver/ldlidar_driver.h" 25 | #include "ldlidar_driver/ldlidar_dataprocess.h" 26 | #include "ldlidar_driver/log_module.h" 27 | #include "ldlidar_driver/serial_interface_win.h" 28 | 29 | namespace ldlidar { 30 | 31 | class LDLidarDriverWinInterface : public LDLidarDriver { 32 | public: 33 | LDLidarDriverWinInterface(); 34 | 35 | ~LDLidarDriverWinInterface(); 36 | 37 | /** 38 | * @brief Communcation port open and assert initlization param 39 | * @param product_name 40 | * ldlidar product type: ldlidar::LDType, value: 41 | * - ldlidar::LDType::NOVER 42 | * - ldlidar::LDType::LD_14 43 | * .... 44 | * - else definition in "ldlidar_driver/include/ldlidar_datatype.h" 45 | * @param serial_port_name 46 | * serial device system path, eg: "/dev/ttyUSB0" 47 | * @param serial_baudrate 48 | * serial communication baudrate value(> 0), unit is bit/s. 49 | * @retval value is true, start is success; 50 | * value is false, start is failed. 51 | */ 52 | bool Connect(LDType product_name, 53 | std::string& serial_port_name, 54 | PortParams& port_params); 55 | 56 | /** 57 | * @brief Communication port close 58 | * @retval value is ture, close success. 59 | * value is false, close fail. 60 | */ 61 | bool Disconnect(void); 62 | 63 | void EnablePointCloudDataFilter(bool is_enable) override; 64 | 65 | /** 66 | * @brief Whether the connection of the communication channel is normal after the lidar is powered on 67 | * @param[in] 68 | * *@param timeout: Wait timeout, in milliseconds 69 | * @retval if times >= 1000, return false, communication connection is fail; 70 | * if "times < 1000", return ture, communication connection is successful. 71 | */ 72 | bool WaitLidarComm(int64_t timeout = 1000) override; 73 | 74 | /** 75 | * @brief get lidar laser scan point cloud data 76 | * @param [output] 77 | * *@param dst: type is ldlidar::Point2D, value is laser scan point cloud data 78 | * @param [in] 79 | * *@param timeout: Wait timeout, in milliseconds 80 | * @retval value is ldlidar::LidarStatus Enum Type, definition in "include/ldlidar_datatype.h", value is: 81 | * ldlidar::LidarStatus::NORMAL // 雷达正常 82 | * lldlidar::LidarStatus::ERROR // 雷达异常错误 83 | * .... 84 | */ 85 | LidarStatus GetLaserScanData(Points2D& dst, int64_t timeout = 1000) override; 86 | 87 | LidarStatus GetLaserScanData(LaserScan& dst, int64_t timeout = 1000) override; 88 | 89 | /** 90 | * @brief get lidar scan frequence 91 | * @param [output] 92 | * *@param spin_hz: value is lidar scan frequence, unit is Hz 93 | * @retval value is true, get sucess; 94 | */ 95 | bool GetLidarScanFreq(double& spin_hz) override; 96 | 97 | /** 98 | * @brief register get timestamp handle functional. 99 | * @param [input] 100 | * *@param get_timestamp_handle: type is `uint64_t (*)(void)`, value is pointer get timestamp fuction. 101 | * @retval none 102 | */ 103 | void RegisterGetTimestampFunctional(std::function get_timestamp_handle) override; 104 | 105 | /** 106 | * @brief When the lidar is in an error state, get the error code fed back by the lidar 107 | * @param none 108 | * @retval errcode 109 | */ 110 | uint8_t GetLidarErrorCode(void) override; 111 | 112 | /** 113 | * @brief Start lidar driver node 114 | * @param none 115 | * @retval value is true, start is success; 116 | * value is false, start is failed. 117 | */ 118 | bool Start(void) override; 119 | 120 | /** 121 | * @brief Stop lidar driver node 122 | * @param none 123 | * @retval value is true, stop is success; 124 | * value is false, stop is failed. 125 | */ 126 | bool Stop(void) override; 127 | 128 | static LDLidarDriverWinInterface* Create(void) { 129 | LDLidarDriverWinInterface* pdrv = new LDLidarDriverWinInterface(); 130 | return pdrv; 131 | } 132 | 133 | static void Destory(LDLidarDriverWinInterface* pdrv) { 134 | if (pdrv != nullptr) { 135 | delete pdrv; 136 | } 137 | } 138 | 139 | private: 140 | // bool is_start_flag_; 141 | // bool is_connect_flag_; 142 | LdLidarDataProcess* comm_pkg_; 143 | SerialInterfaceWin* comm_port_handle_; 144 | std::function register_get_timestamp_handle_; 145 | std::chrono::steady_clock::time_point last_pubdata_times_; 146 | }; 147 | 148 | } // namespace ldlidar 149 | 150 | #endif // __LDLIDAR_DRIVER_NODE_H__ 151 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 152 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/ldlidar_protocol.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_protocol.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * David Hu(hmd_hubei_cn@163.com) 5 | * @brief 6 | * @version 1.0 7 | * @date 2023.3.11 8 | * @note 9 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | **/ 19 | 20 | #ifndef __LDLIDAR_PROTOCOL_H__ 21 | #define __LDLIDAR_PROTOCOL_H__ 22 | 23 | #include 24 | #include 25 | 26 | namespace ldlidar { 27 | 28 | struct CmdIdAckFlag { 29 | bool is_ack_start; 30 | bool is_ack_stop; 31 | bool is_ack_set_scan_freq; 32 | bool is_ack_get_motor_board_ver; 33 | 34 | CmdIdAckFlag() { 35 | is_ack_start = false; 36 | is_ack_stop = false; 37 | is_ack_set_scan_freq = false; 38 | is_ack_get_motor_board_ver = false; 39 | } 40 | 41 | void Clear() { 42 | is_ack_start = false; 43 | is_ack_stop = false; 44 | is_ack_set_scan_freq = false; 45 | is_ack_get_motor_board_ver = false; 46 | } 47 | }; 48 | 49 | #define CMD_ACK_PROTOCOL_DATABUFF_MAX_SIZE 50 50 | #define CMD_ACK_PROTOCOL_DATABUFF_MIN_SIZE 4 51 | #define CMD_ACK_PROTOCOL_DATABUFF_INDEX 3 52 | #define CMD_ACK_PROTOCOL_NO_DATABUFF_SIZE 4 53 | 54 | typedef enum LidarCmdModeEnum { 55 | SET_START_LIDAR_SCAN = 0xA0, 56 | SET_STOP_LIDAR_SCAN = 0xA1, 57 | SET_LIDAR_SCAN_FRE = 0xA2, 58 | GET_LIDAR_MOTOR_BOARD_FIRMWARE = 0xA8 59 | } LidarCmdIDType; 60 | 61 | struct LidarCmdAckData { 62 | LidarCmdIDType cmd_mode; 63 | uint8_t databuff[CMD_ACK_PROTOCOL_DATABUFF_MAX_SIZE]; 64 | }; 65 | 66 | #ifdef __linux__ 67 | 68 | typedef struct __attribute__((packed)) { 69 | uint8_t header; 70 | uint8_t mode; 71 | uint8_t datalen; 72 | uint32_t data; 73 | uint8_t crc8; 74 | } LidarCmdDataType; 75 | 76 | #else 77 | 78 | #pragma pack(1) 79 | typedef struct { 80 | uint8_t header; 81 | uint8_t mode; 82 | uint8_t datalen; 83 | uint32_t data; 84 | uint8_t crc8; 85 | } LidarCmdDataType; 86 | #pragma pack() 87 | 88 | #endif 89 | 90 | // ---- 91 | 92 | #define PKG_HEADER 0x54 93 | #define DATA_PKG_INFO 0x2C 94 | #define POINT_PER_PACK 12 95 | #define HEALTH_PKG_INFO 0xE0 96 | #define MANUFACT_PKG_INF 0x0F 97 | 98 | #define GET_PKG_PCD 1 99 | #define GET_PKG_HEALTH 2 100 | #define GET_PKG_MANUFACT 3 101 | #define GET_PKG_ERROR 0 102 | 103 | #ifdef __linux__ 104 | 105 | typedef struct __attribute__((packed)) { 106 | uint8_t header; 107 | uint8_t information; 108 | uint16_t speed; 109 | uint16_t product_version; 110 | uint32_t sn_high; 111 | uint32_t sn_low; 112 | uint32_t hardware_version; 113 | uint32_t firmware_version; 114 | uint8_t crc8; 115 | } LiDARManufactureInfoType; 116 | 117 | typedef struct __attribute__((packed)) { 118 | uint16_t distance; 119 | uint8_t intensity; 120 | } LidarPointStructType; 121 | 122 | typedef struct __attribute__((packed)) { 123 | uint8_t header; 124 | uint8_t ver_len; 125 | uint16_t speed; 126 | uint16_t start_angle; 127 | LidarPointStructType point[POINT_PER_PACK]; 128 | uint16_t end_angle; 129 | uint16_t timestamp; 130 | uint8_t crc8; 131 | } LiDARMeasureDataType; 132 | 133 | typedef struct __attribute__((packed)) { 134 | uint8_t header; 135 | uint8_t information; 136 | uint8_t error_code; 137 | uint8_t crc8; 138 | } LiDARHealthInfoType; 139 | 140 | #else 141 | 142 | #pragma pack(1) 143 | typedef struct { 144 | uint8_t header; 145 | uint8_t information; 146 | uint16_t speed; 147 | uint16_t product_version; 148 | uint32_t sn_high; 149 | uint32_t sn_low; 150 | uint32_t hardware_version; 151 | uint32_t firmware_version; 152 | uint8_t crc8; 153 | } LiDARManufactureInfoType; 154 | 155 | typedef struct { 156 | uint16_t distance; 157 | uint8_t intensity; 158 | } LidarPointStructType; 159 | 160 | typedef struct { 161 | uint8_t header; 162 | uint8_t ver_len; 163 | uint16_t speed; 164 | uint16_t start_angle; 165 | LidarPointStructType point[POINT_PER_PACK]; 166 | uint16_t end_angle; 167 | uint16_t timestamp; 168 | uint8_t crc8; 169 | } LiDARMeasureDataType; 170 | 171 | typedef struct { 172 | uint8_t header; 173 | uint8_t information; 174 | uint8_t error_code; 175 | uint8_t crc8; 176 | } LiDARHealthInfoType; 177 | #pragma pack() 178 | 179 | #endif 180 | 181 | class LdLidarProtocol { 182 | public: 183 | LdLidarProtocol(); 184 | ~LdLidarProtocol(); 185 | 186 | /** 187 | * @brief analysis data packet. . 188 | * @param[in] 189 | * * @param byte : input serial byte data 190 | * @retval 191 | * If the return value is GET_PKG_PCD macro, the lidar point cloud data is obtained 192 | * If the return value is GET_PKG_HEALTH macro, the lidar health information is obtained. 193 | * If the return value is GET_PKG_MANUFACT macro, the lidar manufacture information is obtained. 194 | */ 195 | uint8_t AnalysisDataPacket(uint8_t byte); 196 | /** 197 | * @brief get point cloud data. 198 | */ 199 | LiDARMeasureDataType& GetPCDPacketData(void); 200 | /** 201 | * @brief get health information 202 | */ 203 | LiDARHealthInfoType& GetHealthPacketData(void); 204 | 205 | /** 206 | * @brief get manufacture information 207 | */ 208 | LiDARManufactureInfoType& GetManufactureInfoPacketData(void); 209 | 210 | private: 211 | LiDARMeasureDataType pcdpkg_data_; 212 | LiDARHealthInfoType healthpkg_data_; 213 | LiDARManufactureInfoType manufacinfpkg_data_; 214 | }; 215 | 216 | #ifdef __cplusplus 217 | extern "C" { 218 | #endif 219 | 220 | uint8_t CalCRC8(const uint8_t *data, uint16_t data_len); 221 | 222 | #ifdef __cplusplus 223 | } 224 | #endif 225 | 226 | 227 | } // namespace ldlidar 228 | 229 | #endif //__LDLIDAR_PROTOCOL_H__ 230 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ 231 | -------------------------------------------------------------------------------- /include/ldlidar_driver/log_module.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file log_module.h 3 | * @author David Hu (hmd_hubei_cn@163.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022.08.10 7 | * @note 8 | * @copyright Copyright (c) 2022 DAVID HU All rights reserved. 9 | * Licensed under the MIT License (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License in the file LICENSE 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | **/ 18 | 19 | #ifndef __LOGMODULE_H_ 20 | #define __LOGMODULE_H_ 21 | 22 | 23 | #define ENABLE_LOG_DIS_OUTPUT 24 | 25 | #define ENABLE_CONSOLE_LOG_DISPALY 26 | 27 | //#define ENABLE_LOG_WRITE_TO_FILE 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #ifndef __linux__ 35 | #include 36 | #else 37 | //#include 38 | #include 39 | #define printf_s(fileptr,str) (fprintf(fileptr,"%s",str)) 40 | #define __in 41 | #endif // ?????????????????????? 42 | 43 | 44 | struct LogVersion { 45 | int n_version; 46 | std::string str_descruble; 47 | }; 48 | 49 | 50 | class ILogRealization { 51 | public: 52 | virtual ~ILogRealization() { 53 | 54 | } 55 | virtual void Initializion(const char* path = NULL) = 0; 56 | virtual void LogPrintInf(const char* str) = 0; 57 | virtual void LogPrintData(const char* str) = 0; 58 | 59 | void free() { 60 | free(this); 61 | //this = NULL; 62 | }; 63 | private: 64 | virtual void free(ILogRealization *plogger) = 0 ; 65 | }; 66 | 67 | 68 | #define ILOGFREE(LogRealizationClass) virtual void free(ILogRealization* plogger) \ 69 | { \ 70 | LogRealizationClass* prealization = static_cast(plogger); \ 71 | if (prealization != NULL){ delete prealization;} \ 72 | } 73 | 74 | class LogPrint :public ILogRealization { 75 | public: 76 | virtual void Initializion(const char* path = NULL); 77 | virtual void free(ILogRealization *plogger); 78 | virtual void LogPrintInf(const char* str); 79 | virtual void LogPrintData(const char* str); 80 | 81 | inline std::string GetLogFilePathName(void) { 82 | std::string curr_date_log_file; 83 | char stdtime_str[50] = {0}; 84 | time_t std_time = 0; 85 | struct tm* local_time = NULL; 86 | std_time = time(NULL); 87 | local_time = localtime(&std_time); 88 | snprintf(stdtime_str, 50, "./lds-%d-%d-%d-%d.log", 89 | local_time->tm_year+1900, local_time->tm_mon+1, local_time->tm_mday, 90 | local_time->tm_hour); 91 | curr_date_log_file.assign(stdtime_str); 92 | return curr_date_log_file; 93 | } 94 | 95 | inline std::string GetOriginDataFilePathName(void) { 96 | std::string curr_date_log_file; 97 | char stdtime_str[50] = {0}; 98 | time_t std_time = 0; 99 | struct tm* local_time = NULL; 100 | std_time = time(NULL); 101 | local_time = localtime(&std_time); 102 | snprintf(stdtime_str, 50, "./serialdata-%d-%d-%d-%d.log", 103 | local_time->tm_year+1900, local_time->tm_mon+1, local_time->tm_mday, 104 | local_time->tm_hour); 105 | curr_date_log_file.assign(stdtime_str); 106 | return curr_date_log_file; 107 | } 108 | }; 109 | 110 | #ifndef __linux__ 111 | class LogOutputString :public ILogRealization { 112 | public: 113 | virtual void Initializion(const char* path = NULL) { 114 | return ; 115 | } 116 | 117 | virtual void LogPrintInf(const char* str) { 118 | //OutputDebugString((LPCTSTR)str); /* 将字符串发送到调试器进行显示。*/ 119 | //OutputDebugString("\r\n"); 120 | printf("%s\r\n", str); /* 将字符串发送到控制台显示 */ 121 | } 122 | 123 | virtual void LogPrintData(const char* str) { 124 | //OutputDebugString((LPCTSTR)str); 125 | //OutputDebugString("\r\n"); 126 | printf("%s\r\n", str); 127 | } 128 | 129 | ILOGFREE(LogOutputString) 130 | /* 131 | virtual void free(ILogRealization *plogger) 132 | { 133 | LogOutputString* poutput = static_cast(plogger); 134 | if (poutput != NULL) 135 | { 136 | delete poutput; 137 | } 138 | } 139 | */ 140 | }; 141 | #endif 142 | 143 | 144 | class LogModule { 145 | public: 146 | enum LogLevel { 147 | DEBUG_LEVEL, 148 | WARNING_LEVEL, 149 | ERROR_LEVEL, 150 | INFO_LEVEL 151 | }; 152 | 153 | struct LOGMODULE_INFO { 154 | LogLevel loglevel; 155 | std::string str_filename; 156 | std::string str_funcname; 157 | int n_linenumber; 158 | }logInfo_; 159 | 160 | ILogRealization* p_realization_; 161 | public: 162 | static LogModule* GetInstance( __in const char* filename, __in const char* funcname,__in int lineno, LogLevel level, ILogRealization* plog = NULL); 163 | static LogModule* GetInstance(LogLevel level, ILogRealization* plog = NULL); 164 | static LogModule* GetInstancePrintOriginData(LogLevel level, ILogRealization* plog = NULL); 165 | 166 | void LogPrintInf(const char* format,...); 167 | void LogPrintNoLocationInf(const char* format,...); 168 | void LogPrintOriginData(const char* format,...); 169 | 170 | private: 171 | LogModule(); 172 | 173 | ~LogModule(); 174 | 175 | void InitLock(); 176 | 177 | void RealseLock(); 178 | 179 | void Lock(); 180 | 181 | void UnLock(); 182 | 183 | std::string GetCurrentISOTime(); 184 | 185 | std::string GetCurrentLocalTimeStamp(); 186 | 187 | std::string GetFormatValue(std::string str_value); 188 | 189 | std::string GetFormatValue(int n_value); 190 | 191 | std::string GetLevelValue(int level); 192 | 193 | static LogModule* s_plog_module_; 194 | 195 | #ifndef __linux__ 196 | CRITICAL_SECTION mutex_lock_; 197 | #else 198 | pthread_mutex_t mutex_lock_; 199 | #endif 200 | }; 201 | 202 | #ifdef ENABLE_LOG_DIS_OUTPUT 203 | #define LOG(level,format,...) LogModule::GetInstance(__FILE__, __FUNCTION__, __LINE__,level)->LogPrintInf(format,__VA_ARGS__); 204 | #define LOG_DEBUG(format,...) LOG(LogModule::DEBUG_LEVEL,format,__VA_ARGS__) 205 | #define LOG_INFO(format,...) LOG(LogModule::INFO_LEVEL,format,__VA_ARGS__) 206 | #define LOG_WARN(format,...) LOG(LogModule::WARNING_LEVEL,format,__VA_ARGS__) 207 | #define LOG_ERROR(format,...) LOG(LogModule::ERROR_LEVEL,format,__VA_ARGS__) 208 | #else 209 | #define LOG_DEBUG(format,...) do {} while(0) 210 | #define LOG_INFO(format,...) do {} while(0) 211 | #define LOG_WARN(format,...) do {} while(0) 212 | #define LOG_ERROR(format,...) do {} while(0) 213 | #endif 214 | 215 | #ifdef ENABLE_LOG_DIS_OUTPUT 216 | #define LOG_LITE(level,format,...) LogModule::GetInstance(level)->LogPrintNoLocationInf(format,__VA_ARGS__); 217 | #define LOG_DEBUG_LITE(format,...) LOG_LITE(LogModule::DEBUG_LEVEL,format,__VA_ARGS__) 218 | #define LOG_INFO_LITE(format,...) LOG_LITE(LogModule::INFO_LEVEL,format,__VA_ARGS__) 219 | #define LOG_WARN_LITE(format,...) LOG_LITE(LogModule::WARNING_LEVEL,format,__VA_ARGS__) 220 | #define LOG_ERROR_LITE(format,...) LOG_LITE(LogModule::ERROR_LEVEL,format,__VA_ARGS__) 221 | #else 222 | #define LOG_DEBUG_LITE(format,...) do {} while(0) 223 | #define LOG_INFO_LITE(format,...) do {} while(0) 224 | #define LOG_WARN_LITE(format,...) do {} while(0) 225 | #define LOG_ERROR_LITE(format,...) do {} while(0) 226 | #endif 227 | 228 | #ifdef ENABLE_LOG_DIS_OUTPUT 229 | #define LOG_PRINT(level,format,...) LogModule::GetInstancePrintOriginData(level)->LogPrintOriginData(format,__VA_ARGS__); 230 | #define LOG_DEBUG_PRINT(format,...) LOG_PRINT(LogModule::DEBUG_LEVEL,format,__VA_ARGS__) 231 | #define LOG_INFO_PRINT(format,...) LOG_PRINT(LogModule::INFO_LEVEL,format,__VA_ARGS__) 232 | #else 233 | #define LOG_DEBUG_PRINT(format,...) do {} while(0) 234 | #define LOG_INFO_PRINT(format,...) do {} while(0) 235 | #endif 236 | 237 | #endif//__LOGGER_MODULE_H__ 238 | /********************* (C) COPYRIGHT DAVID HU *******END OF FILE ********/ 239 | -------------------------------------------------------------------------------- /include/ldlidar_driver/network_socket_interface_linux.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file network_socket_interface_linux.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux network App 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef __SOCKET_INTERFACE_LINUX_H__ 21 | #define __SOCKET_INTERFACE_LINUX_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace ldlidar { 47 | 48 | typedef enum NetworkCommDevEnum { 49 | NET_NULL, 50 | UDP_SERVER, 51 | UDP_CLIENT, 52 | TCP_SERVER, 53 | TCP_CLIENT 54 | }NetCommDevTypeDef; 55 | 56 | class UDPSocketInterfaceLinux { 57 | public: 58 | UDPSocketInterfaceLinux(); 59 | 60 | ~UDPSocketInterfaceLinux(); 61 | 62 | bool CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port); 63 | 64 | bool CloseSocket(); 65 | 66 | bool TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len); 67 | 68 | void SetRecvCallback(std::function callback); 69 | 70 | bool IsClientAck() { return is_server_recv_ack_flag_.load();} 71 | 72 | private: 73 | std::thread *recv_thread_; 74 | long long recv_count_; 75 | int32_t com_sockfd_; 76 | NetCommDevTypeDef ncd_; 77 | std::atomic is_cmd_created_, recv_thread_exit_flag_, is_server_recv_ack_flag_; 78 | std::function recv_callback_; 79 | std::string server_ip_, server_port_; 80 | std::string client_ip_, client_port_; 81 | 82 | bool IsCreated() { return is_cmd_created_.load(); } 83 | 84 | bool RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len); 85 | 86 | static void RecvThreadProc(void *param); 87 | }; 88 | 89 | 90 | class TCPSocketInterfaceLinux { 91 | public: 92 | TCPSocketInterfaceLinux(); 93 | 94 | ~TCPSocketInterfaceLinux(); 95 | 96 | bool CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port); 97 | 98 | bool CloseSocket(); 99 | 100 | bool TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len); 101 | 102 | void SetRecvCallback(std::function callback); 103 | 104 | 105 | 106 | private: 107 | std::thread *recv_thread_; 108 | long long recv_count_; 109 | int32_t com_sockfd_; 110 | int32_t listend_client_sockfd_; //// server model used 111 | NetCommDevTypeDef ncd_; 112 | std::atomic is_cmd_created_, recv_thread_exit_flag_; 113 | std::function recv_callback_; 114 | 115 | bool IsCreated() { return is_cmd_created_.load(); } 116 | 117 | bool RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len); 118 | 119 | static void RecvThreadProc(void *param); 120 | }; 121 | 122 | } // namespace ldlidar 123 | #endif // __SOCKET_INTERFACE_LINUX_H__ 124 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 125 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/serial_interface_linux.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmd_interface_linux.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef __LINUX_SERIAL_PORT_H__ 21 | #define __LINUX_SERIAL_PORT_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | namespace asmtermios { 31 | #include 32 | } 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace ldlidar { 46 | 47 | class SerialInterfaceLinux { 48 | public: 49 | SerialInterfaceLinux(); 50 | ~SerialInterfaceLinux(); 51 | // open serial port 52 | bool Open(std::string &port_name, uint32_t com_baudrate); 53 | // close serial port 54 | bool Close(); 55 | // receive from port channel data 56 | bool ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len); 57 | // transmit data to port channel 58 | bool WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len); 59 | // set receive port channel data callback deal with fuction 60 | void SetReadCallback(std::function callback) { 61 | read_callback_ = callback; 62 | } 63 | // whether open 64 | bool IsOpened() { return is_cmd_opened_.load(); }; 65 | 66 | private: 67 | std::thread *rx_thread_; 68 | long long rx_count_; 69 | int32_t com_handle_; 70 | uint32_t com_baudrate_; 71 | std::atomic is_cmd_opened_, rx_thread_exit_flag_; 72 | std::function read_callback_; 73 | static void RxThreadProc(void *param); 74 | }; 75 | 76 | } // namespace ldlidar 77 | 78 | #endif //__LINUX_SERIAL_PORT_H__ 79 | 80 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 81 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/serial_interface_win.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file serial_interface_win.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief Win32 serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #pragma once 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | struct PortInfo { 29 | std::string name; 30 | std::string description; 31 | std::string pid; 32 | std::string vid; 33 | }; 34 | 35 | enum class BaudRate { 36 | Baud1200 = 1200, 37 | Baud2400 = 2400, 38 | Baud4800 = 4800, 39 | Baud9600 = 9600, 40 | Baud19200 = 19200, 41 | Baud38400 = 38400, 42 | Baud57600 = 57600, 43 | Baud115200 = 115200, 44 | Baud230400 = 230400, 45 | Unknonw = -1 46 | }; 47 | enum class DataBits { 48 | Data5 = 5, 49 | Data6 = 6, 50 | Data7 = 7, 51 | Data8 = 8, 52 | Unknonw = -1 53 | }; 54 | enum class FlowControl { 55 | NoFlowControl, 56 | HardwareControl, 57 | SoftwareControl, 58 | Unknonw = -1 59 | }; 60 | enum class Parity { 61 | NoParity, 62 | EvenParity, 63 | OddParity, 64 | SpaceParity, 65 | MarkParity, 66 | Unknonw = -1 67 | }; 68 | enum class StopBits { OneStop = 0, TwoStop, OneAndHalfStop, Unknow }; 69 | 70 | struct PortParams { 71 | std::string portName; 72 | BaudRate baudrate = BaudRate::Baud115200; 73 | Parity parity = Parity::NoParity; 74 | DataBits dataBits = DataBits::Data8; 75 | StopBits stopBits = StopBits::OneStop; 76 | FlowControl flowControl = FlowControl::NoFlowControl; 77 | }; 78 | 79 | class SerialInterfaceWin { 80 | public: 81 | SerialInterfaceWin(); 82 | ~SerialInterfaceWin(); 83 | 84 | static bool availablePorts(std::vector &availabelPortInfo); 85 | bool open(std::string port_name); 86 | bool close(); 87 | bool write(const char *tx_buf, uint32_t tx_buf_len); 88 | int readAll(); 89 | long long rxLength() { return mRxCounter; } 90 | long long txLength() { return mTxCounter; } 91 | void clearRxLength(void) { mRxCounter = 0; } 92 | bool isOpen() { return mIsOpened; } 93 | void setBufferSize(size_t size) { mMaxBuffSize = size; } 94 | void setCommErrorCallback(std::function callback) { 95 | commErrorHandle = callback; 96 | } 97 | void setReadCallback( 98 | std::function callback) { 99 | readCallback = callback; 100 | } 101 | 102 | void setPortParams(PortParams params); 103 | PortParams currPortParams(void); 104 | 105 | private: 106 | bool mIsOpened; 107 | HANDLE mComHandle; 108 | std::thread *mRxThread; 109 | static void rxThreadProc(SerialInterfaceWin *pClass); 110 | size_t mByteToRead; 111 | long long mRxCounter; 112 | long long mTxCounter; 113 | size_t mMaxBuffSize; 114 | OVERLAPPED mOverlappedSend; 115 | OVERLAPPED mOverlappedRecv; 116 | std::atomic mRxThreadRunFlag; 117 | 118 | std::function commErrorHandle; 119 | std::function readCallback; 120 | 121 | PortParams portParams; 122 | }; 123 | 124 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/sl_transform.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file transform.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief Ranging center conversion with left and right hand system changes App 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __TRANSFORM_H 22 | #define __TRANSFORM_H 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include "ldlidar_driver/ldlidar_datatype.h" 30 | 31 | namespace ldlidar { 32 | 33 | class SlTransform { 34 | private: 35 | bool to_right_hand_ = true; 36 | double offset_x_; 37 | double offset_y_; 38 | LDType version_; 39 | 40 | public: 41 | SlTransform(LDType version, bool to_right_hand = false); 42 | Points2D Transform(const Points2D &data); 43 | ~SlTransform(); 44 | }; 45 | 46 | } //namespace ldlidar 47 | 48 | #endif // __TRANSFORM_H 49 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 50 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/slbf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file slbf.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __SLBF_H_ 22 | #define __SLBF_H_ 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include "ldlidar_driver/ldlidar_datatype.h" 29 | 30 | namespace ldlidar { 31 | 32 | class Slbf { 33 | private: 34 | const int kConfidenceHigh = 200; 35 | const int kConfidenceMiddle = 150; 36 | const int kConfidenceLow = 92; 37 | const int kScanFre = 2300; // Default scanning frequency, 38 | // which can be changed according to radar protocol 39 | double curr_speed_; 40 | bool enable_strict_policy_; // whether strict filtering is enabled within 300 41 | // mm, the effective value may be lost, and the 42 | // time sequence of recharging needs to be 43 | // disabled 44 | Slbf() = delete; 45 | Slbf(const Slbf &) = delete; 46 | Slbf &operator=(const Slbf &) = delete; 47 | 48 | public: 49 | Slbf(int speed, bool strict_policy = true); 50 | Points2D NearFilter(const Points2D &tmp) const; 51 | void EnableStrictPolicy(bool enable); 52 | ~Slbf(); 53 | }; 54 | 55 | } // namespace ldlidar 56 | 57 | #endif // __SLBF_H_ 58 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 59 | * FILE ********/ -------------------------------------------------------------------------------- /include/ldlidar_driver/tofbf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tofbf.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef __TOFBF_H_ 23 | #define __TOFBF_H_ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include "ldlidar_driver/ldlidar_datatype.h" 34 | 35 | namespace ldlidar { 36 | 37 | enum class FilterType{ 38 | NO_FILTER, 39 | NEAR_FILTER, 40 | NOISE_FILTER 41 | }; 42 | 43 | class Tofbf { 44 | public: 45 | Tofbf(int speed, LDType type); 46 | ~Tofbf(); 47 | std::vector Filter(const std::vector &tmp) const; 48 | 49 | private: 50 | FilterType filter_type_; 51 | // Low intensity threshold 52 | int intensity_low_; 53 | // Discrete points require higher intensity 54 | int intensity_single_; 55 | // Default scan frequency, to change, read according to radar protocol 56 | int scan_frequency_; 57 | double curr_speed_; 58 | Tofbf() = delete; 59 | Tofbf(const Tofbf &) = delete; 60 | Tofbf &operator=(const Tofbf &) = delete; 61 | std::vector NearFilter(const std::vector &tmp) const; 62 | std::vector NoiseFilter(const std::vector &tmp) const; 63 | }; 64 | 65 | } // namespace ldlidar 66 | 67 | #endif //__TOFBF_H_ 68 | 69 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 70 | * FILE ********/ -------------------------------------------------------------------------------- /sample/linux/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ldlidar) 3 | 4 | if(${CMAKE_BUILD_TYPE} MATCHES "Release") 5 | set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -Wall") 6 | message(STATUS "Mode: Release") 7 | message(STATUS "optional:-std=c++11 -Wall") 8 | elseif(${CMAKE_BUILD_TYPE} MATCHES "Debug") 9 | set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -Wall -Wextra -g2 -ggdb") 10 | message(STATUS "Mode: Debug") 11 | message(STATUS "optional:-std=c++11 -Wall -Wextra -g2 -ggdb") 12 | else() 13 | set(CMAKE_BUILD_TYPE "Debug") 14 | set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -Wall -Wextra -g2 -ggdb") 15 | message(STATUS "Mode: Debug") 16 | message(STATUS "optional:-std=c++11 -Wall -Wextra -g2 -ggdb") 17 | endif() 18 | 19 | ########### 20 | ## Build ## 21 | ########### 22 | 23 | ## Specify additional locations of header files 24 | ## Your package locations should be listed before other locations 25 | include_directories( 26 | ${CMAKE_CURRENT_SOURCE_DIR}/../../include/ 27 | ) 28 | 29 | set(LDLIDAR_DRIVER_SOURCE_LINUX 30 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_driver.cpp 31 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_driver_linux.cpp 32 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_dataprocess.cpp 33 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_protocol.cpp 34 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/log_module.cpp 35 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/network_socket_interface_linux.cpp 36 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/serial_interface_linux.cpp 37 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/sl_transform.cpp 38 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/slbf.cpp 39 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/tofbf.cpp 40 | ) 41 | 42 | add_executable(${PROJECT_NAME} 43 | demo.cpp 44 | ) 45 | 46 | add_library(ldlidar_driver_static STATIC 47 | ${LDLIDAR_DRIVER_SOURCE_LINUX} 48 | ) 49 | 50 | add_library(ldlidar_driver_shared SHARED 51 | ${LDLIDAR_DRIVER_SOURCE_LINUX} 52 | ) 53 | 54 | # rename library name 55 | set_target_properties (ldlidar_driver_static PROPERTIES OUTPUT_NAME "ldlidar_driver") 56 | set_target_properties (ldlidar_driver_shared PROPERTIES OUTPUT_NAME "ldlidar_driver") 57 | 58 | # binary file link to library 59 | target_link_libraries(${PROJECT_NAME} ldlidar_driver_static pthread) 60 | 61 | ########### 62 | ## Install ## 63 | ########### 64 | 65 | install(TARGETS ldlidar_driver_static ldlidar_driver_shared 66 | ARCHIVE DESTINATION lib/ldlidar_driver 67 | LIBRARY DESTINATION share/ldlidar_driver 68 | ) 69 | 70 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include 71 | DESTINATION include 72 | ) 73 | -------------------------------------------------------------------------------- /sample/linux/README.md: -------------------------------------------------------------------------------- 1 | # 1.system setup 2 | - Connect the LiDAR to your system motherboard via an onboard serial port or usB-to-serial module (for example, CP2102 module). 3 | 4 | - Set the -x permission for the serial port device mounted by the lidar in the system (for example, /dev/ttyUSB0),In actual use, the LiDAR can be set according to the actual mounted status of your system, you can use 'ls -l /dev' command to view. 5 | 6 | ``` bash 7 | $ cd ~/ldlidar_ws/ldlidar_sdk/sample/linux 8 | 9 | $ sudo chmod 777 /dev/ttyUSB0 10 | ``` 11 | 12 | # 2.build 13 | 14 | ``` bash 15 | $ cd ~/ldlidar_ws/ldlidar_sdk/sample/linux 16 | 17 | $ ./auto_build.sh 18 | ``` 19 | 20 | # 3.run 21 | ``` bash 22 | $ ./build/ldlidar 23 | # example: 24 | # LDLiDAR LD14 25 | $ ./build/ldlidar LD14 /dev/ttyUSB0 26 | # LDLiDAR LD14P 27 | $ ./build/ldlidar LD14P /dev/ttyUSB0 28 | # LDLiDAR LD06 29 | $ ./build/ldlidar LD06 /dev/ttyUSB0 30 | # LDLiDAR LD19 31 | $ ./build/ldlidar LD19 /dev/ttyUSB0 32 | ``` -------------------------------------------------------------------------------- /sample/linux/README_CN.md: -------------------------------------------------------------------------------- 1 | # 1.系统设置 2 | 3 | - 第一步，通过板载串口或者USB转串口模块(例如,cp2102模块)的方式,使激光雷达连接到你的系统主板. 4 | - 第二步，设置激光雷达在系统中挂载的串口设备-x权限,根据激光雷达在你的系统中的实际挂载情况来设置，可以使用`ls -l /dev`命令查看,(以/dev/ttyUSB0为例). 5 | 6 | ``` bash 7 | $ cd ~/ldlidar_ws/ldlidar_sdk/sample/linux 8 | 9 | $ sudo chmod 777 /dev/ttyUSB0 10 | ``` 11 | 12 | # 2.编译 13 | 14 | ```bash 15 | $ cd ~/ldlidar_ws/ldlidar_sdk/sample/linux 16 | 17 | $ ./auto_build.sh 18 | ``` 19 | 20 | # 3.运行 21 | ``` bash 22 | $ ./build/ldlidar 23 | # example: 24 | # LDLiDAR LD14 25 | $ ./build/ldlidar LD14 /dev/ttyUSB0 26 | # LDLiDAR LD14P 27 | $ ./build/ldlidar LD14P /dev/ttyUSB0 28 | # LDLiDAR LD06 29 | $ ./build/ldlidar LD06 /dev/ttyUSB0 30 | # LDLiDAR LD19 31 | $ ./build/ldlidar LD19 /dev/ttyUSB0 32 | ``` -------------------------------------------------------------------------------- /sample/linux/auto_build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #Author: David Hu 3 | #Date: 2022-09 4 | 5 | # Exit on error 6 | set -e 7 | 8 | BUILD_MODE_DEBUG=1 9 | BUILD_MODE_RELEASE=0 10 | OP_YES=1 11 | 12 | echo "Start cmake build" 13 | read -p "Please select build mode [Debug(1)/Release(0)]:" BUILD_MODE 14 | read -p "Is install lib and include files on the workspace, please input [yes(1)/no[0]]:" IS_INSTALL 15 | 16 | if [ ! -e "./build" ];then 17 | mkdir build 18 | echo "create ./build/" 19 | fi 20 | 21 | cd ./build 22 | 23 | if [ ${BUILD_MODE} == ${BUILD_MODE_DEBUG} ];then 24 | cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_INSTALL_PREFIX=../install .. 25 | elif [ ${BUILD_MODE} == ${BUILD_MODE_RELEASE} ];then 26 | cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=../install .. 27 | else 28 | echo "build mode input error" 29 | exit 0 30 | fi 31 | 32 | make 33 | 34 | if [ ${IS_INSTALL} == ${OP_YES} ] 35 | then 36 | if [ ! -e "../install" ] 37 | then 38 | mkdir ../install 39 | echo "create ../install/" 40 | fi 41 | make install 42 | echo "build and install finished." 43 | else 44 | echo "build finished." 45 | fi -------------------------------------------------------------------------------- /sample/linux/clean_build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #Author: David Hu 3 | #Date: 2022-09 4 | if [ -e "./build" ];then 5 | rm -rf build/ 6 | echo "del ./build/" 7 | fi 8 | 9 | if [ -e "./install" ];then 10 | rm -rf install/ 11 | echo "del ./install/" 12 | fi 13 | 14 | echo "del is ok....." -------------------------------------------------------------------------------- /sample/linux/demo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief main process App 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-08 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #include "ldlidar_driver/ldlidar_driver_linux.h" 23 | 24 | uint64_t GetTimestamp(void) { 25 | std::chrono::time_point tp = 26 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 27 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 28 | return ((uint64_t)tmp.count()); 29 | } 30 | 31 | // void LidarPowerOn(void) { 32 | // LOG_DEBUG("Lidar Power On",""); 33 | // // ... 34 | // } 35 | 36 | // void LidarPowerOff(void) { 37 | // LOG_DEBUG("Lidar Power Off",""); 38 | // // ... 39 | // } 40 | 41 | struct LdsInfoStruct { 42 | std::string ldtype_str; 43 | ldlidar::LDType ldtype_enum; 44 | uint32_t baudrate; 45 | }; 46 | 47 | LdsInfoStruct LdsInfoArrary[4] = { 48 | {"LD14", ldlidar::LDType::LD_14, 115200}, 49 | {"LD14P", ldlidar::LDType::LD_14P, 230400}, 50 | {"LD06", ldlidar::LDType::LD_06, 230400}, 51 | {"LD19", ldlidar::LDType::LD_19, 230400}, 52 | }; 53 | 54 | ldlidar::LDType GetLdsType(std::string in_str) { 55 | for (auto item : LdsInfoArrary) { 56 | if (!strcmp(in_str.c_str(), item.ldtype_str.c_str())) { 57 | return item.ldtype_enum; 58 | } 59 | } 60 | return ldlidar::LDType::NO_VER; 61 | } 62 | 63 | uint32_t GetLdsSerialPortBaudrateValue(std::string in_str) { 64 | for (auto item : LdsInfoArrary) { 65 | if (!strcmp(in_str.c_str(), item.ldtype_str.c_str())) { 66 | return item.baudrate; 67 | } 68 | } 69 | return 0; 70 | } 71 | 72 | int main(int argc, char **argv) { 73 | 74 | if (argc != 3) { 75 | LOG_INFO("cmd error",""); 76 | LOG_INFO("please input: ./ldlidar ",""); 77 | LOG_INFO("example:",""); 78 | LOG_INFO("./ldlidar LD14 /dev/ttyUSB0",""); 79 | LOG_INFO("./ldlidar LD14P /dev/ttyUSB0",""); 80 | LOG_INFO("./ldlidar LD06 /dev/ttyUSB0",""); 81 | LOG_INFO("./ldlidar LD19 /dev/ttyUSB0",""); 82 | exit(EXIT_FAILURE); 83 | } 84 | 85 | std::string ldlidar_type_str(argv[1]); 86 | std::string serial_port_name(argv[2]); 87 | 88 | // select ldrobot lidar sensor type. 89 | ldlidar::LDType ldlidar_type_dest; 90 | ldlidar_type_dest = GetLdsType(ldlidar_type_str); 91 | if (ldlidar_type_dest == ldlidar::LDType::NO_VER) { 92 | LOG_WARN("ldlidar_type_str value is not sure: %s", ldlidar_type_str.c_str()); 93 | exit(EXIT_FAILURE); 94 | } 95 | 96 | // if use serial communications interface, as select serial baudrate paramters. 97 | uint32_t serial_baudrate_val; 98 | serial_baudrate_val = GetLdsSerialPortBaudrateValue(ldlidar_type_str); 99 | if (!serial_baudrate_val) { 100 | LOG_WARN("ldlidar_type_str value is not sure: %s", ldlidar_type_str.c_str()); 101 | exit(EXIT_FAILURE); 102 | } 103 | 104 | ldlidar::LDLidarDriverLinuxInterface* lidar_drv = 105 | ldlidar::LDLidarDriverLinuxInterface::Create(); 106 | 107 | LOG_INFO("LDLiDAR SDK Pack Version is %s", lidar_drv->GetLidarSdkVersionNumber().c_str()); 108 | 109 | lidar_drv->RegisterGetTimestampFunctional(std::bind(&GetTimestamp)); 110 | 111 | lidar_drv->EnablePointCloudDataFilter(true); 112 | 113 | if (lidar_drv->Connect(ldlidar_type_dest, serial_port_name, serial_baudrate_val)) { 114 | LOG_INFO("ldlidar serial connect is success",""); 115 | // LidarPowerOn(); 116 | } else { 117 | LOG_ERROR("ldlidar serial connect is fail",""); 118 | exit(EXIT_FAILURE); 119 | } 120 | 121 | if (lidar_drv->WaitLidarComm(3500)) { 122 | LOG_INFO("ldlidar communication is normal.",""); 123 | } else { 124 | LOG_ERROR("ldlidar communication is abnormal.",""); 125 | lidar_drv->Disconnect(); 126 | } 127 | 128 | if (lidar_drv->Start()) { 129 | LOG_INFO_LITE("ldlidar driver start is success.",""); 130 | } else { 131 | LOG_ERROR_LITE("ldlidar driver start is fail.",""); 132 | } 133 | 134 | ldlidar::Points2D laser_scan_points; 135 | while (ldlidar::LDLidarDriverLinuxInterface::Ok()) { 136 | 137 | switch (lidar_drv->GetLaserScanData(laser_scan_points, 1500)){ 138 | case ldlidar::LidarStatus::NORMAL: { 139 | double lidar_scan_freq = 0; 140 | lidar_drv->GetLidarScanFreq(lidar_scan_freq); 141 | #ifdef __LP64__ 142 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%lu, stamp_end:%lu", 143 | lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp); 144 | #else 145 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%llu, stamp_end:%llu", 146 | lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp); 147 | #endif 148 | // output 2d point cloud data 149 | #if 0 150 | for (auto point : laser_scan_points) { 151 | #ifdef __LP64__ 152 | LOG_INFO_LITE("stamp(ns):%lu,angle:%f,distance(mm):%d,intensity:%d", 153 | point.stamp, point.angle, point.distance, point.intensity); 154 | #else 155 | LOG_INFO_LITE("stamp(ns):%llu,angle:%f,distance(mm):%d,intensity:%d", 156 | point.stamp, point.angle, point.distance, point.intensity); 157 | #endif 158 | } 159 | #endif 160 | break; 161 | } 162 | case ldlidar::LidarStatus::DATA_TIME_OUT: { 163 | LOG_ERROR_LITE("point cloud data publish time out, please check your lidar device.",""); 164 | lidar_drv->Stop(); 165 | break; 166 | } 167 | case ldlidar::LidarStatus::DATA_WAIT: { 168 | break; 169 | } 170 | default: 171 | break; 172 | } 173 | 174 | usleep(1000*166); // sleep 166ms , 6hz 175 | } 176 | 177 | lidar_drv->Stop(); 178 | lidar_drv->Disconnect(); 179 | // LidarPowerOff(); 180 | 181 | ldlidar::LDLidarDriverLinuxInterface::Destory(lidar_drv); 182 | 183 | return 0; 184 | } 185 | 186 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 187 | * FILE ********/ 188 | -------------------------------------------------------------------------------- /sample/windows/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project (ldlidar_driver_win LANGUAGES CXX) 4 | 5 | include_directories( 6 | ${CMAKE_CURRENT_SOURCE_DIR}/../../include 7 | ) 8 | 9 | add_executable(demo 10 | ${CMAKE_CURRENT_SOURCE_DIR}/windemo.cpp 11 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_driver.cpp 12 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_driver_win.cpp 13 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/serial_interface_win.cpp 14 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_dataprocess.cpp 15 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/ldlidar_protocol.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/log_module.cpp 17 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/sl_transform.cpp 18 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/slbf.cpp 19 | ${CMAKE_CURRENT_SOURCE_DIR}/../../src/tofbf.cpp 20 | ) 21 | target_compile_features(demo PUBLIC cxx_std_11) -------------------------------------------------------------------------------- /sample/windows/README.md: -------------------------------------------------------------------------------- 1 | # Use CMake to generate visual studio C++ projects 2 | > Prerequisites The visual studio IDE and CMake Tool must be installed on the PC. 3 | 4 | ## Method 1: Command line mode 5 | - Open the powershell terminal in the 'sample/windows' directory and run the following command to create a C++ project in the' Visual Studio 15 2017 Win64 'environment as an example. 6 | ``` powershell 7 | mkdir build 8 | 9 | cd build 10 | 11 | cmake -G "Visual Studio 15 2017 Win64" .. / 12 | ``` 13 | 14 | ## Method 2： CMake GUI 15 | 16 | - Create a `build` folder in the `sample/windows` director 17 | 18 | - ![](./pic/1.png) 19 | 20 | - Run CMake GUI, complete related configuration, generate VC++ project, compile and run 21 | - 22 | 23 | - 24 | 25 | - 26 | 27 | - 28 | 29 | - -------------------------------------------------------------------------------- /sample/windows/README_CN.md: -------------------------------------------------------------------------------- 1 | # 使用CMake生成visual studio C++ 工程 2 | > 前提条件，PC上需要提前安装有visual studio IDE 和 CMake Tool。 3 | 4 | ## 方法一：命令行方式 5 | - 在`sample/windows`目录下打开powershell终端，以创建`Visual Studio 15 2017 Win64`环境下的C++工程为例，运行如下命令。 6 | ```powershell 7 | mkdir build # create `build` folder. 8 | 9 | cd build 10 | 11 | cmake -G "Visual Studio 15 2017 Win64" ../ 12 | ``` 13 | 14 | ## 方法二： CMake GUI 15 | 16 | - 在`sample/windows` 目录下创建`build` 文件夹 17 | 18 | - ![](./pic/1.png) 19 | 20 | - 运行CMake GUI,完成相关配置，生成VC++工程并编译运行 21 | - 22 | 23 | - 24 | 25 | - 26 | 27 | - 28 | 29 | - -------------------------------------------------------------------------------- /sample/windows/pic/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/1.png -------------------------------------------------------------------------------- /sample/windows/pic/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/2.png -------------------------------------------------------------------------------- /sample/windows/pic/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/3.png -------------------------------------------------------------------------------- /sample/windows/pic/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/4.png -------------------------------------------------------------------------------- /sample/windows/pic/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/5.png -------------------------------------------------------------------------------- /sample/windows/pic/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ldlidar_sdk/6148cd3c33ae39adbd7e12faf82567fa8f8e7b0d/sample/windows/pic/6.png -------------------------------------------------------------------------------- /sample/windows/windemo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief main process App 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 1.0 8 | * @date 2023-2-20 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver/ldlidar_driver_win.h" 22 | 23 | uint64_t GetTimestamp(void) { 24 | std::chrono::time_point 25 | tp = std::chrono::time_point_cast( 26 | std::chrono::system_clock::now()); 27 | auto tmp = std::chrono::duration_cast( 28 | tp.time_since_epoch()); 29 | return ((uint64_t)tmp.count()); 30 | } 31 | 32 | int main(int argc, char** argv) { 33 | struct lidar_inf_struct { 34 | uint8_t index; 35 | std::string type_name; 36 | ldlidar::LDType type_number; 37 | uint32_t baudrate; 38 | }; 39 | 40 | const int inf_array_size = 4; 41 | lidar_inf_struct inf_array[inf_array_size] = { 42 | {0, "LDLiDAR LD06", ldlidar::LDType::LD_06, 230400}, 43 | {1, "LDLiDAR LD14", ldlidar::LDType::LD_14, 115200}, 44 | {2, "LDLiDAR LD14P", ldlidar::LDType::LD_14P, 230400}, 45 | {3, "LDLiDAR LD19", ldlidar::LDType::LD_19, 230400}}; 46 | 47 | printf("The SDK support ldlidar product:\n"); 48 | for (auto inf_base : inf_array) { 49 | printf("index:[%d] --- %s\n", inf_base.index, inf_base.type_name.c_str()); 50 | } 51 | uint8_t product_index; 52 | printf("\nplease select product and input index value:\n"); 53 | scanf_s("%d", &product_index); 54 | if ((product_index < 0) || (product_index >= inf_array_size)) { 55 | return -1; 56 | } 57 | 58 | PortParams port_param; 59 | port_param.baudrate = (BaudRate)inf_array[product_index].baudrate; 60 | ldlidar::LDType lidar_type_number = inf_array[product_index].type_number; 61 | 62 | std::vector info; 63 | SerialInterfaceWin::availablePorts(info); // get serial port device info 64 | printf("\nAvailable port:\n\n"); 65 | int port_index = 0; 66 | for (auto port_item : info) { 67 | printf("index:[%d] %s, %s,\n", 68 | port_index++, port_item.name.c_str(), 69 | port_item.description.c_str()); 70 | } 71 | 72 | printf("\nplease select port and input index value:\n"); 73 | scanf_s("%d", &port_index); 74 | if (port_index > info.size()) { 75 | return -1; 76 | } 77 | 78 | ldlidar::LDLidarDriverWinInterface* ldlidar_drv = 79 | ldlidar::LDLidarDriverWinInterface::Create(); 80 | 81 | LOG_INFO_LITE("LDLiDAR SDK Pack Version is %s", 82 | ldlidar_drv->GetLidarSdkVersionNumber().c_str()); 83 | 84 | ldlidar_drv->RegisterGetTimestampFunctional(std::bind(&GetTimestamp)); 85 | 86 | ldlidar_drv->EnablePointCloudDataFilter(true); 87 | 88 | if (ldlidar_drv->Connect(lidar_type_number, info[port_index].name, port_param)) { 89 | LOG_INFO_LITE("ldlidar serial connect is success", ""); 90 | } else { 91 | LOG_ERROR_LITE("ldlidar serial connect is fail", ""); 92 | exit(EXIT_FAILURE); 93 | } 94 | 95 | if (ldlidar_drv->WaitLidarComm(3500)) { 96 | LOG_INFO_LITE("ldlidar communication is normal.", ""); 97 | } else { 98 | LOG_ERROR_LITE("ldlidar communication is abnormal.", ""); 99 | ldlidar_drv->Disconnect(); 100 | } 101 | 102 | if (ldlidar_drv->Start()) { 103 | LOG_INFO_LITE("ldlidar driver start is success.",""); 104 | } else { 105 | LOG_ERROR_LITE("ldlidar driver start is fail.",""); 106 | } 107 | 108 | ldlidar::Points2D laser_scan_points; 109 | while (ldlidar::LDLidarDriverWinInterface::Ok()) { 110 | switch (ldlidar_drv->GetLaserScanData(laser_scan_points, 1500)) { 111 | case ldlidar::LidarStatus::NORMAL: { 112 | double lidar_scan_freq = 0; 113 | ldlidar_drv->GetLidarScanFreq(lidar_scan_freq); 114 | #ifdef _WIN64 115 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%lu, stamp_end:%lu", 116 | lidar_scan_freq, laser_scan_points.size(), 117 | laser_scan_points.front().stamp, 118 | laser_scan_points.back().stamp); 119 | #else 120 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%llu, stamp_end:%llu", 121 | lidar_scan_freq, laser_scan_points.size(), 122 | laser_scan_points.front().stamp, 123 | laser_scan_points.back().stamp); 124 | #endif 125 | // output 2d point cloud data 126 | #if 0 127 | for (auto point : laser_scan_points) { 128 | #ifdef _WIN64 129 | LOG_INFO_LITE("stamp(ns):%lu,angle:%f,distance(mm):%d,intensity:%d", 130 | point.stamp, point.angle, point.distance, 131 | point.intensity); 132 | #else 133 | LOG_INFO_LITE("stamp(ns):%llu,angle:%f,distance(mm):%d,intensity:%d", 134 | point.stamp, point.angle, point.distance, 135 | point.intensity); 136 | #endif 137 | } 138 | #endif 139 | break; 140 | } 141 | case ldlidar::LidarStatus::DATA_TIME_OUT: { 142 | LOG_ERROR_LITE("point cloud data publish time out, please check your lidar device.",""); 143 | ldlidar_drv->Stop(); 144 | break; 145 | } 146 | case ldlidar::LidarStatus::DATA_WAIT: { 147 | break; 148 | } 149 | default: 150 | break; 151 | } 152 | 153 | Sleep(166); // sleep 166ms , 6hz 154 | } 155 | 156 | ldlidar_drv->Stop(); 157 | ldlidar_drv->Disconnect(); 158 | 159 | ldlidar::LDLidarDriverWinInterface::Destory(ldlidar_drv); 160 | 161 | system("pause"); 162 | return 0; 163 | } -------------------------------------------------------------------------------- /src/ldlidar_dataprocess.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * David Hu (hmd_hubei_cn@163.com) 5 | * @brief LiDAR data protocol processing App 6 | * @version 1.0 7 | * @date 2023-03-12 8 | * 9 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 10 | * reserved. 11 | * Licensed under the MIT License (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License in the file LICENSE 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | #include "ldlidar_driver/ldlidar_dataprocess.h" 21 | 22 | namespace ldlidar { 23 | 24 | LdLidarDataProcess::LdLidarDataProcess() 25 | : lidar_measure_freq_(2300), 26 | typenumber_(LDType::NO_VER), 27 | lidarstatus_(LidarStatus::NORMAL), 28 | lidarerrorcode_(LIDAR_NO_ERROR), 29 | is_frame_ready_(false), 30 | is_noise_filter_(false), 31 | timestamp_(0), 32 | speed_(0), 33 | get_timestamp_(nullptr), 34 | is_poweron_comm_normal_(false), 35 | last_pkg_timestamp_(0), 36 | protocol_handle_(new LdLidarProtocol()) { 37 | 38 | } 39 | 40 | LdLidarDataProcess::~LdLidarDataProcess() { 41 | if (protocol_handle_ != nullptr) { 42 | delete protocol_handle_; 43 | } 44 | } 45 | 46 | void LdLidarDataProcess::SetProductType(LDType typenumber) { 47 | typenumber_ = typenumber; 48 | switch (typenumber) { 49 | case LDType::LD_14: 50 | lidar_measure_freq_ = 2300; 51 | break; 52 | case LDType::LD_14P: 53 | lidar_measure_freq_ = 4000; 54 | break; 55 | case LDType::LD_06: 56 | case LDType::LD_19: 57 | lidar_measure_freq_ = 4500; 58 | break; 59 | case LDType::STL_06P: 60 | case LDType::STL_26: 61 | lidar_measure_freq_ = 5000; 62 | break; 63 | case LDType::STL_27L: 64 | lidar_measure_freq_ = 21600; 65 | break; 66 | default : 67 | lidar_measure_freq_ = 2300; 68 | break; 69 | } 70 | } 71 | 72 | void LdLidarDataProcess::SetNoiseFilter(bool is_enable) { 73 | is_noise_filter_ = is_enable; 74 | } 75 | 76 | void LdLidarDataProcess::RegisterTimestampGetFunctional(std::function timestamp_handle) { 77 | get_timestamp_ = timestamp_handle; 78 | } 79 | 80 | bool LdLidarDataProcess::Parse(const uint8_t *data, long len) { 81 | for (int i = 0; i < len; i++) { 82 | 83 | uint8_t ret = protocol_handle_->AnalysisDataPacket(data[i]); 84 | if (ret == GET_PKG_PCD) { 85 | LiDARMeasureDataType datapkg = protocol_handle_->GetPCDPacketData(); 86 | is_poweron_comm_normal_ = true; 87 | speed_ = datapkg.speed; 88 | timestamp_ = datapkg.timestamp; 89 | // parse a package is success 90 | double diff = (datapkg.end_angle / 100 - datapkg.start_angle / 100 + 360) % 360; 91 | if (diff <= ((double)datapkg.speed * POINT_PER_PACK / lidar_measure_freq_ * 1.5)) { 92 | if (0 == last_pkg_timestamp_) { 93 | last_pkg_timestamp_ = get_timestamp_(); 94 | } else { 95 | uint64_t current_pack_stamp = get_timestamp_(); 96 | int pkg_point_number = POINT_PER_PACK; 97 | double pack_stamp_point_step = 98 | static_cast(current_pack_stamp - last_pkg_timestamp_) / static_cast(pkg_point_number - 1); 99 | uint32_t diff =((uint32_t)datapkg.end_angle + 36000 - (uint32_t)datapkg.start_angle) % 36000; 100 | float step = diff / (POINT_PER_PACK - 1) / 100.0; 101 | float start = (double)datapkg.start_angle / 100.0; 102 | PointData data; 103 | for (int i = 0; i < POINT_PER_PACK; i++) { 104 | data.distance = datapkg.point[i].distance; 105 | data.angle = start + i * step; 106 | if (data.angle >= 360.0) { 107 | data.angle -= 360.0; 108 | } 109 | data.intensity = datapkg.point[i].intensity; 110 | data.stamp = static_cast(last_pkg_timestamp_ + (pack_stamp_point_step * i)); 111 | tmp_lidar_scan_data_vec_.push_back(PointData(data.angle, data.distance, data.intensity, data.stamp)); 112 | } 113 | last_pkg_timestamp_ = current_pack_stamp; //// update last pkg timestamp 114 | } 115 | } 116 | } 117 | } 118 | 119 | return true; 120 | } 121 | 122 | bool LdLidarDataProcess::AssemblePacket() { 123 | float last_angle = 0; 124 | Points2D tmp, data; 125 | int count = 0; 126 | 127 | if (speed_ <= 0) { 128 | tmp_lidar_scan_data_vec_.erase(tmp_lidar_scan_data_vec_.begin(), tmp_lidar_scan_data_vec_.end()); 129 | return false; 130 | } 131 | 132 | for (auto n : tmp_lidar_scan_data_vec_) { 133 | // wait for enough data, need enough data to show a circle 134 | // enough data has been obtained 135 | if ((n.angle < 20.0) && (last_angle > 340.0)) { 136 | if ((count * GetSpeed()) > (lidar_measure_freq_ * 1.4)) { 137 | if (count >= (int)tmp_lidar_scan_data_vec_.size()) { 138 | tmp_lidar_scan_data_vec_.clear(); 139 | } else { 140 | tmp_lidar_scan_data_vec_.erase(tmp_lidar_scan_data_vec_.begin(), tmp_lidar_scan_data_vec_.begin() + count); 141 | } 142 | return false; 143 | } 144 | data.insert(data.begin(), tmp_lidar_scan_data_vec_.begin(), tmp_lidar_scan_data_vec_.begin() + count); 145 | 146 | switch (typenumber_) { 147 | case LDType::LD_14: 148 | case LDType::LD_14P: { 149 | SlTransform trans(typenumber_); 150 | data = trans.Transform(data); // transform raw data to stantard data 151 | if (is_noise_filter_ && (typenumber_ != LDType::LD_14P)) { 152 | std::sort(data.begin(), data.end(), 153 | [](PointData a, PointData b) { return a.angle < b.angle;}); 154 | Slbf sb(speed_); 155 | tmp = sb.NearFilter(data); // filter noise point 156 | } else { 157 | tmp = data; 158 | } 159 | break; 160 | } 161 | case LDType::LD_06: 162 | case LDType::LD_19: 163 | case LDType::STL_06P: 164 | case LDType::STL_26: 165 | case LDType::STL_27L: { 166 | if (is_noise_filter_) { 167 | Tofbf tofbfLd(speed_, typenumber_); 168 | tmp = tofbfLd.Filter(data); // filter noise point 169 | } else { 170 | tmp = data; 171 | } 172 | break; 173 | } 174 | default : { 175 | tmp = data; 176 | break; 177 | } 178 | } 179 | 180 | std::sort(tmp.begin(), tmp.end(), 181 | [](PointData a, PointData b) { return a.stamp < b.stamp; }); 182 | if (tmp.size() > 0) { 183 | SetLaserScanData(tmp); 184 | SetFrameReady(); 185 | 186 | if (count >= (int)tmp_lidar_scan_data_vec_.size()) { 187 | tmp_lidar_scan_data_vec_.clear(); 188 | } else { 189 | tmp_lidar_scan_data_vec_.erase(tmp_lidar_scan_data_vec_.begin(), tmp_lidar_scan_data_vec_.begin() + count); 190 | } 191 | return true; 192 | } 193 | } 194 | count++; 195 | 196 | if ((count * GetSpeed()) > (lidar_measure_freq_ * 2)) { 197 | if (count >= (int)tmp_lidar_scan_data_vec_.size()) { 198 | tmp_lidar_scan_data_vec_.clear(); 199 | } else { 200 | tmp_lidar_scan_data_vec_.erase(tmp_lidar_scan_data_vec_.begin(), tmp_lidar_scan_data_vec_.begin() + count); 201 | } 202 | return false; 203 | } 204 | 205 | last_angle = n.angle; 206 | } 207 | 208 | return false; 209 | } 210 | 211 | void LdLidarDataProcess::CommReadCallback(const char *byte, size_t len) { 212 | if (Parse((uint8_t *)byte, len)) { 213 | AssemblePacket(); 214 | } 215 | } 216 | 217 | bool LdLidarDataProcess::GetLaserScanData(Points2D& out) { 218 | if (IsFrameReady()) { 219 | ResetFrameReady(); 220 | out = GetLaserScanData(); 221 | return true; 222 | } else { 223 | return false; 224 | } 225 | } 226 | 227 | double LdLidarDataProcess::GetSpeed(void) { 228 | return (speed_ / 360.0); // unit is Hz 229 | } 230 | 231 | LidarStatus LdLidarDataProcess::GetLidarStatus(void) { 232 | return lidarstatus_; 233 | } 234 | 235 | uint8_t LdLidarDataProcess::GetLidarErrorCode(void) { 236 | return lidarerrorcode_; 237 | } 238 | 239 | bool LdLidarDataProcess::GetLidarPowerOnCommStatus(void) { 240 | if (is_poweron_comm_normal_) { 241 | is_poweron_comm_normal_ = false; 242 | return true; 243 | } else { 244 | return false; 245 | } 246 | } 247 | 248 | void LdLidarDataProcess::SetLidarStatus(LidarStatus status) { 249 | lidarstatus_ = status; 250 | } 251 | 252 | void LdLidarDataProcess::SetLidarErrorCode(uint8_t errorcode) { 253 | lidarerrorcode_ = errorcode; 254 | } 255 | 256 | bool LdLidarDataProcess::IsFrameReady(void) { 257 | std::lock_guard lg(mutex_lock1_); 258 | return is_frame_ready_; 259 | } 260 | 261 | void LdLidarDataProcess::ResetFrameReady(void) { 262 | std::lock_guard lg(mutex_lock1_); 263 | is_frame_ready_ = false; 264 | } 265 | 266 | void LdLidarDataProcess::SetFrameReady(void) { 267 | std::lock_guard lg(mutex_lock1_); 268 | is_frame_ready_ = true; 269 | } 270 | 271 | void LdLidarDataProcess::SetLaserScanData(Points2D& src) { 272 | std::lock_guard lg(mutex_lock2_); 273 | lidar_scan_data_vec_ = src; 274 | } 275 | 276 | Points2D LdLidarDataProcess::GetLaserScanData(void) { 277 | std::lock_guard lg(mutex_lock2_); 278 | return lidar_scan_data_vec_; 279 | } 280 | 281 | } // namespace ldlidar 282 | 283 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 284 | * FILE ********/ -------------------------------------------------------------------------------- /src/ldlidar_driver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-05-12 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver/ldlidar_driver.h" 22 | 23 | namespace ldlidar { 24 | 25 | bool LDLidarDriver::is_ok_ = false; 26 | 27 | LDLidarDriver::LDLidarDriver() : 28 | is_start_flag_(false), 29 | is_connect_flag_(false), 30 | sdk_pack_version_(LDLiDAR_SDK_VERSION_NUMBER) { 31 | 32 | } 33 | 34 | LDLidarDriver::~LDLidarDriver() { 35 | 36 | } 37 | 38 | std::string LDLidarDriver::GetLidarSdkVersionNumber(void) { 39 | return sdk_pack_version_; 40 | } 41 | 42 | bool LDLidarDriver::Ok() { 43 | return is_ok_; 44 | } 45 | 46 | void LDLidarDriver::SetLidarDriverStatus(bool status) { 47 | is_ok_ = status; 48 | } 49 | 50 | } // namespace ldlidar 51 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 52 | * FILE ********/ -------------------------------------------------------------------------------- /src/ldlidar_driver_linux.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2023-03 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver/ldlidar_driver_linux.h" 22 | 23 | namespace ldlidar { 24 | 25 | LDLidarDriverLinuxInterface::LDLidarDriverLinuxInterface() : comm_pkg_(new LdLidarDataProcess()), 26 | comm_serial_(new SerialInterfaceLinux()), 27 | comm_tcp_network_(new TCPSocketInterfaceLinux()), 28 | comm_udp_network_(new UDPSocketInterfaceLinux()){ 29 | 30 | last_pubdata_times_ = std::chrono::steady_clock::now(); 31 | } 32 | 33 | LDLidarDriverLinuxInterface::~LDLidarDriverLinuxInterface() { 34 | if (comm_pkg_ != nullptr) { 35 | delete comm_pkg_; 36 | } 37 | 38 | if (comm_serial_ != nullptr) { 39 | delete comm_serial_; 40 | } 41 | 42 | if (comm_tcp_network_ != nullptr) { 43 | delete comm_tcp_network_; 44 | } 45 | 46 | if (comm_udp_network_ != nullptr) { 47 | delete comm_udp_network_; 48 | } 49 | } 50 | 51 | bool LDLidarDriverLinuxInterface::Connect(LDType product_name, 52 | std::string serial_port_name, 53 | uint32_t serial_baudrate, 54 | CommunicationModeType comm_mode) { 55 | 56 | if (is_connect_flag_) { 57 | return true; 58 | } 59 | 60 | if (serial_port_name.empty()) { 61 | LOG_ERROR("input is empty.",""); 62 | return false; 63 | } 64 | 65 | if (register_get_timestamp_handle_ == nullptr) { 66 | LOG_ERROR("get timestamp fuctional is not register.",""); 67 | return false; 68 | } 69 | 70 | comm_pkg_->ClearDataProcessStatus(); 71 | comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); 72 | comm_pkg_->SetProductType(product_name); 73 | 74 | if (COMM_SERIAL_MODE == comm_mode) { 75 | comm_serial_->SetReadCallback(std::bind( 76 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 77 | if (!comm_serial_->Open(serial_port_name, serial_baudrate)) { 78 | LOG_ERROR("serial is not open:%s", serial_port_name.c_str()); 79 | return false; 80 | } 81 | } else { 82 | LOG_ERROR("input value is not \"ldlidar::COMM_SERIAL_MODE\"",""); 83 | return false; 84 | } 85 | 86 | is_connect_flag_ = true; 87 | 88 | SetLidarDriverStatus(true); 89 | 90 | return true; 91 | } 92 | 93 | bool LDLidarDriverLinuxInterface::Connect(LDType product_name, 94 | const char* server_ip, 95 | const char* server_port, 96 | CommunicationModeType comm_mode) { 97 | 98 | if (is_connect_flag_) { 99 | return true; 100 | } 101 | 102 | if (LDType::NO_VER == product_name) { 103 | LOG_ERROR("input is abnormal.",""); 104 | return false; 105 | } 106 | 107 | if ((server_ip == nullptr) || (server_port == nullptr)) { 108 | LOG_ERROR("input server_ip or server_port is null",""); 109 | return false; 110 | } 111 | 112 | if ((COMM_NO_NULL == comm_mode) || (COMM_SERIAL_MODE == comm_mode)) { 113 | LOG_ERROR("input comm_mode param is error.",""); 114 | return false; 115 | } 116 | 117 | if (register_get_timestamp_handle_ == nullptr) { 118 | LOG_ERROR("get timestamp fuctional is not register.",""); 119 | return false; 120 | } 121 | 122 | comm_pkg_->ClearDataProcessStatus(); 123 | comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); 124 | comm_pkg_->SetProductType(product_name); 125 | 126 | switch (comm_mode) { 127 | case COMM_TCP_CLIENT_MODE: { 128 | comm_tcp_network_->SetRecvCallback(std::bind( 129 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 130 | bool result = comm_tcp_network_->CreateSocket(TCP_CLIENT, server_ip, server_port); 131 | if (!result) { 132 | LOG_ERROR("client host: create socket is fail.",""); 133 | return false; 134 | } 135 | LOG_INFO("client host: create socket is ok.",""); 136 | } 137 | break; 138 | case COMM_TCP_SERVER_MODE: { 139 | comm_tcp_network_->SetRecvCallback(std::bind( 140 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 141 | bool result = comm_tcp_network_->CreateSocket(TCP_SERVER, server_ip, server_port); 142 | if (!result) { 143 | LOG_ERROR("server host: create socket is fail.",""); 144 | return false; 145 | } 146 | LOG_INFO("server host: create socket is ok.",""); 147 | } 148 | break; 149 | case COMM_UDP_CLIENT_MODE: { 150 | comm_udp_network_->SetRecvCallback(std::bind( 151 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 152 | bool result = comm_udp_network_->CreateSocket(UDP_CLIENT, server_ip, server_port); 153 | if (!result) { 154 | LOG_ERROR("client host: create socket is fail.",""); 155 | return false; 156 | } 157 | // 主动向服务端发布消息使服务端保存客户端ip，port 信息，建立沟通渠道 158 | uint8_t trans_buf[4] = {0xa5, 0x5a, 0x00, 0x00}; 159 | uint32_t tx_len; 160 | if (!comm_udp_network_->TransToNet((uint8_t *)trans_buf, sizeof(trans_buf), &tx_len)) { 161 | LOG_ERROR("client host: send request to server is fail. %s", strerror(errno)); 162 | return false; 163 | } 164 | LOG_INFO("client host: create socket is ok.",""); 165 | } 166 | break; 167 | case COMM_UDP_SERVER_MODE: { 168 | comm_udp_network_->SetRecvCallback(std::bind( 169 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 170 | bool result = comm_udp_network_->CreateSocket(UDP_SERVER, server_ip, server_port); 171 | if (!result) { 172 | LOG_ERROR("server host: create socket is fail.",""); 173 | return false; 174 | } 175 | LOG_INFO("server host: create socket is ok.",""); 176 | LOG_INFO("server host: wait client ack connect..",""); 177 | while (!comm_udp_network_->IsClientAck()) { 178 | usleep(1000); 179 | } 180 | } 181 | break; 182 | default: { 183 | LOG_ERROR("input comm_mode param is error.",""); 184 | return false; 185 | } 186 | break; 187 | } 188 | 189 | is_connect_flag_ = true; 190 | 191 | SetLidarDriverStatus(true); 192 | 193 | return true; 194 | } 195 | 196 | bool LDLidarDriverLinuxInterface::Disconnect(void) { 197 | if (!is_connect_flag_) { 198 | return true; 199 | } 200 | 201 | SetLidarDriverStatus(false); 202 | 203 | comm_serial_->Close(); 204 | comm_tcp_network_->CloseSocket(); 205 | comm_udp_network_->CloseSocket(); 206 | 207 | is_connect_flag_ = false; 208 | 209 | return true; 210 | } 211 | 212 | void LDLidarDriverLinuxInterface::EnablePointCloudDataFilter(bool is_enable) { 213 | comm_pkg_->SetNoiseFilter(is_enable); 214 | } 215 | 216 | bool LDLidarDriverLinuxInterface::WaitLidarComm(int64_t timeout) { 217 | auto last_time = std::chrono::steady_clock::now(); 218 | 219 | bool is_recvflag = false; 220 | do { 221 | if (comm_pkg_->GetLidarPowerOnCommStatus()) { 222 | is_recvflag = true; 223 | } 224 | usleep(1000); 225 | } while (!is_recvflag && (std::chrono::duration_cast( 226 | std::chrono::steady_clock::now() - last_time).count() < timeout)); 227 | 228 | if (is_recvflag) { 229 | SetLidarDriverStatus(true); 230 | return true; 231 | } else { 232 | SetLidarDriverStatus(false); 233 | return false; 234 | } 235 | } 236 | 237 | LidarStatus LDLidarDriverLinuxInterface::GetLaserScanData(Points2D& dst, int64_t timeout) { 238 | if (!is_start_flag_) { 239 | return LidarStatus::STOP; 240 | } 241 | 242 | LidarStatus status = comm_pkg_->GetLidarStatus(); 243 | if (LidarStatus::NORMAL == status) { 244 | if (comm_pkg_->GetLaserScanData(dst)) { 245 | last_pubdata_times_ = std::chrono::steady_clock::now(); 246 | return LidarStatus::NORMAL; 247 | } 248 | 249 | if (std::chrono::duration_cast( 250 | std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 251 | return LidarStatus::DATA_TIME_OUT; 252 | } else { 253 | return LidarStatus::DATA_WAIT; 254 | } 255 | } else { 256 | last_pubdata_times_ = std::chrono::steady_clock::now(); 257 | return status; 258 | } 259 | } 260 | 261 | LidarStatus LDLidarDriverLinuxInterface::GetLaserScanData(LaserScan& dst, int64_t timeout) { 262 | if (!is_start_flag_) { 263 | return LidarStatus::STOP; 264 | } 265 | 266 | LidarStatus status = comm_pkg_->GetLidarStatus(); 267 | if (LidarStatus::NORMAL == status) { 268 | Points2D recvpcd; 269 | if (comm_pkg_->GetLaserScanData(recvpcd)) { 270 | last_pubdata_times_ = std::chrono::steady_clock::now(); 271 | dst.stamp = recvpcd.front().stamp; 272 | dst.points = recvpcd; 273 | return LidarStatus::NORMAL; 274 | } 275 | 276 | if (std::chrono::duration_cast( 277 | std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 278 | return LidarStatus::DATA_TIME_OUT; 279 | } else { 280 | return LidarStatus::DATA_WAIT; 281 | } 282 | } else { 283 | last_pubdata_times_ = std::chrono::steady_clock::now(); 284 | return status; 285 | } 286 | } 287 | 288 | bool LDLidarDriverLinuxInterface::GetLidarScanFreq(double& spin_hz) { 289 | if (!is_start_flag_) { 290 | return false; 291 | } 292 | spin_hz = comm_pkg_->GetSpeed(); 293 | return true; 294 | } 295 | 296 | void LDLidarDriverLinuxInterface::RegisterGetTimestampFunctional(std::function get_timestamp_handle) { 297 | register_get_timestamp_handle_ = get_timestamp_handle; 298 | } 299 | 300 | uint8_t LDLidarDriverLinuxInterface::GetLidarErrorCode(void) { 301 | if (!is_start_flag_) { 302 | return LIDAR_NO_ERROR; 303 | } 304 | 305 | uint8_t errcode = comm_pkg_->GetLidarErrorCode(); 306 | return errcode; 307 | } 308 | 309 | bool LDLidarDriverLinuxInterface::Start(void) { 310 | if (is_start_flag_) { 311 | return true; 312 | } 313 | 314 | if (!is_connect_flag_) { 315 | return false; 316 | } 317 | 318 | is_start_flag_ = true; 319 | 320 | last_pubdata_times_ = std::chrono::steady_clock::now(); 321 | 322 | SetLidarDriverStatus(true); 323 | 324 | return true; 325 | } 326 | 327 | bool LDLidarDriverLinuxInterface::Stop(void) { 328 | if (!is_start_flag_) { 329 | return true; 330 | } 331 | 332 | SetLidarDriverStatus(false); 333 | 334 | is_start_flag_ = false; 335 | 336 | return true; 337 | } 338 | 339 | } // namespace ldlidar 340 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 341 | * FILE ********/ -------------------------------------------------------------------------------- /src/ldlidar_driver_win.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar sdk interface 5 | * This code is only applicable to LDROBOT LiDAR products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2023-03 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver/ldlidar_driver_win.h" 22 | 23 | namespace ldlidar { 24 | 25 | LDLidarDriverWinInterface::LDLidarDriverWinInterface() : 26 | comm_pkg_(new LdLidarDataProcess()), 27 | comm_port_handle_(new SerialInterfaceWin()) { 28 | 29 | last_pubdata_times_ = std::chrono::steady_clock::now(); 30 | } 31 | 32 | LDLidarDriverWinInterface::~LDLidarDriverWinInterface() { 33 | if (comm_pkg_ != nullptr) { 34 | delete comm_pkg_; 35 | } 36 | 37 | if (comm_port_handle_ != nullptr) { 38 | delete comm_port_handle_; 39 | } 40 | } 41 | 42 | bool LDLidarDriverWinInterface::Connect(LDType product_name, 43 | std::string& serial_port_name, 44 | PortParams& port_params) { 45 | 46 | if (is_connect_flag_) { 47 | return true; 48 | } 49 | 50 | if (serial_port_name.empty()) { 51 | LOG_ERROR("input is empty.",""); 52 | return false; 53 | } 54 | 55 | if (register_get_timestamp_handle_ == nullptr) { 56 | LOG_ERROR("get timestamp fuctional is not register.",""); 57 | return false; 58 | } 59 | 60 | comm_pkg_->ClearDataProcessStatus(); 61 | comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); 62 | comm_pkg_->SetProductType(product_name); 63 | 64 | // Win32 65 | // 设置串口参数 66 | comm_port_handle_->setPortParams(port_params); 67 | // 设置串口接收回调函数 68 | comm_port_handle_->setReadCallback(std::bind( 69 | &LdLidarDataProcess::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 70 | // 打开串口 71 | if (!comm_port_handle_->open(serial_port_name)) { 72 | LOG_ERROR("serial is not open:%s", serial_port_name.c_str()); 73 | return false; 74 | } 75 | 76 | is_connect_flag_ = true; 77 | 78 | SetLidarDriverStatus(true); 79 | 80 | return true; 81 | } 82 | 83 | bool LDLidarDriverWinInterface::Disconnect(void) { 84 | if (!is_connect_flag_) { 85 | return true; 86 | } 87 | 88 | SetLidarDriverStatus(false); 89 | 90 | comm_port_handle_->close(); 91 | 92 | is_connect_flag_ = false; 93 | 94 | return true; 95 | } 96 | 97 | void LDLidarDriverWinInterface::EnablePointCloudDataFilter(bool is_enable) { 98 | comm_pkg_->SetNoiseFilter(is_enable); 99 | } 100 | 101 | bool LDLidarDriverWinInterface::WaitLidarComm(int64_t timeout) { 102 | if (!is_connect_flag_) { 103 | return false; 104 | } 105 | 106 | auto last_time = std::chrono::steady_clock::now(); 107 | 108 | bool is_recvflag = false; 109 | do { 110 | if (comm_pkg_->GetLidarPowerOnCommStatus()) { 111 | is_recvflag = true; 112 | } 113 | Sleep(1); 114 | } while (!is_recvflag && (std::chrono::duration_cast( 115 | std::chrono::steady_clock::now() - last_time).count() < timeout)); 116 | 117 | if (is_recvflag) { 118 | SetLidarDriverStatus(true); 119 | return true; 120 | } else { 121 | SetLidarDriverStatus(false); 122 | return false; 123 | } 124 | } 125 | 126 | LidarStatus LDLidarDriverWinInterface::GetLaserScanData(Points2D& dst, int64_t timeout) { 127 | if (!is_start_flag_) { 128 | return LidarStatus::STOP; 129 | } 130 | 131 | LidarStatus status = comm_pkg_->GetLidarStatus(); 132 | if (LidarStatus::NORMAL == status) { 133 | if (comm_pkg_->GetLaserScanData(dst)) { 134 | last_pubdata_times_ = std::chrono::steady_clock::now(); 135 | return LidarStatus::NORMAL; 136 | } 137 | 138 | if (std::chrono::duration_cast( 139 | std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 140 | return LidarStatus::DATA_TIME_OUT; 141 | } else { 142 | return LidarStatus::DATA_WAIT; 143 | } 144 | } else { 145 | last_pubdata_times_ = std::chrono::steady_clock::now(); 146 | return status; 147 | } 148 | } 149 | 150 | LidarStatus LDLidarDriverWinInterface::GetLaserScanData(LaserScan& dst, int64_t timeout) { 151 | if (!is_start_flag_) { 152 | return LidarStatus::STOP; 153 | } 154 | 155 | LidarStatus status = comm_pkg_->GetLidarStatus(); 156 | if (LidarStatus::NORMAL == status) { 157 | Points2D recvpcd; 158 | if (comm_pkg_->GetLaserScanData(recvpcd)) { 159 | last_pubdata_times_ = std::chrono::steady_clock::now(); 160 | dst.stamp = recvpcd.front().stamp; 161 | dst.points = recvpcd; 162 | return LidarStatus::NORMAL; 163 | } 164 | 165 | if (std::chrono::duration_cast( 166 | std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 167 | return LidarStatus::DATA_TIME_OUT; 168 | } else { 169 | return LidarStatus::DATA_WAIT; 170 | } 171 | } else { 172 | last_pubdata_times_ = std::chrono::steady_clock::now(); 173 | return status; 174 | } 175 | } 176 | 177 | bool LDLidarDriverWinInterface::GetLidarScanFreq(double& spin_hz) { 178 | if (!is_start_flag_) { 179 | return false; 180 | } 181 | spin_hz = comm_pkg_->GetSpeed(); 182 | return true; 183 | } 184 | 185 | void LDLidarDriverWinInterface::RegisterGetTimestampFunctional(std::function get_timestamp_handle) { 186 | register_get_timestamp_handle_ = get_timestamp_handle; 187 | } 188 | 189 | uint8_t LDLidarDriverWinInterface::GetLidarErrorCode(void) { 190 | if (!is_start_flag_) { 191 | return LIDAR_NO_ERROR; 192 | } 193 | 194 | uint8_t errcode = comm_pkg_->GetLidarErrorCode(); 195 | return errcode; 196 | } 197 | 198 | bool LDLidarDriverWinInterface::Start(void) { 199 | if (is_start_flag_) { 200 | return true; 201 | } 202 | 203 | if (!is_connect_flag_) { 204 | return false; 205 | } 206 | 207 | is_start_flag_ = true; 208 | 209 | last_pubdata_times_ = std::chrono::steady_clock::now(); 210 | 211 | SetLidarDriverStatus(true); 212 | 213 | return true; 214 | } 215 | 216 | bool LDLidarDriverWinInterface::Stop(void) { 217 | if (!is_start_flag_) { 218 | return true; 219 | } 220 | 221 | SetLidarDriverStatus(false); 222 | 223 | is_start_flag_ = false; 224 | 225 | return true; 226 | } 227 | 228 | } // namespace ldlidar 229 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 230 | * FILE ********/ -------------------------------------------------------------------------------- /src/ldlidar_protocol.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_protocol.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * David Hu(hmd_hubei_cn@163.com) 5 | * @brief 6 | * @version 1.0 7 | * @date 2023.3.11 8 | * @note 9 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | **/ 19 | 20 | #include "ldlidar_driver/ldlidar_protocol.h" 21 | 22 | namespace ldlidar { 23 | 24 | // LD protocol 25 | static const uint8_t CrcTable[256] = { 26 | 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 0xae, 0xf2, 0xbf, 0x68, 0x25, 27 | 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 28 | 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 0xf5, 0x1f, 0x52, 0x85, 0xc8, 29 | 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, 0x3a, 0x94, 0xd9, 0x0e, 0x43, 30 | 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, 0x18, 0x44, 0x09, 0xde, 0x93, 31 | 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, 0xe9, 0x47, 0x0a, 0xdd, 0x90, 32 | 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, 0x62, 0x97, 0xda, 0x0d, 0x40, 33 | 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, 0xb2, 0x1c, 0x51, 0x86, 0xcb, 34 | 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, 0x8f, 0xd3, 0x9e, 0x49, 0x04, 35 | 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 36 | 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 0xd4, 0x7c, 0x31, 0xe6, 0xab, 37 | 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 0x59, 0xf7, 0xba, 0x6d, 0x20, 38 | 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 39 | 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, 0xb4, 0x1a, 0x57, 0x80, 0xcd, 40 | 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 0x3f, 0xca, 0x87, 0x50, 0x1d, 41 | 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 0xef, 0x41, 0x0c, 0xdb, 0x96, 42 | 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 0xec, 0xb0, 0xfd, 0x2a, 0x67, 43 | 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, 0x3c, 0x92, 0xdf, 0x08, 0x45, 44 | 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 45 | 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 0x78, 0xd6, 0x9b, 0x4c, 0x01, 46 | 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 47 | 0x7f, 0x32, 0xe5, 0xa8}; 48 | 49 | uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) { 50 | uint8_t crc = 0; 51 | while (data_len--) { 52 | crc = CrcTable[(crc ^ *data) & 0xff]; 53 | data++; 54 | } 55 | return crc; 56 | } 57 | // << LD protocol 58 | 59 | LdLidarProtocol::LdLidarProtocol() { 60 | 61 | } 62 | 63 | LdLidarProtocol::~LdLidarProtocol() { 64 | 65 | } 66 | 67 | uint8_t LdLidarProtocol::AnalysisDataPacket(uint8_t byte) { 68 | static enum { 69 | HEADER, 70 | VER_LEN, 71 | DATA, 72 | DATA_HEALTH, 73 | DATA_MANUFACTURE, 74 | } state = HEADER; 75 | static uint16_t count = 0; 76 | static uint8_t tmp[128] = {0}; 77 | static uint16_t pkg_count = sizeof(LiDARMeasureDataType); 78 | static uint16_t pkghealth_count = sizeof(LiDARHealthInfoType); 79 | static uint16_t pkgmanufac_count = sizeof(LiDARManufactureInfoType); 80 | static bool check_healthinf_flag = false; 81 | 82 | switch (state) { 83 | case HEADER: { 84 | if (byte == PKG_HEADER) { 85 | tmp[count++] = byte; 86 | state = VER_LEN; 87 | } else { 88 | if (check_healthinf_flag) { 89 | check_healthinf_flag = false; 90 | } 91 | } 92 | break; 93 | } 94 | case VER_LEN: { 95 | if (byte == DATA_PKG_INFO) { 96 | tmp[count++] = byte; 97 | state = DATA; 98 | if (check_healthinf_flag) { 99 | check_healthinf_flag = false; 100 | return GET_PKG_HEALTH; 101 | } 102 | } else if (byte == HEALTH_PKG_INFO) { 103 | tmp[count++] = byte; 104 | state = DATA_HEALTH; 105 | if (check_healthinf_flag) { 106 | check_healthinf_flag = false; 107 | return GET_PKG_HEALTH; 108 | } 109 | } else if (byte == MANUFACT_PKG_INF) { 110 | tmp[count++] = byte; 111 | state = DATA_MANUFACTURE; 112 | } else { 113 | state = HEADER; 114 | count = 0; 115 | if (check_healthinf_flag) { 116 | check_healthinf_flag = false; 117 | } 118 | return GET_PKG_ERROR; 119 | } 120 | break; 121 | } 122 | case DATA: { 123 | tmp[count++] = byte; 124 | if (count >= pkg_count) { 125 | memcpy((uint8_t *)&pcdpkg_data_, tmp, pkg_count); 126 | uint8_t crc = CalCRC8((uint8_t *)&pcdpkg_data_, pkg_count - 1); 127 | state = HEADER; 128 | count = 0; 129 | if (crc == pcdpkg_data_.crc8) { 130 | return GET_PKG_PCD; 131 | } else { 132 | return GET_PKG_ERROR; 133 | } 134 | } 135 | break; 136 | } 137 | case DATA_HEALTH: { 138 | tmp[count++] = byte; 139 | if (count >= pkghealth_count) { 140 | memcpy((uint8_t *)&healthpkg_data_, tmp, pkghealth_count); 141 | uint8_t crc = CalCRC8((uint8_t *)&healthpkg_data_, pkghealth_count - 1); 142 | state = HEADER; 143 | count = 0; 144 | if (crc == healthpkg_data_.crc8) { 145 | check_healthinf_flag = true; 146 | } else { 147 | check_healthinf_flag = false; 148 | } 149 | return GET_PKG_ERROR; 150 | } 151 | break; 152 | } 153 | case DATA_MANUFACTURE: { 154 | tmp[count++] = byte; 155 | if (count >= pkgmanufac_count) { 156 | memcpy((uint8_t *)&manufacinfpkg_data_, tmp, pkgmanufac_count); 157 | uint8_t crc = CalCRC8((uint8_t *)&manufacinfpkg_data_, pkgmanufac_count - 1); 158 | state = HEADER; 159 | count = 0; 160 | if (crc == manufacinfpkg_data_.crc8) { 161 | return GET_PKG_MANUFACT; 162 | } else { 163 | return GET_PKG_ERROR; 164 | } 165 | } 166 | break; 167 | } 168 | default: { 169 | break; 170 | } 171 | } 172 | 173 | return GET_PKG_ERROR; 174 | } 175 | 176 | LiDARMeasureDataType& LdLidarProtocol::GetPCDPacketData(void) { 177 | return pcdpkg_data_; 178 | } 179 | 180 | LiDARHealthInfoType& LdLidarProtocol::GetHealthPacketData(void) { 181 | return healthpkg_data_; 182 | } 183 | 184 | LiDARManufactureInfoType& LdLidarProtocol::GetManufactureInfoPacketData(void) { 185 | return manufacinfpkg_data_; 186 | } 187 | 188 | } // namespace ldlidar 189 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ 190 | -------------------------------------------------------------------------------- /src/log_module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file log_module.cpp 3 | * @author David Hu (hmd_hubei_cn@163.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022.08.10 7 | * @note 8 | * @copyright Copyright (c) 2022 DAVID HU All rights reserved. 9 | * Licensed under the MIT License (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License in the file LICENSE 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | **/ 18 | #include "ldlidar_driver/log_module.h" 19 | 20 | #include 21 | #include 22 | 23 | #ifndef __linux__ 24 | #include 25 | #pragma comment(lib, "comsuppw.lib") 26 | #else 27 | #include 28 | #endif 29 | 30 | /* 使用vswprintf会出现奔溃的情况如果，传入数据大于 VA_PARAMETER_MAX 就会出现崩溃 */ 31 | #define VA_PARAMETER_MAX (1024 * 2) 32 | 33 | LogModule* LogModule::s_plog_module_ = NULL; 34 | 35 | 36 | LogModule* LogModule::GetInstance(__in const char* filename, __in const char* funcname, __in int lineno,LogLevel level,ILogRealization* plog) { 37 | if (s_plog_module_ == NULL) { 38 | s_plog_module_ = new LogModule(); 39 | } 40 | s_plog_module_->logInfo_.str_filename = filename; 41 | s_plog_module_->logInfo_.str_funcname = funcname; 42 | s_plog_module_->logInfo_.n_linenumber = lineno; 43 | s_plog_module_->logInfo_.loglevel = level; 44 | 45 | if (plog != NULL) { 46 | s_plog_module_->p_realization_->free(); 47 | s_plog_module_->p_realization_ = plog; 48 | } 49 | return s_plog_module_; 50 | } 51 | 52 | LogModule* LogModule::GetInstance(LogLevel level, ILogRealization* plog) { 53 | if (s_plog_module_ == NULL) { 54 | s_plog_module_ = new LogModule(); 55 | } 56 | s_plog_module_->logInfo_.loglevel = level; 57 | 58 | if (plog != NULL) { 59 | s_plog_module_->p_realization_->free(); 60 | s_plog_module_->p_realization_ = plog; 61 | } 62 | return s_plog_module_; 63 | } 64 | 65 | LogModule* LogModule::GetInstancePrintOriginData(LogLevel level, ILogRealization* plog) { 66 | if (s_plog_module_ == NULL) { 67 | s_plog_module_ = new LogModule(); 68 | } 69 | s_plog_module_->logInfo_.loglevel = level; 70 | 71 | if (plog != NULL) { 72 | s_plog_module_->p_realization_->free(); 73 | s_plog_module_->p_realization_ = plog; 74 | } 75 | return s_plog_module_; 76 | } 77 | 78 | LogModule::LogModule() { 79 | logInfo_.n_linenumber = -1; 80 | logInfo_.str_filename = ""; 81 | logInfo_.str_funcname = ""; 82 | #ifndef __linux__ 83 | p_realization_ = new LogOutputString(); 84 | #else 85 | p_realization_ = new LogPrint(); 86 | #endif 87 | InitLock(); 88 | } 89 | 90 | LogModule::~LogModule() { 91 | RealseLock(); 92 | } 93 | 94 | void LogModule::LogPrintInf(const char* format,...) { 95 | Lock(); 96 | if (p_realization_) { 97 | std::string str_temp; 98 | str_temp.append("[LOG]"); 99 | // Time [week month day hours:minutes:seconds year] 100 | str_temp.append(GetFormatValue(GetCurrentISOTime())); 101 | // Stamp uint is ns 102 | str_temp.append(GetCurrentLocalTimeStamp()); 103 | // LogLevel 104 | str_temp.append(GetLevelValue(logInfo_.loglevel)); 105 | // File name 106 | str_temp.append(GetFormatValue(logInfo_.str_filename)); 107 | // Function name 108 | str_temp.append(GetFormatValue(logInfo_.str_funcname)); 109 | // Line number 110 | str_temp.append(GetFormatValue(logInfo_.n_linenumber)); 111 | 112 | va_list ptr; 113 | va_start(ptr, format); 114 | char c_value[VA_PARAMETER_MAX] = {0}; 115 | vsnprintf(c_value,sizeof(c_value),format,ptr); 116 | va_end(ptr); 117 | 118 | str_temp.append(GetFormatValue(c_value)); 119 | 120 | p_realization_->LogPrintInf(str_temp.c_str()); 121 | } 122 | UnLock(); 123 | } 124 | 125 | void LogModule::LogPrintNoLocationInf(const char* format,...) { 126 | Lock(); 127 | if (p_realization_) { 128 | std::string str_temp; 129 | // manufacture 130 | str_temp.append("[LOG]"); 131 | // time [week month day hours:minutes:seconds year] 132 | str_temp.append(GetFormatValue(GetCurrentISOTime())); 133 | // stamp uint is ns 134 | str_temp.append(GetCurrentLocalTimeStamp()); 135 | //LogLevel 136 | str_temp.append(GetLevelValue(logInfo_.loglevel)); 137 | 138 | va_list ptr; 139 | va_start(ptr, format); 140 | char c_value[VA_PARAMETER_MAX] = {0}; 141 | vsnprintf(c_value,sizeof(c_value),format,ptr); 142 | va_end(ptr); 143 | 144 | str_temp.append(GetFormatValue(c_value)); 145 | 146 | p_realization_->LogPrintInf(str_temp.c_str()); 147 | } 148 | UnLock(); 149 | } 150 | 151 | void LogModule::LogPrintOriginData(const char* format,...) { 152 | Lock(); 153 | if (p_realization_) { 154 | std::string str_temp; 155 | 156 | switch (logInfo_.loglevel) { 157 | case INFO_LEVEL: { 158 | str_temp.append("[LOG]"); 159 | str_temp.append(GetFormatValue(GetCurrentISOTime())); 160 | str_temp.append(GetCurrentLocalTimeStamp()); 161 | str_temp.append(GetLevelValue(logInfo_.loglevel)); 162 | break; 163 | } 164 | default: { 165 | break; 166 | } 167 | } 168 | 169 | va_list ptr; 170 | va_start(ptr, format); 171 | char c_value[VA_PARAMETER_MAX] = {0}; 172 | vsnprintf(c_value,sizeof(c_value),format,ptr); 173 | va_end(ptr); 174 | 175 | str_temp.append(c_value); 176 | 177 | p_realization_->LogPrintData(str_temp.c_str()); 178 | } 179 | UnLock(); 180 | } 181 | 182 | void LogModule::InitLock() { 183 | #ifndef __linux__ 184 | InitializeCriticalSection(&mutex_lock_); 185 | #else 186 | pthread_mutex_init(&mutex_lock_,NULL); 187 | #endif 188 | } 189 | 190 | void LogModule::RealseLock() { 191 | #ifndef __linux__ 192 | DeleteCriticalSection(&mutex_lock_); 193 | #else 194 | pthread_mutex_unlock(&mutex_lock_); 195 | #endif 196 | } 197 | 198 | void LogModule::Lock() { 199 | #ifndef __linux__ 200 | EnterCriticalSection(&mutex_lock_); 201 | #else 202 | pthread_mutex_lock(&mutex_lock_); 203 | #endif 204 | } 205 | 206 | void LogModule::UnLock() { 207 | #ifndef __linux__ 208 | LeaveCriticalSection(&mutex_lock_); 209 | #else 210 | pthread_mutex_unlock(&mutex_lock_); 211 | #endif 212 | } 213 | 214 | std::string LogModule::GetCurrentISOTime() { 215 | std::string curr_time; 216 | #if 0 217 | //Current date/time based on current time 218 | time_t now = time(0); 219 | // Convert current time to string 220 | curr_time.assign(ctime(&now)); 221 | // Last charactor of currentTime is "\n", so remove it 222 | std::string current_time = curr_time.substr(0, curr_time.size()-1); 223 | return current_time; 224 | #else 225 | char stdtime_str[50] = {0}; 226 | time_t std_time = 0; 227 | struct tm* local_time = NULL; 228 | std_time = time(NULL); 229 | local_time = localtime(&std_time); 230 | snprintf(stdtime_str, 50, "%d-%d-%d,%d:%d:%d", 231 | local_time->tm_year+1900, local_time->tm_mon+1, local_time->tm_mday, 232 | local_time->tm_hour, local_time->tm_min, local_time->tm_sec); 233 | curr_time.assign(stdtime_str); 234 | return curr_time; 235 | #endif 236 | } 237 | 238 | std::string LogModule::GetCurrentLocalTimeStamp() { 239 | std::string stamp_str; 240 | std::chrono::time_point tp = 241 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 242 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 243 | uint64_t timestamp = (uint64_t)tmp.count(); 244 | char s_stamp[100] = {0}; 245 | #ifdef __LP64__ 246 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 247 | #else 248 | #ifdef _WIN64 249 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 250 | #else 251 | snprintf(s_stamp, 100, "[%llu.%llu]", (timestamp/1000000000), (timestamp%1000000000)); 252 | #endif 253 | #endif 254 | stamp_str = s_stamp; 255 | 256 | return stamp_str; 257 | } 258 | 259 | std::string LogModule::GetFormatValue(std::string str_value) { 260 | std::string str_temp; 261 | str_temp.append("["); 262 | str_temp.append(str_value); 263 | str_temp.append("]"); 264 | return str_temp; 265 | } 266 | 267 | std::string LogModule::GetFormatValue(int n_value) { 268 | std::string str_temp; 269 | str_temp.append("["); 270 | char c_value[16]; 271 | sprintf(c_value,"%d",n_value); 272 | str_temp.append(c_value); 273 | str_temp.append("]"); 274 | return str_temp; 275 | } 276 | 277 | std::string LogModule::GetLevelValue(int level){ 278 | std::string tmp; 279 | switch (level) { 280 | case DEBUG_LEVEL: 281 | tmp = "DEBUG"; 282 | break; 283 | case WARNING_LEVEL: 284 | tmp = "WARN"; 285 | break; 286 | case ERROR_LEVEL: 287 | tmp = "ERROR"; 288 | break; 289 | case INFO_LEVEL: 290 | tmp = "INFO"; 291 | break; 292 | default: 293 | tmp = "UnKnown"; 294 | break; 295 | } 296 | std::string str_temp; 297 | str_temp.append("["); 298 | str_temp.append(tmp); 299 | str_temp.append("]"); 300 | return str_temp; 301 | } 302 | 303 | void LogPrint::Initializion(const char* path) { 304 | printf("%s", path); 305 | return ; 306 | } 307 | 308 | void LogPrint::free(ILogRealization *plogger) { 309 | LogPrint* pOutput = static_cast(plogger); 310 | if (pOutput != NULL) { 311 | delete pOutput; 312 | } 313 | } 314 | 315 | void LogPrint::LogPrintInf(const char* str) { 316 | #ifdef ENABLE_CONSOLE_LOG_DISPALY 317 | printf("%s\r\n", str); 318 | #endif 319 | 320 | #ifdef ENABLE_LOG_WRITE_TO_FILE 321 | std::string log_filename = GetLogFilePathName(); 322 | FILE *fp = fopen(log_filename.c_str() ,"a"); 323 | if(!fp) { 324 | printf("%s open filed!\n", log_filename.c_str()); 325 | return ; 326 | } 327 | printf_s(fp,str); 328 | printf_s(fp,"\r\n"); 329 | fclose(fp); 330 | #endif 331 | } 332 | 333 | void LogPrint::LogPrintData(const char* str) { 334 | #ifdef ENABLE_CONSOLE_LOG_DISPALY 335 | printf("%s", str); 336 | #endif 337 | 338 | #ifdef ENABLE_LOG_WRITE_TO_FILE 339 | std::string log_filename = GetOriginDataFilePathName(); 340 | FILE *fp = fopen(log_filename.c_str() ,"a"); 341 | if(!fp) { 342 | printf("%s open filed!\n", log_filename.c_str()); 343 | return ; 344 | } 345 | printf_s(fp,str); 346 | fclose(fp); 347 | #endif 348 | } 349 | 350 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ -------------------------------------------------------------------------------- /src/network_socket_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file network_socket_interface_linux.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux network App 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include "ldlidar_driver/network_socket_interface_linux.h" 21 | #include "ldlidar_driver/log_module.h" 22 | 23 | #define MAX_RECV_BUF_LEN 4096 24 | 25 | namespace ldlidar { 26 | 27 | UDPSocketInterfaceLinux::UDPSocketInterfaceLinux() : recv_thread_(nullptr), 28 | recv_count_(0), 29 | com_sockfd_(-1), 30 | ncd_(NET_NULL), 31 | is_cmd_created_(false), 32 | recv_thread_exit_flag_(true), 33 | is_server_recv_ack_flag_(false), 34 | recv_callback_(nullptr) { 35 | 36 | } 37 | 38 | UDPSocketInterfaceLinux::~UDPSocketInterfaceLinux() { 39 | CloseSocket(); 40 | } 41 | 42 | bool UDPSocketInterfaceLinux::CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port) { 43 | 44 | if (is_cmd_created_ == true) { 45 | return true; 46 | } 47 | 48 | if ((ip == nullptr) || (port == nullptr)) { 49 | LOG_ERROR("UDP,input ip address or port number is null",""); 50 | return false; 51 | } 52 | 53 | if ((UDP_CLIENT == obj) || (UDP_SERVER == obj)) { 54 | com_sockfd_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 55 | if(com_sockfd_ == -1) { 56 | LOG_DEBUG("UDP,fail to socket. %s", strerror(errno)); 57 | return false; 58 | } 59 | } else { 60 | LOG_ERROR("UDP,input value is error",""); 61 | return false; 62 | } 63 | 64 | struct sockaddr_in net_addr; 65 | int addrlen = sizeof(net_addr); 66 | bzero(&net_addr, sizeof(net_addr)); 67 | net_addr.sin_family = AF_INET; 68 | net_addr.sin_addr.s_addr = inet_addr(ip); 69 | net_addr.sin_port = htons((short)atoi(port)); 70 | 71 | if(obj == UDP_SERVER){ 72 | if(-1 == bind(com_sockfd_, (const struct sockaddr*)&net_addr, (socklen_t)addrlen)) { 73 | LOG_ERROR("UDP,fail to bind. %s", strerror(errno)); 74 | if (com_sockfd_ != -1) { 75 | close(com_sockfd_); 76 | com_sockfd_ = -1; 77 | } 78 | return false; 79 | } 80 | LOG_INFO_LITE("UDP,bind success..",""); 81 | } 82 | 83 | ncd_ = obj; 84 | server_ip_ = ip; 85 | server_port_ = port; 86 | 87 | // create recv process thread. 88 | recv_thread_exit_flag_ = false; 89 | recv_thread_ = new std::thread(RecvThreadProc, this); 90 | is_cmd_created_ = true; 91 | 92 | return true; 93 | } 94 | 95 | bool UDPSocketInterfaceLinux::CloseSocket() { 96 | if (is_cmd_created_ == false) { 97 | return true; 98 | } 99 | 100 | recv_thread_exit_flag_ = true; 101 | 102 | if (com_sockfd_ != -1) { 103 | close(com_sockfd_); 104 | com_sockfd_ = -1; 105 | } 106 | 107 | if ((recv_thread_ != nullptr) && recv_thread_->joinable()) { 108 | recv_thread_->join(); 109 | delete recv_thread_; 110 | recv_thread_ = nullptr; 111 | } 112 | 113 | is_cmd_created_ = false; 114 | 115 | return true; 116 | } 117 | 118 | bool UDPSocketInterfaceLinux::RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len) { 119 | static timespec timeout = {0, (long)(100 * 1e6)}; 120 | int32_t len = -1; 121 | struct sockaddr_in sender_addr_inf; 122 | bzero(&sender_addr_inf, sizeof(sender_addr_inf)); 123 | int addrlen = sizeof(struct sockaddr_in); 124 | 125 | if (IsCreated()) { 126 | //// setting and init fd table 127 | fd_set recv_fds; 128 | FD_ZERO(&recv_fds); 129 | FD_SET(com_sockfd_, &recv_fds); 130 | int ret = pselect(com_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 131 | if (ret < 0) { 132 | // select was interrputed 133 | if (errno == EINTR) { 134 | return false; 135 | } 136 | } else if (ret == 0) { 137 | // timeout 138 | return false; 139 | } 140 | 141 | if (FD_ISSET(com_sockfd_, &recv_fds)) { 142 | len = (int32_t)recvfrom(com_sockfd_, rx_buf, rx_buff_len, 0, 143 | (struct sockaddr *)&sender_addr_inf, (socklen_t *)&addrlen); 144 | if ((len != -1) && (rx_len != nullptr)) { 145 | *rx_len = len; 146 | 147 | std::string sender_ip = inet_ntoa(sender_addr_inf.sin_addr); 148 | uint16_t sender_port = ntohs(sender_addr_inf.sin_port); 149 | char sender_port_str[10] = {0}; 150 | snprintf(sender_port_str, 10, "%d", sender_port); 151 | 152 | if (ncd_ == UDP_SERVER) { 153 | if (!is_server_recv_ack_flag_.load()) { 154 | ///// 保存与服务端通信的客户端IP和端口号，仅支持一对一 155 | client_ip_ = sender_ip; 156 | client_port_ = sender_port_str; 157 | is_server_recv_ack_flag_.store(true); 158 | } 159 | } 160 | 161 | // printf("Recv lens:%d\n", len); 162 | // printf("Sender IP:%s\n", sender_ip.c_str()); 163 | // printf("Sender port:%s\n", sender_port_str); 164 | } 165 | } 166 | } 167 | 168 | return ((len == -1) ? false : true); 169 | } 170 | 171 | bool UDPSocketInterfaceLinux::TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len) { 172 | int32_t len = -1; 173 | struct sockaddr_in recver_net_addr; 174 | int addrlen = sizeof(recver_net_addr); 175 | bzero(&recver_net_addr, sizeof(recver_net_addr)); 176 | recver_net_addr.sin_family = AF_INET; 177 | 178 | if (IsCreated()) { 179 | if (ncd_ == UDP_CLIENT) { 180 | recver_net_addr.sin_addr.s_addr = inet_addr(server_ip_.c_str()); 181 | recver_net_addr.sin_port = htons((short)atoi(server_port_.c_str())); 182 | } else if (ncd_ == UDP_SERVER) { 183 | if (!is_server_recv_ack_flag_.load()) { 184 | return false; 185 | } else { 186 | recver_net_addr.sin_addr.s_addr = inet_addr(client_ip_.c_str()); 187 | recver_net_addr.sin_port = htons((short)atoi(client_port_.c_str())); 188 | } 189 | } 190 | 191 | len = (int32_t)sendto(com_sockfd_, tx_buf, tx_buff_len, 0, 192 | (struct sockaddr *)&recver_net_addr, (socklen_t)addrlen); 193 | if ((len != -1) && (tx_len != nullptr)) { 194 | *tx_len = len; 195 | // printf("send lens:%d\n", len); 196 | // printf("Reciver IP:%s\n", inet_ntoa(recver_net_addr.sin_addr)); 197 | // printf("Reciver port:%d\n", ntohs(recver_net_addr.sin_port)); 198 | } 199 | } 200 | 201 | return ((len == -1) ? false : true); 202 | } 203 | 204 | void UDPSocketInterfaceLinux::SetRecvCallback(std::function callback) { 205 | recv_callback_ = callback; 206 | } 207 | 208 | void UDPSocketInterfaceLinux::RecvThreadProc(void *param) { 209 | UDPSocketInterfaceLinux *cmd_if = (UDPSocketInterfaceLinux *)param; 210 | char *recieve_buff = new char[MAX_RECV_BUF_LEN + 1]; 211 | 212 | while (!cmd_if->recv_thread_exit_flag_.load()) { 213 | uint32_t recved = 0; 214 | bool res = cmd_if->RecvFromNet((uint8_t *)recieve_buff, MAX_RECV_BUF_LEN, &recved); 215 | if (res && recved) { 216 | cmd_if->recv_count_ += recved; 217 | if (cmd_if->recv_callback_ != nullptr) { 218 | cmd_if->recv_callback_(recieve_buff, recved); 219 | } 220 | } 221 | } 222 | 223 | delete[] recieve_buff; 224 | } 225 | 226 | 227 | TCPSocketInterfaceLinux::TCPSocketInterfaceLinux() : recv_thread_(nullptr), 228 | recv_count_(0), 229 | com_sockfd_(-1), 230 | listend_client_sockfd_(-1), 231 | is_cmd_created_(false), 232 | recv_thread_exit_flag_(false), 233 | recv_callback_(nullptr) { 234 | 235 | } 236 | 237 | TCPSocketInterfaceLinux::~TCPSocketInterfaceLinux() { 238 | CloseSocket(); 239 | } 240 | 241 | bool TCPSocketInterfaceLinux::CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port) { 242 | if (is_cmd_created_ == true) { 243 | return true; 244 | } 245 | 246 | if ((ip == nullptr) || (port == nullptr)) { 247 | LOG_ERROR("TCP,input ip address or port number is null",""); 248 | return false; 249 | } 250 | 251 | if ((TCP_CLIENT == obj) || (TCP_SERVER == obj)) { 252 | com_sockfd_ = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 253 | if(com_sockfd_ == -1) { 254 | LOG_DEBUG("TCP,fail to socket. %s", strerror(errno)); 255 | return false; 256 | } 257 | } else { 258 | LOG_ERROR("TCP,input value is error",""); 259 | return false; 260 | } 261 | 262 | struct sockaddr_in net_addr; 263 | int addrlen = sizeof(net_addr); 264 | bzero(&net_addr, sizeof(net_addr)); 265 | net_addr.sin_family = AF_INET; 266 | net_addr.sin_addr.s_addr = inet_addr(ip); 267 | net_addr.sin_port = htons((short)atoi(port)); 268 | 269 | ncd_ = obj; 270 | 271 | if (TCP_SERVER == obj) { 272 | // bind sockfd 273 | if (-1 == bind(com_sockfd_,(const struct sockaddr*)&net_addr,(socklen_t)addrlen)) { 274 | LOG_ERROR("TCP,fail to bind. %s", strerror(errno)); 275 | if (com_sockfd_ != -1) { 276 | close(com_sockfd_); 277 | com_sockfd_ = -1; 278 | } 279 | return false; 280 | } 281 | LOG_INFO_LITE("TCP,bind success..",""); 282 | 283 | // sockfd in listen 284 | if (-1 == listen(com_sockfd_, 1)) { 285 | LOG_ERROR("TCP,fail to listen. %s", strerror(errno)); 286 | if (com_sockfd_ != -1) { 287 | close(com_sockfd_); 288 | com_sockfd_ = -1; 289 | } 290 | return false; 291 | } 292 | LOG_INFO_LITE("TCP,wait client connect.",""); 293 | 294 | // wait client request. one by one 295 | struct sockaddr_in client_addr; 296 | int addrlens = sizeof(client_addr); 297 | bzero(&client_addr, sizeof(client_addr)); 298 | 299 | if (-1 == (listend_client_sockfd_ = accept(com_sockfd_, (struct sockaddr*)&client_addr, (socklen_t*)&addrlens))) { 300 | LOG_ERROR("TCP,fail to accept. %s", strerror(errno)); 301 | if (com_sockfd_ != -1) { 302 | close(com_sockfd_); 303 | com_sockfd_ = -1; 304 | } 305 | return false; 306 | } 307 | LOG_INFO_LITE("TCP,connect_client_fd: %d", listend_client_sockfd_); 308 | LOG_INFO_LITE("TCP,client port: %d", ntohs(client_addr.sin_port)); 309 | LOG_INFO_LITE("TCP,client ip: %s", inet_ntoa(client_addr.sin_addr)); 310 | 311 | } else if (TCP_CLIENT == obj) { 312 | // connect to server 313 | if (-1 == connect(com_sockfd_, (const struct sockaddr*)&net_addr, (socklen_t)addrlen)) { 314 | LOG_ERROR("TCP,connect server error. %s", strerror(errno)); 315 | if (com_sockfd_ != -1) { 316 | close(com_sockfd_); 317 | com_sockfd_ = -1; 318 | } 319 | return false; 320 | } 321 | LOG_INFO_LITE("TCP,connect server success.",""); 322 | } 323 | 324 | // create recv process thread. 325 | recv_thread_exit_flag_ = false; 326 | recv_thread_ = new std::thread(RecvThreadProc, this); 327 | is_cmd_created_ = true; 328 | 329 | return true; 330 | } 331 | 332 | bool TCPSocketInterfaceLinux::CloseSocket() { 333 | if (is_cmd_created_ == false) { 334 | return true; 335 | } 336 | 337 | recv_thread_exit_flag_ = true; 338 | 339 | if (com_sockfd_ != -1) { 340 | close(com_sockfd_); 341 | com_sockfd_ = -1; 342 | } 343 | 344 | if (listend_client_sockfd_ != -1) { 345 | close(listend_client_sockfd_); 346 | listend_client_sockfd_ = -1; 347 | } 348 | 349 | if ((recv_thread_ != nullptr) && recv_thread_->joinable()) { 350 | recv_thread_->join(); 351 | delete recv_thread_; 352 | recv_thread_ = nullptr; 353 | } 354 | 355 | is_cmd_created_ = false; 356 | 357 | return true; 358 | } 359 | 360 | bool TCPSocketInterfaceLinux::RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len) { 361 | static timespec timeout = {0, (long)(100 * 1e6)}; 362 | int32_t len = -1; 363 | 364 | if (IsCreated()) { 365 | if (TCP_CLIENT == ncd_) { 366 | //// io multiplexing 367 | fd_set recv_fds; 368 | FD_ZERO(&recv_fds); 369 | FD_SET(com_sockfd_, &recv_fds); 370 | int ret = pselect(com_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 371 | if (ret < 0) { 372 | // select was interrputed 373 | if (errno == EINTR) { 374 | return false; 375 | } 376 | } else if (ret == 0) { 377 | // timeout 378 | return false; 379 | } 380 | 381 | if (FD_ISSET(com_sockfd_, &recv_fds)) { 382 | len = (int32_t)recv(com_sockfd_, rx_buf, rx_buff_len, 0); 383 | if ((len != -1) && (rx_len != nullptr)) { 384 | *rx_len = len; 385 | } 386 | } 387 | } else if (TCP_SERVER == ncd_) { 388 | fd_set recv_fds; 389 | FD_ZERO(&recv_fds); 390 | FD_SET(listend_client_sockfd_, &recv_fds); 391 | int ret = pselect(listend_client_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 392 | if (ret < 0) { 393 | // select was interrputed 394 | if (errno == EINTR) { 395 | return false; 396 | } 397 | } else if (ret == 0) { 398 | // timeout 399 | return false; 400 | } 401 | 402 | if (FD_ISSET(listend_client_sockfd_, &recv_fds)) { 403 | len = (int32_t)recv(listend_client_sockfd_, rx_buf, rx_buff_len, 0); 404 | if ((len != -1) && (rx_len != nullptr)) { 405 | *rx_len = len; 406 | } 407 | } 408 | } 409 | } 410 | 411 | return ((len == -1) ? false : true); 412 | } 413 | 414 | bool TCPSocketInterfaceLinux::TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len) { 415 | int32_t len = 1; 416 | 417 | if (IsCreated()) { 418 | if (TCP_CLIENT == ncd_) { 419 | len = (int32_t)send(com_sockfd_, tx_buf, tx_buff_len, 0); 420 | if ((len != -1) && (tx_len == nullptr)) { 421 | *tx_len = len; 422 | } 423 | } else if (TCP_SERVER == ncd_) { 424 | len = (int32_t)send(listend_client_sockfd_, tx_buf, tx_buff_len, 0); 425 | if ((len != -1) && (tx_len != nullptr)) { 426 | *tx_len = len; 427 | } 428 | } 429 | } 430 | 431 | return ((len == -1) ? false : true); 432 | } 433 | 434 | void TCPSocketInterfaceLinux::SetRecvCallback(std::function callback) { 435 | recv_callback_ = callback; 436 | } 437 | 438 | void TCPSocketInterfaceLinux::RecvThreadProc(void *param) { 439 | TCPSocketInterfaceLinux *cmd_if = (TCPSocketInterfaceLinux *)param; 440 | char *recieve_buff = new char[MAX_RECV_BUF_LEN + 1]; 441 | 442 | while (!cmd_if->recv_thread_exit_flag_.load()) { 443 | uint32_t recved = 0; 444 | bool res = cmd_if->RecvFromNet((uint8_t *)recieve_buff, MAX_RECV_BUF_LEN, &recved); 445 | if (res && recved) { 446 | cmd_if->recv_count_ += recved; 447 | if (cmd_if->recv_callback_ != nullptr) { 448 | cmd_if->recv_callback_(recieve_buff, recved); 449 | } 450 | } 451 | } 452 | 453 | delete[] recieve_buff; 454 | } 455 | 456 | } // namespace ldlidar 457 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 458 | * FILE ********/ -------------------------------------------------------------------------------- /src/serial_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmd_interface_linux.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include "ldlidar_driver/serial_interface_linux.h" 21 | #include "ldlidar_driver/log_module.h" 22 | 23 | 24 | #define MAX_ACK_BUF_LEN 4096 25 | 26 | namespace ldlidar { 27 | 28 | SerialInterfaceLinux::SerialInterfaceLinux() 29 | : rx_thread_(nullptr), rx_count_(0), read_callback_(nullptr) { 30 | com_handle_ = -1; 31 | com_baudrate_ = 0; 32 | } 33 | 34 | SerialInterfaceLinux::~SerialInterfaceLinux() { Close(); } 35 | 36 | bool SerialInterfaceLinux::Open(std::string &port_name, uint32_t com_baudrate) { 37 | int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); 38 | 39 | com_handle_ = open(port_name.c_str(), flags); 40 | if (-1 == com_handle_) { 41 | LOG_ERROR("Open open error,%s", strerror(errno)); 42 | return false; 43 | } 44 | 45 | com_baudrate_ = com_baudrate; 46 | 47 | struct asmtermios::termios2 options; 48 | if (ioctl(com_handle_, _IOC(_IOC_READ, 'T', 0x2A, sizeof(struct asmtermios::termios2)), &options)) { 49 | LOG_ERROR("TCGETS2 first error,%s", strerror(errno)); 50 | if (com_handle_ != -1) { 51 | close(com_handle_); 52 | com_handle_ = -1; 53 | } 54 | return false; 55 | } 56 | 57 | options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8); 58 | options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB); 59 | options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | 60 | ISIG | IEXTEN); //|ECHOPRT 61 | options.c_oflag &= (tcflag_t) ~(OPOST); 62 | options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); 63 | 64 | options.c_cflag &= ~CBAUD; 65 | options.c_cflag |= BOTHER; 66 | options.c_ispeed = this->com_baudrate_; 67 | options.c_ospeed = this->com_baudrate_; 68 | 69 | options.c_cc[VMIN] = 0; 70 | options.c_cc[VTIME] = 0; 71 | 72 | if (ioctl(com_handle_, _IOC(_IOC_WRITE, 'T', 0x2B, sizeof(struct asmtermios::termios2)), &options)) { 73 | LOG_ERROR("TCSETS2 error,%s", strerror(errno)); 74 | if (com_handle_ != -1) { 75 | close(com_handle_); 76 | com_handle_ = -1; 77 | } 78 | return false; 79 | } 80 | 81 | if (ioctl(com_handle_, _IOC(_IOC_READ, 'T', 0x2A, sizeof(struct asmtermios::termios2)), &options)) { 82 | LOG_ERROR("TCGETS2 second error,%s", strerror(errno)); 83 | if (com_handle_ != -1) { 84 | close(com_handle_); 85 | com_handle_ = -1; 86 | } 87 | return false; 88 | } 89 | 90 | tcflush(com_handle_, TCIFLUSH); 91 | 92 | rx_thread_exit_flag_ = false; 93 | rx_thread_ = new std::thread(RxThreadProc, this); 94 | is_cmd_opened_ = true; 95 | 96 | return true; 97 | } 98 | 99 | bool SerialInterfaceLinux::Close() { 100 | if (is_cmd_opened_ == false) { 101 | return true; 102 | } 103 | 104 | rx_thread_exit_flag_ = true; 105 | 106 | if (com_handle_ != -1) { 107 | close(com_handle_); 108 | com_handle_ = -1; 109 | } 110 | 111 | if ((rx_thread_ != nullptr) && rx_thread_->joinable()) { 112 | rx_thread_->join(); 113 | delete rx_thread_; 114 | rx_thread_ = nullptr; 115 | } 116 | 117 | is_cmd_opened_ = false; 118 | 119 | return true; 120 | } 121 | 122 | bool SerialInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, 123 | uint32_t *rx_len) { 124 | static timespec timeout = {0, (long)(100 * 1e6)}; 125 | int32_t len = -1; 126 | 127 | if (IsOpened()) { 128 | fd_set read_fds; 129 | FD_ZERO(&read_fds); 130 | FD_SET(com_handle_, &read_fds); 131 | int r = pselect(com_handle_ + 1, &read_fds, NULL, NULL, &timeout, NULL); 132 | if (r < 0) { 133 | // Select was interrupted 134 | if (errno == EINTR) { 135 | return false; 136 | } 137 | } else if (r == 0) { // timeout 138 | return false; 139 | } 140 | 141 | if (FD_ISSET(com_handle_, &read_fds)) { 142 | len = (int32_t)read(com_handle_, rx_buf, rx_buf_len); 143 | if ((len != -1) && rx_len) { 144 | *rx_len = len; 145 | } 146 | } 147 | } 148 | return len == -1 ? false : true; 149 | } 150 | 151 | bool SerialInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, 152 | uint32_t *tx_len) { 153 | int32_t len = -1; 154 | 155 | if (IsOpened()) { 156 | len = (int32_t)write(com_handle_, tx_buf, tx_buf_len); 157 | if ((len != -1) && tx_len) { 158 | *tx_len = len; 159 | } 160 | } 161 | return len == -1 ? false : true; 162 | } 163 | 164 | void SerialInterfaceLinux::RxThreadProc(void *param) { 165 | SerialInterfaceLinux *cmd_if = (SerialInterfaceLinux *)param; 166 | char *rx_buf = new char[MAX_ACK_BUF_LEN + 1]; 167 | while (!cmd_if->rx_thread_exit_flag_.load()) { 168 | uint32_t readed = 0; 169 | bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); 170 | if (res && readed) { 171 | cmd_if->rx_count_ += readed; 172 | if (cmd_if->read_callback_ != nullptr) { 173 | cmd_if->read_callback_(rx_buf, readed); 174 | } 175 | } 176 | } 177 | 178 | delete[] rx_buf; 179 | } 180 | 181 | } // namespace ldlidar 182 | 183 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 184 | * FILE ********/ -------------------------------------------------------------------------------- /src/serial_interface_win.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file serial_interface_win.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief Win32 serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include "ldlidar_driver/serial_interface_win.h" 21 | #include "ldlidar_driver/log_module.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #pragma comment(lib, "SetupAPI.lib") 33 | #pragma comment(lib, "Cfgmgr32.lib") 34 | 35 | using namespace std; 36 | 37 | SerialInterfaceWin::SerialInterfaceWin() 38 | : mIsOpened(false), 39 | mByteToRead(4096), 40 | mRxCounter(0), 41 | mTxCounter(0), 42 | commErrorHandle(nullptr), 43 | readCallback(nullptr), 44 | mRxThread(nullptr), 45 | mMaxBuffSize(4096) { 46 | mComHandle = INVALID_HANDLE_VALUE; 47 | memset(&mOverlappedSend, 0, sizeof(OVERLAPPED)); 48 | memset(&mOverlappedRecv, 0, sizeof(OVERLAPPED)); 49 | mOverlappedSend.hEvent = CreateEvent(NULL, FALSE, FALSE, NULL); 50 | mOverlappedRecv.hEvent = CreateEvent(NULL, FALSE, FALSE, NULL); 51 | } 52 | 53 | SerialInterfaceWin::~SerialInterfaceWin() { 54 | if (mOverlappedRecv.hEvent != INVALID_HANDLE_VALUE) { 55 | CloseHandle(mOverlappedRecv.hEvent); 56 | mOverlappedRecv.hEvent = INVALID_HANDLE_VALUE; 57 | } 58 | if (mOverlappedSend.hEvent != INVALID_HANDLE_VALUE) { 59 | CloseHandle(mOverlappedSend.hEvent); 60 | mOverlappedSend.hEvent = INVALID_HANDLE_VALUE; 61 | } 62 | } 63 | 64 | std::string wcharToChar(const wchar_t *wp, UINT encode = CP_ACP) { 65 | std::string str; 66 | int len = 67 | WideCharToMultiByte(encode, 0, wp, (int)wcslen(wp), NULL, 0, NULL, NULL); 68 | char *m_char = new char[len + 1]; 69 | WideCharToMultiByte(encode, 0, wp, (int)wcslen(wp), m_char, len, NULL, NULL); 70 | m_char[len] = '\0'; 71 | str = m_char; 72 | delete m_char; 73 | return str; 74 | } 75 | 76 | bool SerialInterfaceWin::availablePorts(vector &availabelPortInfo) { 77 | DWORD dwGuids = 0; 78 | TCHAR propBuf[1024]; 79 | PortInfo portInfo; 80 | 81 | availabelPortInfo.clear(); 82 | 83 | SetupDiClassGuidsFromName(_T("Ports"), NULL, 0, &dwGuids); 84 | if (dwGuids == 0) return false; 85 | 86 | GUID *pGuids = new GUID[dwGuids]; 87 | SetupDiClassGuidsFromName(_T("Ports"), pGuids, dwGuids, &dwGuids); 88 | 89 | for (DWORD i = 0; i < dwGuids; i++) { 90 | HDEVINFO hDevInfo = 91 | SetupDiGetClassDevs(&pGuids[i], NULL, NULL, DIGCF_PRESENT); 92 | if (hDevInfo == INVALID_HANDLE_VALUE) break; 93 | 94 | for (int index = 0;; index++) { 95 | SP_DEVINFO_DATA devInfoData; 96 | 97 | devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); 98 | 99 | if (!SetupDiEnumDeviceInfo(hDevInfo, index, &devInfoData)) { 100 | break; 101 | } 102 | propBuf[0] = 0; 103 | 104 | CM_Get_Device_ID(devInfoData.DevInst, propBuf, 1024, 0); 105 | 106 | string s(propBuf); 107 | 108 | string wusb = s.substr(0, 3); 109 | 110 | if (wusb == "USB") { 111 | portInfo.vid = s.substr(8, 4); 112 | portInfo.pid = s.substr(17, 4); 113 | } else { 114 | portInfo.pid = "None"; 115 | portInfo.vid = "None"; 116 | } 117 | 118 | if (!SetupDiGetDeviceRegistryProperty(hDevInfo, &devInfoData, 119 | SPDRP_DEVICEDESC, NULL, 120 | (PBYTE)propBuf, MAX_PATH - 1, NULL)) 121 | continue; 122 | 123 | portInfo.description = propBuf; 124 | 125 | HKEY hDeviceKey = 126 | SetupDiOpenDevRegKey(hDevInfo, &devInfoData, DICS_FLAG_GLOBAL, 0, 127 | DIREG_DEV, KEY_QUERY_VALUE); 128 | if (hDeviceKey) { 129 | propBuf[0] = 0; 130 | DWORD dw_size = sizeof(propBuf); 131 | DWORD dw_type = 0; 132 | 133 | if ((RegQueryValueEx(hDeviceKey, _T("PortName"), NULL, &dw_type, 134 | (LPBYTE)propBuf, &dw_size) == ERROR_SUCCESS) && 135 | (dw_type == REG_SZ)) { 136 | portInfo.name = propBuf; 137 | availabelPortInfo.push_back(portInfo); 138 | } 139 | } 140 | } 141 | SetupDiDestroyDeviceInfoList(hDevInfo); 142 | } 143 | delete[] pGuids; 144 | 145 | return true; 146 | } 147 | 148 | bool SerialInterfaceWin::open(string portName) { 149 | if (mIsOpened) close(); 150 | 151 | portParams.portName = portName; 152 | 153 | std::string name = "\\\\.\\" + portName; 154 | 155 | mComHandle = ::CreateFile(name.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, 156 | OPEN_EXISTING, 157 | FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL); 158 | 159 | if (mComHandle == INVALID_HANDLE_VALUE) { 160 | CloseHandle(mComHandle); 161 | return false; 162 | } 163 | 164 | PurgeComm(mComHandle, 165 | PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); 166 | if (SetupComm(mComHandle, (DWORD)mMaxBuffSize, (DWORD)mMaxBuffSize) == 0) { 167 | CloseHandle(mComHandle); 168 | return false; 169 | } 170 | 171 | DCB dcb; 172 | dcb.DCBlength = sizeof(DCB); 173 | if (GetCommState(mComHandle, &dcb) == 0) { 174 | CloseHandle(mComHandle); 175 | return false; 176 | } else { 177 | dcb.BaudRate = (DWORD)portParams.baudrate; 178 | dcb.ByteSize = (BYTE)portParams.dataBits; 179 | dcb.Parity = (BYTE)portParams.parity; 180 | dcb.StopBits = (BYTE)portParams.stopBits; 181 | if (portParams.flowControl == FlowControl::SoftwareControl) { 182 | dcb.fOutX = 1; 183 | dcb.fInX = 1; 184 | } else if (portParams.flowControl == FlowControl::HardwareControl) { 185 | dcb.fRtsControl = 0; 186 | dcb.fDtrControl = 1; 187 | } else { 188 | dcb.fOutX = 0; 189 | dcb.fInX = 0; 190 | dcb.fRtsControl = 0; 191 | dcb.fDtrControl = 0; 192 | } 193 | 194 | if (SetCommState(mComHandle, &dcb) == 0) { 195 | CloseHandle(mComHandle); 196 | return false; 197 | } 198 | } 199 | 200 | COMMTIMEOUTS timeouts; 201 | timeouts.ReadIntervalTimeout = 2; 202 | timeouts.ReadTotalTimeoutMultiplier = 0; 203 | timeouts.ReadTotalTimeoutConstant = 5; 204 | timeouts.WriteTotalTimeoutConstant = 50000; 205 | timeouts.WriteTotalTimeoutMultiplier = 1000; 206 | SetCommTimeouts(mComHandle, &timeouts); 207 | 208 | mIsOpened = true; 209 | 210 | mRxThreadRunFlag = true; 211 | mRxThread = new std::thread(rxThreadProc, this); 212 | 213 | return true; 214 | } 215 | 216 | bool SerialInterfaceWin::close() { 217 | if (!mIsOpened) return true; 218 | 219 | mIsOpened = false; 220 | mRxThreadRunFlag = false; 221 | 222 | if (mComHandle != INVALID_HANDLE_VALUE) { 223 | CloseHandle(mComHandle); 224 | mComHandle = INVALID_HANDLE_VALUE; 225 | } 226 | 227 | try { 228 | if (mRxThread->joinable()) mRxThread->join(); 229 | } catch (const std::system_error &e) { 230 | LOG_INFO("Caught system_error with code:%d, meaning:%s", e.code(), e.what()); 231 | } 232 | delete mRxThread; 233 | mRxThread = nullptr; 234 | 235 | return true; 236 | } 237 | 238 | int SerialInterfaceWin::readAll() { return 0; } 239 | 240 | bool SerialInterfaceWin::write(const char *txBuf, uint32_t txBufLen) { 241 | DWORD txLen = 0; 242 | if (mIsOpened) { 243 | ResetEvent(mOverlappedSend.hEvent); 244 | uint32_t res = WriteFile(mComHandle, txBuf, txBufLen, (LPDWORD)&txLen, 245 | &mOverlappedSend); 246 | if (!res) { 247 | if (GetLastError() == ERROR_IO_PENDING) { 248 | WaitForSingleObject(mOverlappedSend.hEvent, INFINITE); 249 | GetOverlappedResult(mComHandle, &mOverlappedSend, (LPDWORD)&txLen, 250 | FALSE); 251 | mTxCounter += txLen; 252 | return true; 253 | } 254 | } 255 | } 256 | 257 | return false; 258 | } 259 | 260 | void SerialInterfaceWin::setPortParams(PortParams params) { 261 | memcpy(&portParams, ¶ms, sizeof(params)); 262 | } 263 | 264 | PortParams SerialInterfaceWin::currPortParams(void) { return portParams; } 265 | 266 | void SerialInterfaceWin::rxThreadProc(SerialInterfaceWin *pClass) { 267 | char *rxBuf = new char[pClass->mMaxBuffSize + 1]; 268 | ResetEvent(pClass->mOverlappedRecv.hEvent); 269 | LOG_INFO("rx thread start...",""); 270 | 271 | map error_code{ 272 | {ERROR_INVALID_PARAMETER, "Invalid parameter"}, 273 | {ERROR_ACCESS_DENIED, "Access denied"}, 274 | {ERROR_INVALID_HANDLE, "Open failed"}, 275 | {ERROR_BAD_COMMAND, "Illegal disconnection"}, 276 | }; 277 | 278 | while (pClass->mRxThreadRunFlag) { 279 | uint32_t readed = 0; 280 | bool res = ReadFile(pClass->mComHandle, rxBuf, (DWORD)pClass->mByteToRead, 281 | (LPDWORD)&readed, &pClass->mOverlappedRecv); 282 | if (res == FALSE) { 283 | long last_error = GetLastError(); 284 | switch (last_error) { 285 | case ERROR_IO_PENDING: { 286 | if (WaitForSingleObject( 287 | pClass->mOverlappedRecv.hEvent, INFINITE) == WAIT_OBJECT_0) { 288 | GetOverlappedResult(pClass->mComHandle, &pClass->mOverlappedRecv, (LPDWORD)&readed, FALSE); 289 | if (readed) { 290 | pClass->mRxCounter += readed; 291 | // callback 292 | if (pClass->readCallback != nullptr) { 293 | pClass->readCallback(rxBuf, readed); 294 | } 295 | } 296 | } 297 | break; 298 | } 299 | case ERROR_INVALID_PARAMETER: /* 系统错误 erroe code:87 */ 300 | case ERROR_ACCESS_DENIED: /* 拒绝访问 erroe code:5 */ 301 | case ERROR_INVALID_HANDLE: /* 打开串口失败 erroe code:6 */ 302 | case ERROR_BAD_COMMAND: /* 连接过程中非法断开 erroe code:22 */ 303 | { 304 | /* 不能再这里及回调函数中调用close，因为close中有join。在当前线程中join自己，会造成死锁 */ 305 | if (pClass->commErrorHandle != nullptr) 306 | pClass->commErrorHandle(error_code[last_error]); 307 | pClass->mRxThreadRunFlag = false; 308 | 309 | break; 310 | } 311 | 312 | default: /* 发生其他错误，其中有串口读写中断开串口连接的错误（错误22） */ 313 | { 314 | if (pClass->commErrorHandle != nullptr) 315 | pClass->commErrorHandle("Unknow error"); 316 | break; 317 | } 318 | } 319 | 320 | ResetEvent(pClass->mOverlappedRecv.hEvent); 321 | } 322 | } 323 | 324 | delete[] rxBuf; 325 | } 326 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ 327 | -------------------------------------------------------------------------------- /src/sl_transform.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file transform.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief Ranging center conversion with left and right hand system changes App 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #include "ldlidar_driver/sl_transform.h" 23 | 24 | namespace ldlidar { 25 | 26 | /*! 27 | \brief transfer the origin to the center of lidar circle 28 | \param[in] 29 | \arg version lidar version, different versions have different 30 | parameter settings \arg data lidar raw data \arg to_right_hand_ a flag 31 | whether convert to right-hand coordinate system 32 | 33 | \param[out] data 34 | \retval Data after coordinate conversion 35 | */ 36 | SlTransform::SlTransform(LDType version, bool to_right_hand) { 37 | switch (version) { 38 | case LDType::LD_14: 39 | case LDType::LD_14P: 40 | offset_x_ = 5.9; 41 | offset_y_ = -18.975571; 42 | break; 43 | default: 44 | offset_x_ = 5.9; 45 | offset_y_ = -20.14; 46 | break; 47 | } 48 | to_right_hand_ = to_right_hand; 49 | version_ = version; 50 | } 51 | 52 | Points2D SlTransform::Transform(const Points2D &data) { 53 | Points2D tmp2; 54 | static double last_shift_delta = 0; 55 | for (auto n : data) { 56 | // transfer the origin to the center of lidar circle 57 | // The default direction of radar rotation is clockwise 58 | // transfer to the right-hand coordinate system 59 | double angle; 60 | if (n.distance > 0) { 61 | double x = n.distance + offset_x_; 62 | double y = n.distance * 0.11923 + offset_y_; 63 | double shift = atan(y / x) * 180.f / 3.14159; 64 | // Choose whether to use the right-hand system according to the flag 65 | if (to_right_hand_) { 66 | float right_hand = (360.f - n.angle); 67 | angle = right_hand + shift; 68 | } else { 69 | angle = n.angle - shift; 70 | } 71 | last_shift_delta = shift; 72 | } else { 73 | if (to_right_hand_) { 74 | float right_hand = (360.f - n.angle); 75 | angle = right_hand + last_shift_delta; 76 | } else { 77 | angle = n.angle - last_shift_delta; 78 | } 79 | } 80 | 81 | if (angle > 360) { 82 | angle -= 360; 83 | } 84 | if (angle < 0) { 85 | angle += 360; 86 | } 87 | 88 | switch (version_) { 89 | case LDType::LD_14: 90 | case LDType::LD_14P: 91 | if (n.distance == 0) { 92 | tmp2.push_back(PointData(angle, n.distance, 0, n.stamp)); 93 | } else { 94 | tmp2.push_back(PointData(angle, n.distance, n.intensity, n.stamp)); 95 | } 96 | break; 97 | default: 98 | break; 99 | } 100 | } 101 | 102 | return tmp2; 103 | } 104 | 105 | SlTransform::~SlTransform() {} 106 | 107 | } // namespace ldlidar 108 | 109 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 110 | * FILE ********/ -------------------------------------------------------------------------------- /src/slbf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file slbf.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver/slbf.h" 22 | 23 | namespace ldlidar { 24 | 25 | /*! 26 | \brief Set current speed 27 | \param[in] 28 | \arg speed Current lidar speed 29 | \arg strict_policy The flag to enable very strict filtering 30 | \param[out] none 31 | \retval none 32 | */ 33 | Slbf::Slbf(int speed, bool strict_policy) { 34 | curr_speed_ = speed; 35 | enable_strict_policy_ = strict_policy; 36 | } 37 | 38 | Slbf::~Slbf() {} 39 | 40 | /*! 41 | \brief Filter within 1m to filter out unreasonable data points 42 | \param[in] 43 | \arg data A circle of lidar data packed 44 | \param[out] none 45 | \retval Standard data 46 | */ 47 | Points2D Slbf::NearFilter(const Points2D &data) const { 48 | Points2D normal, pending, item; 49 | std::vector group; 50 | int sunshine_amount = 0; 51 | int dis_limit = 0; 52 | 53 | for (auto n : data) { 54 | if (n.distance < 20000) { 55 | pending.push_back(n); 56 | } else { 57 | normal.push_back(n); 58 | } 59 | } 60 | 61 | if (data.empty()) return normal; 62 | 63 | double angle_delta_up_limit = curr_speed_ / kScanFre * 1.5; 64 | double angle_delta_down_limit = curr_speed_ / kScanFre - 0.18; 65 | std::sort(pending.begin(), pending.end(), [](PointData a, PointData b) { return a.angle < b.angle; }); 66 | 67 | PointData last(-10, 0, 0); 68 | 69 | for (auto n : pending) { 70 | dis_limit = 50; 71 | if (n.distance > 1000) dis_limit = n.distance / 20; 72 | if (fabs(n.angle - last.angle) > angle_delta_up_limit || 73 | abs(n.distance - last.distance) > dis_limit) { 74 | if (item.empty() == false) { 75 | group.push_back(item); 76 | item.clear(); 77 | } 78 | } 79 | item.push_back(n); 80 | last = n; 81 | } 82 | 83 | if (item.empty() == false) group.push_back(item); 84 | 85 | if (group.empty()) return normal; 86 | 87 | auto first_item = group.front().front(); 88 | auto last_item = group.back().back(); 89 | dis_limit = (first_item.distance + last_item.distance) / 2 / 20; 90 | if (dis_limit < 50) dis_limit = 50; 91 | if (fabs(first_item.angle + 360.f - last_item.angle) < angle_delta_up_limit && 92 | abs(first_item.distance - last_item.distance) < dis_limit) { 93 | if (group.size() > 1) { 94 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 95 | group.erase(group.end() - 1); 96 | } 97 | } 98 | 99 | for (auto n : group) { 100 | if (n.size() == 0) continue; 101 | 102 | if (n.size() > 35) { 103 | normal.insert(normal.end(), n.begin(), n.end()); 104 | continue; 105 | } 106 | 107 | for (auto m : n) { 108 | int flag = m.intensity & 0x01; 109 | sunshine_amount += (flag == 1); 110 | } 111 | 112 | double sunshine_rate = (double)sunshine_amount / (double)n.size(); 113 | 114 | double confidence_avg = 0; 115 | double dis_avg = 0; 116 | for (auto m : n) { 117 | confidence_avg += m.intensity; 118 | dis_avg += m.distance; 119 | } 120 | confidence_avg /= n.size(); 121 | dis_avg /= n.size(); 122 | 123 | if (dis_avg > 1000 && sunshine_rate < 0.2 && 124 | confidence_avg > kConfidenceHigh && n.size() > 2) { 125 | normal.insert(normal.end(), n.begin(), n.end()); 126 | continue; 127 | } 128 | 129 | if (sunshine_rate > 0.5 && confidence_avg < kConfidenceLow) { 130 | for (auto& point : n) { 131 | point.distance = 0; 132 | point.intensity = 0; 133 | } 134 | normal.insert(normal.end(), n.begin(), n.end()); 135 | continue; 136 | } 137 | 138 | if (enable_strict_policy_) { 139 | if (dis_avg > 8100 && confidence_avg < kConfidenceLow && n.size() < 1) { 140 | for (auto& point : n) { 141 | point.distance = 0; 142 | point.intensity = 0; 143 | } 144 | normal.insert(normal.end(), n.begin(), n.end()); 145 | continue; 146 | } else if (dis_avg > 6000 && confidence_avg < kConfidenceLow && n.size() < 2) { 147 | for (auto& point : n) { 148 | point.distance = 0; 149 | point.intensity = 0; 150 | } 151 | normal.insert(normal.end(), n.begin(), n.end()); 152 | continue; 153 | } else if (dis_avg > 4000 && confidence_avg < kConfidenceHigh && n.size() < 2) { 154 | for (auto& point : n) { 155 | point.distance = 0; 156 | point.intensity = 0; 157 | } 158 | normal.insert(normal.end(), n.begin(), n.end()); 159 | continue; 160 | } else if (dis_avg > 300 /*&& confidence_avg < kConfidenceHigh */ && n.size() < 2) { 161 | for (auto& point : n) { 162 | point.distance = 0; 163 | point.intensity = 0; 164 | } 165 | normal.insert(normal.end(), n.begin(), n.end()); 166 | continue; 167 | } 168 | 169 | if (dis_avg < 300 && confidence_avg < kConfidenceHigh && n.size() < 3) { 170 | for (auto& point : n) { 171 | point.distance = 0; 172 | point.intensity = 0; 173 | } 174 | normal.insert(normal.end(), n.begin(), n.end()); 175 | continue; 176 | } 177 | 178 | if (dis_avg < 300 && sunshine_rate > 0.5 && 179 | confidence_avg < kConfidenceMiddle && n.size() < 5) { 180 | for (auto& point : n) { 181 | point.distance = 0; 182 | point.intensity = 0; 183 | } 184 | normal.insert(normal.end(), n.begin(), n.end()); 185 | continue; 186 | } 187 | 188 | if (dis_avg < 200 && sunshine_rate > 0.4 && 189 | confidence_avg < kConfidenceMiddle && n.size() < 6) { 190 | for (auto& point : n) { 191 | point.distance = 0; 192 | point.intensity = 0; 193 | } 194 | normal.insert(normal.end(), n.begin(), n.end()); 195 | continue; 196 | } 197 | 198 | if (dis_avg < 500 && sunshine_rate > 0.9 && n.size() < 3) { 199 | for (auto& point : n) { 200 | point.distance = 0; 201 | point.intensity = 0; 202 | } 203 | normal.insert(normal.end(), n.begin(), n.end()); 204 | continue; 205 | } 206 | 207 | if (dis_avg < 200 && confidence_avg < kConfidenceMiddle && n.size() < 3) { 208 | for (auto& point : n) { 209 | point.distance = 0; 210 | point.intensity = 0; 211 | } 212 | normal.insert(normal.end(), n.begin(), n.end()); 213 | continue; 214 | } 215 | } 216 | 217 | double diff_avg = 0; 218 | int n_size = (int)n.size(); 219 | for (int i = 1; i < n_size; i++) { 220 | if (n[i].angle > n[i - 1].angle) { 221 | diff_avg += fabs(n[i].angle - n[i - 1].angle); 222 | } else { 223 | diff_avg += fabs(n[i].angle + 360.0 - n[i - 1].angle); 224 | } 225 | } 226 | diff_avg /= (double)(n.size() - 1); 227 | 228 | if (diff_avg > angle_delta_down_limit) { 229 | normal.insert(normal.end(), n.begin(), n.end()); 230 | } else { 231 | for (auto& point : n) { 232 | point.distance = 0; 233 | point.intensity = 0; 234 | } 235 | normal.insert(normal.end(), n.begin(), n.end()); 236 | } 237 | } 238 | 239 | return normal; 240 | } 241 | 242 | /*! 243 | \brief Enable strong filtering 244 | \param[in] 245 | \arg enable : true ，false 246 | \param[out] none 247 | \retval 248 | */ 249 | void Slbf::EnableStrictPolicy(bool enable) { enable_strict_policy_ = enable; } 250 | 251 | } // namespace ldlidar 252 | 253 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 254 | * FILE ********/ 255 | -------------------------------------------------------------------------------- /src/tofbf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tofbf.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2017-2023 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #include "ldlidar_driver/tofbf.h" 23 | 24 | namespace ldlidar { 25 | 26 | /** 27 | * @brief Construct a new Tofbf:: Tofbf object 28 | * @param [in] 29 | * @param speed current lidar speed 30 | */ 31 | Tofbf::Tofbf(int speed, LDType type) { 32 | curr_speed_ = speed; // ldliar spin speed, unit is Degrees per second 33 | switch (type) 34 | { 35 | case LDType::LD_06: 36 | case LDType::LD_19: 37 | intensity_low_ = 15; 38 | intensity_single_ = 220; 39 | scan_frequency_ = 4500; 40 | filter_type_ = FilterType::NEAR_FILTER; 41 | break; 42 | case LDType::STL_06P: 43 | case LDType::STL_26: 44 | case LDType::STL_27L: 45 | filter_type_ = FilterType::NOISE_FILTER; 46 | break; 47 | default: 48 | std::cout << "tofbf input ldlidar type error!" << std::endl; 49 | filter_type_ = FilterType::NO_FILTER; 50 | break; 51 | } 52 | } 53 | 54 | Tofbf::~Tofbf() {} 55 | 56 | /** 57 | * @brief The tof lidar filter 58 | * @param [in] 59 | * @param tmp lidar point data 60 | * @return std::vector 61 | */ 62 | std::vector Tofbf::Filter( 63 | const std::vector &tmp) const { 64 | std::vector normal; 65 | 66 | switch(filter_type_){ 67 | case FilterType::NEAR_FILTER: 68 | normal = NearFilter(tmp); 69 | break; 70 | 71 | case FilterType::NOISE_FILTER: 72 | normal = NoiseFilter(tmp); 73 | break; 74 | 75 | default: 76 | normal = tmp; 77 | break; 78 | } 79 | 80 | return normal; 81 | } 82 | 83 | std::vector Tofbf::NearFilter( 84 | const std::vector &tmp) const { 85 | std::vector normal, pending, item; 86 | std::vector> group; 87 | 88 | // Remove points within 5m 89 | for (auto n : tmp) { 90 | if (n.distance < 5000) { 91 | pending.push_back(n); 92 | } else { 93 | normal.push_back(n); 94 | } 95 | } 96 | 97 | if (tmp.empty()) return normal; 98 | 99 | double angle_delta_up_limit = curr_speed_ / scan_frequency_ * 2; 100 | 101 | // sort 102 | std::sort(pending.begin(), pending.end(), 103 | [](PointData a, PointData b) { return a.angle < b.angle; }); 104 | 105 | PointData last(-10, 0, 0); 106 | // group 107 | for (auto n : pending) { 108 | if (fabs(n.angle - last.angle) > angle_delta_up_limit || 109 | fabs(n.distance - last.distance) > last.distance * 0.03) { 110 | if (item.empty() == false) { 111 | group.push_back(item); 112 | item.clear(); 113 | } 114 | } 115 | item.push_back(n); 116 | last = n; 117 | } 118 | // push back last item 119 | if (item.empty() == false) group.push_back(item); 120 | 121 | if (group.empty()) return normal; 122 | 123 | // Connection 0 degree and 359 degree 124 | auto first_item = group.front().front(); 125 | auto last_item = group.back().back(); 126 | if (fabs(first_item.angle + 360.f - last_item.angle) < angle_delta_up_limit && 127 | fabs(first_item.distance - last_item.distance) < last.distance * 0.03) { 128 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 129 | group.erase(group.end() - 1); 130 | } 131 | // selection 132 | for (auto n : group) { 133 | if (n.size() == 0) continue; 134 | // No filtering if there are many points 135 | if (n.size() > 15) { 136 | normal.insert(normal.end(), n.begin(), n.end()); 137 | continue; 138 | } 139 | 140 | // Filter out those with few points 141 | if (n.size() < 3) { 142 | int c = 0; 143 | for (auto m : n) { 144 | c += m.intensity; 145 | } 146 | c /= n.size(); 147 | if (c < intensity_single_) { 148 | for (auto& point: n) { 149 | point.distance = 0; 150 | point.intensity = 0; 151 | } 152 | normal.insert(normal.end(), n.begin(), n.end()); 153 | continue; 154 | } 155 | } 156 | 157 | // Calculate the mean value of distance and intensity 158 | double intensity_avg = 0; 159 | double dis_avg = 0; 160 | for (auto m : n) { 161 | intensity_avg += m.intensity; 162 | dis_avg += m.distance; 163 | } 164 | intensity_avg /= n.size(); 165 | dis_avg /= n.size(); 166 | 167 | // High intensity, no filtering 168 | if (intensity_avg > intensity_low_) { 169 | normal.insert(normal.end(), n.begin(), n.end()); 170 | continue; 171 | } else { 172 | for (auto& point : n) { 173 | point.distance = 0; 174 | point.intensity = 0; 175 | } 176 | normal.insert(normal.end(), n.begin(), n.end()); 177 | continue; 178 | } 179 | } 180 | 181 | return normal; 182 | } 183 | 184 | std::vector Tofbf::NoiseFilter( 185 | const std::vector &tmp) const { 186 | std::vector normal; 187 | PointData last_data, next_data; 188 | 189 | if (tmp.empty()) return normal; 190 | 191 | // Traversing the point data 192 | int count = 0; 193 | for (auto n : tmp) { 194 | if(count == 0) { 195 | last_data = tmp[tmp.size() - 1]; 196 | } else { 197 | last_data = tmp[count - 1]; 198 | } 199 | 200 | if(count == (int)(tmp.size() - 1)) { 201 | next_data = tmp[0]; 202 | } else { 203 | next_data =tmp[count + 1]; 204 | } 205 | count++; 206 | 207 | // Remove points with the opposite trend within 500mm 208 | if(n.distance < 500) { 209 | if((n.distance + 10 < last_data.distance && n.distance + 10 < next_data.distance) 210 | || (n.distance > last_data.distance + 10 && n.distance > next_data.distance + 10)) { 211 | if(n.intensity < 60) { 212 | n.intensity = 0; 213 | n.distance = 0; 214 | normal.push_back(n); 215 | continue; 216 | } 217 | } else if((n.distance + 7 < last_data.distance && n.distance + 7 < next_data.distance) 218 | || (n.distance > last_data.distance + 7 && n.distance > next_data.distance + 7)) { 219 | if(n.intensity < 45) { 220 | n.intensity = 0; 221 | n.distance = 0; 222 | normal.push_back(n); 223 | continue; 224 | } 225 | } else if((n.distance + 5 < last_data.distance && n.distance + 5 < next_data.distance) 226 | || (n.distance > last_data.distance + 5 && n.distance > next_data.distance + 5)) { 227 | if(n.intensity < 30) { 228 | n.intensity = 0; 229 | n.distance = 0; 230 | normal.push_back(n); 231 | continue; 232 | } 233 | } 234 | } 235 | 236 | // Remove points with very low intensity within 5m 237 | if (n.distance < 5000) { 238 | if(n.distance < 200) { 239 | if(n.intensity < 25) { 240 | n.intensity = 0; 241 | n.distance = 0; 242 | normal.push_back(n); 243 | continue; 244 | } 245 | } else { 246 | if(n.intensity < 10) { 247 | n.intensity = 0; 248 | n.distance = 0; 249 | normal.push_back(n); 250 | continue; 251 | } 252 | } 253 | 254 | if((n.distance + 30 < last_data.distance || n.distance > last_data.distance + 30) 255 | &&(n.distance + 30 < next_data.distance || n.distance > next_data.distance + 30)) { 256 | if((n.distance < 2000 && n.intensity < 30) || n.intensity < 20) { 257 | n.intensity = 0; 258 | n.distance = 0; 259 | normal.push_back(n); 260 | continue; 261 | } 262 | } 263 | } 264 | normal.push_back(n); 265 | } 266 | return normal; 267 | } 268 | 269 | } // namespace ldlidar 270 | 271 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 272 | * FILE ********/ -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(test) 3 | 4 | if(${CMAKE_BUILD_TYPE} MATCHES "Release") 5 | #set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -Wall") 7 | #set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -std=c++11 -O3 -Wall") 8 | message(STATUS "Mode: Release") 9 | message(STATUS "optional:-std=c++11 -Wall") 10 | else() 11 | #set(CMAKE_BUILD_TYPE "Debug") 12 | set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -Wall -Wextra -Wpedantic -g2 -ggdb") 13 | #set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -std=c++11 -O2 -Wall -Wextra -Wpedantic -g2 -ggdb") 14 | message(STATUS "Mode: Debug") 15 | message(STATUS "optional:-std=c++11 -Wall -Wextra -Wpedantic -g2 -ggdb") 16 | endif() 17 | 18 | if(${USE_WIRINGPI_LIB} MATCHES "ON") 19 | add_definitions(-D"USE_WIRINGPI") 20 | message(STATUS "-- #define USE_WIRINGPI") 21 | endif() 22 | 23 | 24 | ########### 25 | ## Build ## 26 | ########### 27 | 28 | ## Specify additional locations of header files 29 | ## Your package locations should be listed before other locations 30 | include_directories( 31 | ${CMAKE_CURRENT_SOURCE_DIR}/../include/ 32 | ) 33 | 34 | set(LDLIDAR_DRIVER_SOURCE_LINUX 35 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/ldlidar_driver.cpp 36 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/ldlidar_driver_linux.cpp 37 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/ldlidar_protocol.cpp 38 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/ldlidar_dataprocess.cpp 39 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/log_module.cpp 40 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/network_socket_interface_linux.cpp 41 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/serial_interface_linux.cpp 42 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/sl_transform.cpp 43 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/slbf.cpp 44 | ${CMAKE_CURRENT_SOURCE_DIR}/../src/tofbf.cpp 45 | ) 46 | 47 | add_executable(${PROJECT_NAME}_node 48 | ${CMAKE_CURRENT_SOURCE_DIR}/test.cpp 49 | ) 50 | 51 | add_library(ldlidar_driver_static STATIC 52 | ${LDLIDAR_DRIVER_SOURCE_LINUX} 53 | ) 54 | 55 | add_library(ldlidar_driver_shared SHARED 56 | ${LDLIDAR_DRIVER_SOURCE_LINUX} 57 | ) 58 | 59 | # rename library name 60 | set_target_properties (ldlidar_driver_static PROPERTIES OUTPUT_NAME "ldlidar_driver") 61 | set_target_properties (ldlidar_driver_shared PROPERTIES OUTPUT_NAME "ldlidar_driver") 62 | 63 | # binary file link to library 64 | if(${USE_WIRINGPI_LIB} MATCHES "ON") 65 | target_link_libraries(${PROJECT_NAME}_node 66 | ldlidar_driver_static 67 | pthread 68 | wiringPi 69 | ) 70 | else() 71 | target_link_libraries(${PROJECT_NAME}_node 72 | ldlidar_driver_static 73 | pthread 74 | ) 75 | endif() 76 | 77 | ########### 78 | ## Install ## 79 | ########### 80 | 81 | #INSTALL(TARGETS ldlidar_driver_static ldlidar_driver_shared 82 | # ARCHIVE DESTINATION lib 83 | # LIBRARY DESTINATION share 84 | #) 85 | 86 | #INSTALL(DIRECTORY ${PROJECT_SOURCE_DIR}/ldlidar_driver/include 87 | # DESTINATION include 88 | #) 89 | -------------------------------------------------------------------------------- /test/auto_build_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #Author: David Hu 3 | #Date: 2022-09 4 | 5 | # Exit on error 6 | set -e 7 | 8 | echo "Start cmake build" 9 | 10 | if [ ! -e "./build" ];then 11 | mkdir build 12 | echo "create ./build/" 13 | fi 14 | 15 | cd ./build 16 | 17 | cmake -DCMAKE_BUILD_TYPE=Debug .. 18 | 19 | make -j2 20 | 21 | echo "build finished." -------------------------------------------------------------------------------- /test/clean_build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #Author: David Hu 3 | #Date: 2022-09 4 | if [ -e "./build" ];then 5 | rm -rf build/ 6 | echo "del ./build/" 7 | fi 8 | 9 | if [ -e "./install" ];then 10 | rm -rf install/ 11 | echo "del ./install/" 12 | fi 13 | 14 | echo "del is ok....." -------------------------------------------------------------------------------- /test/test.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file test.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * David Hu (hmd_hubei_cn@163.com) 5 | * @brief sdk function test 6 | * @version 3.0 7 | * @date 2022-12-09 8 | * 9 | * @copyright Copyright (c) 2020 SHENZHEN LDROBOT CO., LTD. All rights 10 | * reserved. 11 | * Licensed under the MIT License (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License in the file LICENSE 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #include "ldlidar_driver/ldlidar_driver_linux.h" 22 | 23 | #ifdef USE_WIRINGPI 24 | #include 25 | #endif 26 | 27 | uint64_t GetTimestamp(void) { 28 | std::chrono::time_point tp = 29 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 30 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 31 | return ((uint64_t)tmp.count()); 32 | } 33 | 34 | bool ControlPinInit(void) { 35 | // raspberrypi wiringPi GPIO Init 36 | #ifdef USE_WIRINGPI 37 | if(wiringPiSetup() == -1) { 38 | return false; 39 | } 40 | pinMode(25,OUTPUT); // set wiringPi 25 Pin number is outuput Mode. 41 | #endif 42 | return true; 43 | } 44 | 45 | void LidarPowerOn(void) { 46 | LOG_DEBUG("Lidar Power On",""); 47 | #ifdef USE_WIRINGPI 48 | digitalWrite(25,HIGH); 49 | #endif 50 | } 51 | 52 | void LidarPowerOff(void) { 53 | LOG_DEBUG("Lidar Power Off",""); 54 | #ifdef USE_WIRINGPI 55 | digitalWrite(25,LOW); 56 | #endif 57 | } 58 | 59 | void AbortTesting(void) { 60 | LOG_WARN("Testing abort",""); 61 | LidarPowerOff(); 62 | exit(EXIT_FAILURE); 63 | } 64 | 65 | struct LdsInfoStruct { 66 | std::string ldtype_str; 67 | ldlidar::LDType ldtype_enum; 68 | uint32_t baudrate; 69 | }; 70 | 71 | LdsInfoStruct LdsInfoArrary[4] = { 72 | {"LD14", ldlidar::LDType::LD_14, 115200}, 73 | {"LD14P", ldlidar::LDType::LD_14P, 230400}, 74 | {"LD06", ldlidar::LDType::LD_06, 230400}, 75 | {"LD19", ldlidar::LDType::LD_19, 230400}, 76 | }; 77 | 78 | ldlidar::LDType GetLdsType(std::string in_str) { 79 | for (auto item : LdsInfoArrary) { 80 | if (!strcmp(in_str.c_str(), item.ldtype_str.c_str())) { 81 | return item.ldtype_enum; 82 | } 83 | } 84 | return ldlidar::LDType::NO_VER; 85 | } 86 | 87 | uint32_t GetLdsSerialPortBaudrateValue(std::string in_str) { 88 | for (auto item : LdsInfoArrary) { 89 | if (!strcmp(in_str.c_str(), item.ldtype_str.c_str())) { 90 | return item.baudrate; 91 | } 92 | } 93 | return 0; 94 | } 95 | 96 | int test(int argc, char **argv) { 97 | 98 | if (argc != 3) { 99 | LOG_INFO("cmd error",""); 100 | LOG_INFO("please input: ./test_node ",""); 101 | LOG_INFO("example:",""); 102 | LOG_INFO("./test_node LD14 /dev/ttyUSB0",""); 103 | LOG_INFO("./test_node LD14P /dev/ttyUSB0",""); 104 | LOG_INFO("./test_node LD06 /dev/ttyUSB0",""); 105 | LOG_INFO("./test_node LD19 /dev/ttyUSB0",""); 106 | exit(EXIT_FAILURE); 107 | } 108 | 109 | std::string ldlidar_type_str(argv[1]); 110 | std::string serial_port_name(argv[2]); 111 | 112 | // select ldrobot lidar sensor type. 113 | ldlidar::LDType ldlidar_type_dest; 114 | ldlidar_type_dest = GetLdsType(ldlidar_type_str); 115 | if (ldlidar_type_dest == ldlidar::LDType::NO_VER) { 116 | LOG_WARN("ldlidar_type_str value is not sure: %s", ldlidar_type_str.c_str()); 117 | exit(EXIT_FAILURE); 118 | } 119 | 120 | // if use serial communications interface, as select serial baudrate paramters. 121 | uint32_t serial_baudrate_val; 122 | serial_baudrate_val = GetLdsSerialPortBaudrateValue(ldlidar_type_str); 123 | if (!serial_baudrate_val) { 124 | LOG_WARN("ldlidar_type_str value is not sure: %s", ldlidar_type_str.c_str()); 125 | exit(EXIT_FAILURE); 126 | } 127 | 128 | ldlidar::LDLidarDriverLinuxInterface* lidar_drv = 129 | ldlidar::LDLidarDriverLinuxInterface::Create(); 130 | 131 | LOG_INFO("LDLiDAR SDK Pack Version is %s", lidar_drv->GetLidarSdkVersionNumber().c_str()); 132 | 133 | lidar_drv->RegisterGetTimestampFunctional(std::bind(&GetTimestamp)); 134 | 135 | lidar_drv->EnablePointCloudDataFilter(true); 136 | 137 | if (lidar_drv->Connect(ldlidar_type_dest, serial_port_name, serial_baudrate_val)) { 138 | LOG_INFO("ldlidar serial connect is success",""); 139 | // LidarPowerOn(); 140 | } else { 141 | LOG_ERROR("ldlidar serial connect is fail",""); 142 | AbortTesting(); 143 | } 144 | 145 | if (lidar_drv->WaitLidarComm(3500)) { 146 | LOG_INFO("ldlidar communication is normal.",""); 147 | } else { 148 | LOG_ERROR("ldlidar communication is abnormal.",""); 149 | lidar_drv->Disconnect(); 150 | AbortTesting(); 151 | } 152 | 153 | if (lidar_drv->Start()) { 154 | LOG_INFO_LITE("ldlidar driver start is success.",""); 155 | } else { 156 | LOG_ERROR_LITE("ldlidar driver start is fail.",""); 157 | } 158 | 159 | ldlidar::Points2D laser_scan_points; 160 | int cnt = 100; 161 | while (ldlidar::LDLidarDriverLinuxInterface::Ok()) { 162 | if ((cnt--) <= 0) { 163 | lidar_drv->Stop(); 164 | } 165 | 166 | switch (lidar_drv->GetLaserScanData(laser_scan_points, 1500)){ 167 | case ldlidar::LidarStatus::NORMAL: { 168 | double lidar_scan_freq = 0; 169 | lidar_drv->GetLidarScanFreq(lidar_scan_freq); 170 | #ifdef __LP64__ 171 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%lu, stamp_end:%lu", 172 | lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp); 173 | #else 174 | LOG_INFO_LITE("speed(Hz):%f, size:%d,stamp_begin:%llu, stamp_end:%llu", 175 | lidar_scan_freq, laser_scan_points.size(), laser_scan_points.front().stamp, laser_scan_points.back().stamp); 176 | #endif 177 | // output 2d point cloud data 178 | #if 0 179 | for (auto point : laser_scan_points) { 180 | #ifdef __LP64__ 181 | LOG_INFO_LITE("stamp(ns):%lu,angle:%f,distance(mm):%d,intensity:%d", 182 | point.stamp, point.angle, point.distance, point.intensity); 183 | #else 184 | LOG_INFO_LITE("stamp(ns):%llu,angle:%f,distance(mm):%d,intensity:%d", 185 | point.stamp, point.angle, point.distance, point.intensity); 186 | #endif 187 | } 188 | #endif 189 | break; 190 | } 191 | case ldlidar::LidarStatus::DATA_TIME_OUT: { 192 | LOG_ERROR_LITE("point cloud data publish time out, please check your lidar device.",""); 193 | lidar_drv->Stop(); 194 | AbortTesting(); 195 | break; 196 | } 197 | case ldlidar::LidarStatus::DATA_WAIT: { 198 | break; 199 | } 200 | default: 201 | break; 202 | } 203 | 204 | usleep(1000*166); // sleep 166ms , 6hz 205 | } 206 | 207 | lidar_drv->Stop(); 208 | lidar_drv->Disconnect(); 209 | sleep(3); 210 | 211 | ldlidar::LDLidarDriverLinuxInterface::Destory(lidar_drv); 212 | 213 | return 0; 214 | } 215 | 216 | int main(int argc, char** argv) { 217 | 218 | if (!ControlPinInit()) { 219 | LOG_ERROR("Control pin Setup failed.",""); 220 | exit(EXIT_FAILURE); 221 | } 222 | 223 | for (int i = 0; i < 10000; i++) { 224 | test(argc, argv); 225 | } 226 | 227 | LOG_INFO("test is end.",""); 228 | while (1) { 229 | sleep(1); 230 | } 231 | 232 | return 0; 233 | } 234 | 235 | 236 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 237 | * FILE ********/ 238 | --------------------------------------------------------------------------------