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