├── CONTRIBUTING.md ├── .github └── workflows │ ├── ros-lint.yml │ └── ros-ci.yml ├── CHANGELOG.rst ├── package.xml ├── launch └── ld08.launch.py ├── include ├── transform.hpp ├── slbf.hpp ├── pointdata.hpp ├── cmd_interface_linux.hpp └── lipkg.hpp ├── CMakeLists.txt ├── README.md ├── src ├── main.cpp ├── transform.cpp ├── slbf.cpp ├── cmd_interface_linux.cpp └── lipkg.cpp └── LICENSE /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /.github/workflows/ros-lint.yml: -------------------------------------------------------------------------------- 1 | # The name of the workflow 2 | name: Lint 3 | 4 | # Specifies the events that trigger the workflow 5 | on: 6 | pull_request: 7 | 8 | # Defines a set of jobs to be run as part of the workflow 9 | jobs: 10 | ament_lint: 11 | runs-on: ubuntu-latest 12 | container: 13 | image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | linter: [cppcheck, cpplint, uncrustify, flake8, pep257, lint_cmake, xmllint, copyright] 18 | steps: 19 | - name: Checkout code 20 | uses: actions/checkout@v4 21 | 22 | - name: Setup ROS environment 23 | uses: ros-tooling/setup-ros@v0.7 24 | 25 | - name: Run Linter 26 | env: 27 | AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: 1 28 | uses: ros-tooling/action-ros-lint@master 29 | with: 30 | linter: ${{ matrix.linter }} 31 | distribution: rolling 32 | package-name: "*" 33 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ld08_driver 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.4 (2025-05-28) 6 | ------------------ 7 | * Deprecate ament_include_dependency usage in CMakeLists.txt 8 | * Contributor: Hyungyu Kim 9 | 10 | 1.1.3 (2025-04-11) 11 | ------------------ 12 | * Support flexible configuration of the frame_id used when publishing the scan topic 13 | * Reduce CPU usage of the ld08 LiDAR node by adding sleep to its main loop, related PR(https://github.com/ROBOTIS-GIT/ld08_driver/pull/23) 14 | * Contributors: Hyungyu Kim, Xander Soldaat 15 | 16 | 1.1.2 (2025-04-02) 17 | ------------------ 18 | * Removed the unused parameter from the constructor of the SlTransform class 19 | * Contributors: Hyungyu Kim 20 | 21 | 1.1.1 (2025-03-27) 22 | ------------------ 23 | * Added the boost and libudev-dev dependency for release 24 | * Modified the CI and lint 25 | * Contributors: Pyo, Hyungyu Kim 26 | 27 | 1.1.0 (2025-02-25) 28 | ------------------ 29 | * ROS 2 Humble Hawksbill supported 30 | * Added Lint and CI 31 | * Contributors: Hye-Jong Kim, Hyungyu Kim 32 | 33 | 1.0.0 (2022-02-04) 34 | ------------------ 35 | * First release 36 | * Contributors: Will Son 37 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ld08_driver 5 | 1.1.4 6 | 7 | ROS package for LDS-02(LD08) Lidar. 8 | The Lidar sensor sends data to the Host controller for the Simultaneous Localization And Mapping(SLAM). 9 | 10 | Pyo 11 | Apache 2.0 12 | http://wiki.ros.org/ld08_driver 13 | https://github.com/ROBOTIS-GIT/ld08_driver 14 | https://github.com/ROBOTIS-GIT/ld08_driver/issues 15 | LD Robot 16 | Weyne 17 | Will Son 18 | Hye-Jong KIM 19 | ament_cmake 20 | boost 21 | sensor_msgs 22 | libudev-dev 23 | rclcpp 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /launch/ld08.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright 2021 ROBOTIS CO., LTD. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # Authors: Will Son 18 | 19 | 20 | from launch import LaunchDescription 21 | from launch.substitutions import LaunchConfiguration 22 | from launch_ros.actions import Node 23 | 24 | 25 | def generate_launch_description(): 26 | 27 | frame_id = LaunchConfiguration('frame_id', default='base_scan') 28 | namespace = LaunchConfiguration('namespace', default='') 29 | 30 | return LaunchDescription([ 31 | Node( 32 | package='ld08_driver', 33 | executable='ld08_driver', 34 | name='ld08_driver', 35 | output='screen', 36 | parameters=[ 37 | {'frame_id': frame_id}, 38 | {'namespace': namespace}, 39 | ]), 40 | ]) 41 | -------------------------------------------------------------------------------- /include/transform.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #ifndef TRANSFORM_HPP_ 18 | #define TRANSFORM_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include "../include/lipkg.hpp" 24 | 25 | enum class LDVersion 26 | { 27 | LD_ZERO, /*Zero generation lidar*/ 28 | LD_THREE, /*Third generation lidar*/ 29 | LD_EIGHT, /*Eight generation radar*/ 30 | LD_NINE, /*Nine generation radar*/ 31 | }; 32 | 33 | class SlTransform 34 | { 35 | private: 36 | bool to_right_hand = true; 37 | double offset_x; 38 | double offset_y; 39 | 40 | public: 41 | explicit SlTransform(LDVersion version); 42 | Points2D Transform(const Points2D & data); 43 | ~SlTransform(); 44 | }; 45 | 46 | 47 | #endif // TRANSFORM_HPP_ 48 | -------------------------------------------------------------------------------- /include/slbf.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #ifndef SLBF_HPP_ 18 | #define SLBF_HPP_ 19 | 20 | #include 21 | #include 22 | #include "../include/pointdata.hpp" 23 | 24 | class Slbf 25 | { 26 | private: 27 | const int CONFIDENCE_LOW = 92; 28 | 29 | // Default scanning frequency, which can be changed according to radar protocol 30 | const int SCAN_FRE = 2300; 31 | double curr_speed; 32 | 33 | // whether strict filtering is enabled within 300 mm, 34 | // the effective value may be lost, 35 | // and the time sequence of recharging needs to be disabled 36 | bool enable_strict_policy; 37 | Slbf() = delete; 38 | Slbf(const Slbf &) = delete; 39 | Slbf & operator=(const Slbf &) = delete; 40 | 41 | public: 42 | explicit Slbf(int speed, bool strict_policy = true); 43 | Points2D NearFilter(const Points2D & tmp) const; 44 | void EnableStrictPolicy(bool enable); 45 | ~Slbf(); 46 | }; 47 | 48 | #endif // SLBF_HPP_ 49 | -------------------------------------------------------------------------------- /include/pointdata.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #ifndef POINTDATA_HPP_ 18 | #define POINTDATA_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | struct PointData 25 | { 26 | // Polar coordinate representation 27 | float angle; 28 | uint16_t distance; 29 | uint8_t confidence; 30 | 31 | // Rectangular coordinate representation 32 | double x; 33 | double y; 34 | 35 | PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0) 36 | { 37 | this->angle = angle; 38 | this->distance = distance; 39 | this->confidence = confidence; 40 | this->x = x; 41 | this->y = y; 42 | } 43 | 44 | PointData() 45 | { 46 | } 47 | 48 | friend std::ostream & operator<<(std::ostream & os, const PointData & data) 49 | { 50 | os << data.angle << " " \ 51 | << data.distance << " " \ 52 | << static_cast(data.confidence) << " " \ 53 | << data.x << " " \ 54 | << data.y; 55 | return os; 56 | } 57 | }; 58 | 59 | 60 | typedef std::vector Points2D; 61 | 62 | #endif // POINTDATA_HPP_ 63 | -------------------------------------------------------------------------------- /include/cmd_interface_linux.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #ifndef CMD_INTERFACE_LINUX_HPP_ 18 | #define CMD_INTERFACE_LINUX_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | class CmdInterfaceLinux 31 | { 32 | public: 33 | explicit CmdInterfaceLinux(int32_t ver); 34 | ~CmdInterfaceLinux(); 35 | 36 | bool Open(std::string & port_name); 37 | bool Close(); 38 | bool ReadFromIO(uint8_t * rx_buf, uint32_t rx_buf_len, uint32_t * rx_len); 39 | bool WriteToIo(const uint8_t * tx_buf, uint32_t tx_buf_len, uint32_t * tx_len); 40 | bool GetCmdDevices(std::vector> & device_list); 41 | void SetReadCallback(std::function callback) 42 | { 43 | mReadCallback = callback; 44 | } 45 | bool IsOpened() 46 | { 47 | return mIsCmdOpened.load(); 48 | } 49 | 50 | private: 51 | std::thread * mRxThread; 52 | int64_t mRxCount; 53 | std::function mReadCallback; 54 | int32_t version; 55 | static void mRxThreadProc(void * param); 56 | int32_t mComHandle; 57 | std::atomic mIsCmdOpened, mRxThreadExitFlag; 58 | }; 59 | 60 | #endif // CMD_INTERFACE_LINUX_HPP_ 61 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 3.5) 5 | project(ld08_driver) 6 | 7 | if(NOT CMAKE_CXX_STANDARD) 8 | set(CMAKE_CXX_STANDARD 17) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | if(MSVC) 16 | add_compile_definitions( 17 | _USE_MATH_DEFINES 18 | ) 19 | endif() 20 | 21 | ################################################################################ 22 | # Find ament packages and libraries for ament and system dependencies 23 | ################################################################################ 24 | find_package(ament_cmake REQUIRED) 25 | find_package(rclcpp REQUIRED) 26 | find_package(sensor_msgs REQUIRED) 27 | find_package(Boost REQUIRED system) 28 | 29 | ################################################################################ 30 | # Build 31 | ################################################################################ 32 | include_directories( 33 | include 34 | ) 35 | 36 | set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}") 37 | 38 | file(GLOB MAIN_SRC src/*.cpp) 39 | 40 | add_executable(ld08_driver ${MAIN_SRC}) 41 | target_link_libraries(ld08_driver 42 | ${sensor_msgs_TARGETS} 43 | ${std_msgs_TARGETS} 44 | Boost::system 45 | pthread 46 | rclcpp::rclcpp 47 | udev 48 | ) 49 | 50 | 51 | ################################################################################ 52 | # Install 53 | ################################################################################ 54 | install(DIRECTORY launch 55 | DESTINATION share/${PROJECT_NAME} 56 | ) 57 | 58 | install( 59 | TARGETS ld08_driver 60 | RUNTIME DESTINATION lib/${PROJECT_NAME} 61 | ) 62 | 63 | ################################################################################ 64 | # Macro for ament package 65 | ################################################################################ 66 | ament_export_dependencies( 67 | rclcpp 68 | std_msgs 69 | sensor_msgs 70 | Boost::system 71 | pthread 72 | udev 73 | ) 74 | ament_export_include_directories(include) 75 | ament_package() 76 | -------------------------------------------------------------------------------- /.github/workflows/ros-ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | # Controls when the action will run. Triggers the workflow on push or pull request 4 | on: 5 | push: 6 | branches: [ main, humble, jazzy] 7 | pull_request: 8 | branches: [ main, humble, jazzy] 9 | 10 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 11 | jobs: 12 | ROS_CI: 13 | runs-on: ubuntu-22.04 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | ros_distribution: 18 | - humble 19 | - jazzy 20 | - rolling 21 | include: 22 | # ROS 2 Humble Hawksbill 23 | - docker_image: ubuntu:jammy 24 | ros_distribution: humble 25 | ros_version: 2 26 | # ROS 2 Jazzy Jalisco 27 | - docker_image: ubuntu:noble 28 | ros_distribution: jazzy 29 | ros_version: 2 30 | # ROS 2 Rolling Ridley 31 | - docker_image: ubuntu:noble 32 | ros_distribution: rolling 33 | ros_version: 2 34 | container: 35 | image: ${{ matrix.docker_image }} 36 | steps: 37 | - name: Setup directories 38 | run: mkdir -p ros_ws/src 39 | 40 | - name: checkout 41 | uses: actions/checkout@v4 42 | with: 43 | path: ros_ws/src 44 | - name: Setup ROS environment 45 | uses: ros-tooling/setup-ros@v0.7 46 | with: 47 | required-ros-distributions: ${{ matrix.ros_distribution }} 48 | 49 | - name: Check and Install ROS dependencies 50 | shell: bash 51 | run: | 52 | set -e 53 | source /opt/ros/${{ matrix.ros_distribution }}/setup.bash 54 | echo "--- Updating rosdep definitions ---" 55 | rosdep update 56 | echo "--- Installing system dependencies for ROS 2 ${{ matrix.ros_distribution }} ---" 57 | rosdep install --from-paths ros_ws/src --ignore-src -y -r --rosdistro ${{ matrix.ros_distribution }} 58 | echo "--- Performing rosdep check for ROS 2 ${{ matrix.ros_distribution }} ---" 59 | if rosdep check --from-paths ros_ws/src --ignore-src --rosdistro ${{ matrix.ros_distribution }}; then 60 | echo "--- rosdep check passed ---" 61 | else 62 | echo "--- rosdep check failed: Missing system dependencies or unresolvable keys. ---" 63 | exit 1 64 | fi 65 | 66 | - name: Build and Test 67 | uses: ros-tooling/action-ros-ci@v0.3 68 | with: 69 | target-ros2-distro: ${{ matrix.ros_distribution }} 70 | vcs-repo-file-url: "" 71 | package-name: ld08_driver 72 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LDS-02 (LD08) 2 | 3 | 4 | Since 2022, the LDS-02 (LD08) replaces the previous LDS-01 sensor. 5 | 6 | - Active Branches: noetic, humble, jazzy, main 7 | - Legacy Branches: *-devel 8 | 9 | ## Open Source Projects Related to TurtleBot3 10 | - [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) 11 | - [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) 12 | - [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) 13 | - [turtlebot3_manipulation](https://github.com/ROBOTIS-GIT/turtlebot3_manipulation) 14 | - [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) 15 | - [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) 16 | - [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) 17 | - [turtlebot3_machine_learning](https://github.com/ROBOTIS-GIT/turtlebot3_machine_learning) 18 | - [turtlebot3_home_service_challenge](https://github.com/ROBOTIS-GIT/turtlebot3_home_service_challenge) 19 | - [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) 20 | - [ld08_driver](https://github.com/ROBOTIS-GIT/ld08_driver) 21 | - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) 22 | - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) 23 | - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) 24 | - [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) 25 | 26 | ## Documentation, Videos, and Community 27 | 28 | ### Official Documentation 29 | - ⚙️ **[ROBOTIS DYNAMIXEL](https://dynamixel.com/)** 30 | - 📚 **[ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/)** 31 | - 📚 **[ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/)** 32 | - 📚 **[ROBOTIS e-Manual for OpenMANIPULATOR-X](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/)** 33 | 34 | ### Learning Resources 35 | - 🎥 **[ROBOTIS YouTube Channel](https://www.youtube.com/@ROBOTISCHANNEL)** 36 | - 🎥 **[ROBOTIS Open Source YouTube Channel](https://www.youtube.com/@ROBOTISOpenSourceTeam)** 37 | - 🎥 **[ROBOTIS TurtleBot3 YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU)** 38 | - 🎥 **[ROBOTIS OpenMANIPULATOR YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb)** 39 | 40 | ### Community & Support 41 | - 💬 **[ROBOTIS Community Forum](https://forum.robotis.com/)** 42 | - 💬 **[TurtleBot category from ROS Community](https://discourse.ros.org/c/turtlebot/)** 43 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "../include/cmd_interface_linux.hpp" 24 | #include "../include/lipkg.hpp" 25 | #include "../include/transform.hpp" 26 | 27 | 28 | int main(int argc, char ** argv) 29 | { 30 | rclcpp::init(argc, argv); 31 | auto node = rclcpp::Node::make_shared("laser_scan_publisher"); 32 | rclcpp::Publisher::SharedPtr lidar_pub; 33 | rclcpp::Rate loop_rate(100.0); 34 | 35 | node->declare_parameter("frame_id", "base_scan"); 36 | node->declare_parameter("namespace", ""); 37 | std::string frame_id = node->get_parameter("frame_id").as_string(); 38 | std::string name_space = node->get_parameter("namespace").as_string(); 39 | if (name_space != "") { 40 | frame_id = name_space + "/" + frame_id; 41 | } 42 | 43 | LiPkg * pkg; 44 | std::string product; 45 | int32_t ver = 8; 46 | pkg = new LD08_LiPkg; 47 | 48 | CmdInterfaceLinux cmd_port(ver); 49 | std::vector> device_list; 50 | std::string port_name; 51 | cmd_port.GetCmdDevices(device_list); 52 | for (auto n : device_list) { 53 | std::cout << n.first << " " << n.second << std::endl; 54 | if (strstr(n.second.c_str(), "CP2102")) { 55 | port_name = n.first; 56 | } 57 | } 58 | 59 | if (port_name.empty() == false) { 60 | std::cout << "FOUND LDS-02" << product << std::endl; 61 | cmd_port.SetReadCallback( 62 | [&pkg](const char * byte, size_t len) { 63 | if (pkg->Parse((const uint8_t *)(byte), len)) { 64 | pkg->AssemblePacket(); 65 | } 66 | }); 67 | 68 | if (cmd_port.Open(port_name)) { 69 | std::cout << "LDS-02" << product << " started successfully " << std::endl; 70 | } 71 | 72 | // char topic_name[20]={0}; 73 | // strcat(topic_name,product.c_str()); 74 | // strcat(topic_name,"/LDLiDAR"); 75 | lidar_pub = node->create_publisher( 76 | "scan", rclcpp::QoS(rclcpp::SensorDataQoS()) 77 | ); 78 | 79 | while (rclcpp::ok()) { 80 | if (pkg->IsFrameReady()) { 81 | pkg->setStamp(node->now()); 82 | pkg->setFrameId(frame_id); 83 | lidar_pub->publish(pkg->GetLaserScan()); 84 | pkg->ResetFrameReady(); 85 | } 86 | loop_rate.sleep(); 87 | } 88 | } else { 89 | std::cout << "Can't find LDS-02" << product << std::endl; 90 | } 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /src/transform.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #include "../include/transform.hpp" 18 | #include 19 | 20 | /*! 21 | \brief transfer the origin to the center of lidar circle 22 | \param[in] 23 | \arg version lidar version, different versions have different parameter settings 24 | \arg data lidar raw data 25 | \arg to_right_hand a flag whether convert to right-hand coordinate system 26 | 27 | \param[out] data 28 | \retval Data after coordinate conversion 29 | */ 30 | SlTransform::SlTransform(LDVersion version) 31 | { 32 | switch (version) { 33 | case LDVersion::LD_ZERO: 34 | case LDVersion::LD_NINE: 35 | offset_x = 8.1; 36 | offset_y = -22.5156; 37 | break; 38 | 39 | case LDVersion::LD_THREE: 40 | case LDVersion::LD_EIGHT: 41 | offset_x = 5.9; 42 | offset_y = -20.14; 43 | break; 44 | 45 | default: 46 | break; 47 | } 48 | } 49 | 50 | Points2D SlTransform::Transform(const Points2D & data) 51 | { 52 | Points2D tmp2; 53 | for (auto n : data) { 54 | /*Filter out invalid data*/ 55 | if (n.distance == 0) { 56 | continue; 57 | } 58 | /*transfer the origin to the center of lidar circle*/ 59 | /*The default direction of radar rotation is clockwise*/ 60 | /*transfer to the right-hand coordinate system*/ 61 | float right_hand = (360.f - n.angle); 62 | double x = n.distance + offset_x; 63 | double y = n.distance * 0.11923 + offset_y; 64 | double d = sqrt(x * x + y * y); 65 | double shift = atan(y / x) * 180.f / 3.14159; 66 | /*Choose whether to use the right-hand system according to the flag*/ 67 | double angle; 68 | if (to_right_hand) { 69 | angle = right_hand + shift; 70 | } else { 71 | angle = n.angle - shift; 72 | } 73 | 74 | if (angle > 360) { 75 | angle -= 360; 76 | } 77 | if (angle < 0) { 78 | angle += 360; 79 | } 80 | 81 | tmp2.push_back(PointData(angle, d, n.confidence, x, y)); 82 | } 83 | 84 | return tmp2; 85 | } 86 | 87 | SlTransform::~SlTransform() 88 | { 89 | } 90 | 91 | 92 | /* 93 | 1.Internal conversion of lidar data, transfer the origin to the center of lidar circle, and transfer to the right-hand coordinate system 94 | 2.TODO : Transfer to machine center (this part needs to be completed by customers according to their own machine structure) 95 | 3.TODO : Real time calibration of rotation and translation deviation through IMU data (this part needs to be completed by customers according to their own equipment) 96 | */ 97 | void LD00_LiPkg::Transform(std::vector & tmp) 98 | { 99 | SlTransform trans(LDVersion::LD_ZERO); 100 | tmp = trans.Transform(tmp); 101 | } 102 | 103 | void LD03_LiPkg::Transform(std::vector & tmp) 104 | { 105 | SlTransform trans(LDVersion::LD_THREE); 106 | tmp = trans.Transform(tmp); 107 | } 108 | 109 | void LD08_LiPkg::Transform(std::vector & tmp) 110 | { 111 | SlTransform trans(LDVersion::LD_EIGHT); 112 | tmp = trans.Transform(tmp); 113 | // std::cout << "Transform LD_EIGHT!!" << std::endl; 114 | } 115 | 116 | void LD09_LiPkg::Transform(std::vector & tmp) 117 | { 118 | SlTransform trans(LDVersion::LD_NINE); 119 | tmp = trans.Transform(tmp); 120 | } 121 | -------------------------------------------------------------------------------- /include/lipkg.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #ifndef LIPKG_HPP_ 18 | #define LIPKG_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include "../include/pointdata.hpp" 30 | 31 | #define ANGLE_TO_RADIAN(angle) ((angle) * 3141.59 / 180000) 32 | #define RADIAN_TO_ANGLE(angle) ((angle) * 180000 / 3141.59) 33 | 34 | enum 35 | { 36 | PKG_HEADER = 0x54, 37 | PKG_VER_LEN = 0x2C, 38 | POINT_PER_PACK = 12, 39 | }; 40 | 41 | 42 | typedef struct __attribute__((packed)) 43 | { 44 | uint16_t distance; 45 | uint8_t confidence; 46 | } LidarPointStructDef; 47 | 48 | typedef struct __attribute__((packed)) 49 | { 50 | uint8_t header; 51 | uint8_t ver_len; 52 | uint16_t speed; 53 | uint16_t start_angle; 54 | LidarPointStructDef point[POINT_PER_PACK]; 55 | uint16_t end_angle; 56 | uint16_t timestamp; 57 | uint8_t crc8; 58 | } LiDARFrameTypeDef; 59 | 60 | 61 | typedef std::vector Points2D; 62 | 63 | struct FrameData 64 | { 65 | float angle_min; 66 | float angle_max; 67 | uint32_t len; 68 | std::vector distance; 69 | std::vector intensities; 70 | }; 71 | 72 | 73 | class LiPkg 74 | { 75 | public: 76 | LiPkg(); 77 | // Lidar spin speed (Hz) 78 | double GetSpeed(void); 79 | // time stamp of the packet 80 | uint16_t GetTimestamp(void) {return mTimestamp;} 81 | // a packet is ready 82 | bool IsPkgReady(void) {return mIsPkgReady;} 83 | // Lidar data frame is ready 84 | bool IsFrameReady(void) {return mIsFrameReady;} 85 | void ResetFrameReady(void) {mIsFrameReady = false;} 86 | // the number of errors in parser process of lidar data frame 87 | int32_t GetErrorTimes(void) {return mErrorTimes;} 88 | // original data package 89 | const std::array & GetPkgData(void); 90 | // parse single packet 91 | bool Parse(const uint8_t * data, int32_t len); 92 | // transform raw data to stantard data 93 | virtual void Transform(std::vector & tmp) = 0; 94 | // combine stantard data into data frames and calibrate 95 | bool AssemblePacket(); 96 | const FrameData & GetFrameData(void) {mIsFrameReady = false; return mFrameData;} 97 | sensor_msgs::msg::LaserScan GetLaserScan() {return output;} 98 | void setStamp(rclcpp::Time timeStamp) {output.header.stamp = timeStamp;} 99 | void setFrameId(std::string frame_id) {output.header.frame_id = frame_id;} 100 | 101 | private: 102 | uint16_t mTimestamp; 103 | double mSpeed; 104 | int32_t mErrorTimes; 105 | bool mIsFrameReady; 106 | bool mIsPkgReady; 107 | std::vector mDataTmp; 108 | std::array mOnePkg; 109 | std::vector mFrameTemp; 110 | FrameData mFrameData; 111 | sensor_msgs::msg::LaserScan output; 112 | void ToLaserscan(std::vector src); 113 | }; 114 | 115 | class LD00_LiPkg : public LiPkg 116 | { 117 | public: 118 | virtual void Transform(std::vector & tmp); 119 | }; 120 | 121 | class LD03_LiPkg : public LiPkg 122 | { 123 | public: 124 | virtual void Transform(std::vector & tmp); 125 | }; 126 | 127 | class LD08_LiPkg : public LiPkg 128 | { 129 | public: 130 | virtual void Transform(std::vector & tmp); 131 | }; 132 | 133 | class LD09_LiPkg : public LiPkg 134 | { 135 | public: 136 | virtual void Transform(std::vector & tmp); 137 | }; 138 | 139 | #endif // LIPKG_HPP_ 140 | -------------------------------------------------------------------------------- /src/slbf.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #include 18 | #include 19 | #include 20 | #include "../include/lipkg.hpp" 21 | #include "../include/slbf.hpp" 22 | 23 | /* 24 | \brief Set current speed 25 | \param[in] 26 | \arg speed Current lidar speed 27 | \arg strict_policy The flag to enable very strict filtering 28 | \param[out] none 29 | \retval none 30 | */ 31 | Slbf::Slbf(int speed, bool strict_policy) 32 | { 33 | curr_speed = speed; 34 | enable_strict_policy = strict_policy; 35 | } 36 | 37 | Slbf::~Slbf() 38 | { 39 | } 40 | 41 | /* 42 | \brief Filter within 1m to filter out unreasonable data points 43 | \param[in] 44 | \arg data A circle of lidar data packed 45 | \param[out] none 46 | \retval Standard data 47 | */ 48 | Points2D Slbf::NearFilter(const Points2D & data) const 49 | { 50 | Points2D normal, pending, item; 51 | std::vector group; 52 | int sunshine_amount = 0; 53 | 54 | for (auto n : data) { 55 | if (n.distance < 1000) { 56 | pending.push_back(n); 57 | } else { 58 | normal.push_back(n); 59 | } 60 | } 61 | 62 | if (data.empty()) { 63 | return normal; 64 | } 65 | 66 | double angle_delta_up_limit = curr_speed / SCAN_FRE * 1.5; 67 | double angle_delta_down_limit = curr_speed / SCAN_FRE - 0.2; 68 | 69 | std::sort( 70 | pending.begin(), pending.end(), [](PointData a, PointData b) {return a.angle < b.angle;} 71 | ); 72 | 73 | PointData last(-10, 0, 0); 74 | 75 | for (auto n : pending) { 76 | if (abs(n.angle - last.angle) > angle_delta_up_limit || 77 | abs(n.distance - last.distance) > 50) 78 | { 79 | if (item.empty() == false) { 80 | group.push_back(item); 81 | item.clear(); 82 | } 83 | } 84 | item.push_back(n); 85 | last = n; 86 | } 87 | 88 | if (item.empty() == false) { 89 | group.push_back(item); 90 | } 91 | 92 | if (group.empty()) { 93 | return normal; 94 | } 95 | 96 | 97 | auto first_item = group.front().front(); 98 | auto last_item = group.back().back(); 99 | if (abs(first_item.angle + 360.f - last_item.angle) < angle_delta_up_limit && 100 | abs(first_item.distance - last_item.distance) < 50) 101 | { 102 | if (group.size() > 1) { 103 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 104 | group.erase(group.end() - 1); 105 | } 106 | } 107 | 108 | for (auto n : group) { 109 | if (n.size() == 0) { 110 | continue; 111 | } 112 | 113 | if (n.size() > 15) { 114 | normal.insert(normal.end(), n.begin(), n.end()); 115 | continue; 116 | } 117 | 118 | 119 | for (auto m : n) { 120 | int flag = m.confidence & 0x01; 121 | sunshine_amount += (flag == 1); 122 | } 123 | 124 | double sunshine_rate = static_cast(sunshine_amount) / static_cast(n.size()); 125 | 126 | double confidence_avg = 0; 127 | double dis_avg = 0; 128 | for (auto m : n) { 129 | confidence_avg += m.confidence; 130 | dis_avg += m.distance; 131 | } 132 | confidence_avg /= n.size(); 133 | dis_avg /= n.size(); 134 | 135 | if (sunshine_rate < 0.2 && confidence_avg > CONFIDENCE_LOW) { 136 | normal.insert(normal.end(), n.begin(), n.end()); 137 | continue; 138 | } 139 | 140 | if (sunshine_rate > 0.5 && confidence_avg < CONFIDENCE_LOW) { 141 | continue; 142 | } 143 | 144 | if (enable_strict_policy) { 145 | if (dis_avg < 300 && confidence_avg < CONFIDENCE_LOW && n.size() < 5) { 146 | continue; 147 | } 148 | 149 | if (dis_avg < 300 && sunshine_rate > 0.9 && n.size() < 3) { 150 | continue; 151 | } 152 | } 153 | 154 | double diff_avg = 0; 155 | 156 | for (uint32_t i = 1; i < (uint32_t)n.size(); i++) { 157 | diff_avg += abs(n[i].angle - n[i - 1].angle); 158 | } 159 | 160 | diff_avg /= static_cast(n.size() - 1); 161 | 162 | if (diff_avg > angle_delta_down_limit) { 163 | normal.insert(normal.end(), n.begin(), n.end()); 164 | } 165 | } 166 | 167 | return normal; 168 | } 169 | 170 | /* 171 | \brief Enable strong filtering 172 | \param[in] 173 | \arg enable : true ，false 174 | \param[out] none 175 | \retval 176 | */ 177 | void Slbf::EnableStrictPolicy(bool enable) 178 | { 179 | enable_strict_policy = enable; 180 | } 181 | -------------------------------------------------------------------------------- /src/cmd_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #include "../include/cmd_interface_linux.hpp" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define MAX_ACK_BUF_LEN 2304000 30 | 31 | CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) 32 | : mRxThread(nullptr), mRxCount(0), mReadCallback(nullptr), version(ver) 33 | { 34 | mComHandle = -1; 35 | } 36 | 37 | 38 | CmdInterfaceLinux::~CmdInterfaceLinux() 39 | { 40 | Close(); 41 | } 42 | 43 | 44 | bool CmdInterfaceLinux::Open(std::string & port_name) 45 | { 46 | int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); 47 | 48 | mComHandle = open(port_name.c_str(), flags); 49 | if (-1 == mComHandle) { 50 | std::cout << "CmdInterfaceLinux::Open open error!" << std::endl; 51 | return false; 52 | } 53 | 54 | // get port options 55 | struct termios options; 56 | if (-1 == tcgetattr(mComHandle, &options)) { 57 | Close(); 58 | std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl; 59 | return false; 60 | } 61 | 62 | options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS); 63 | options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD); 64 | // |ECHOPRT 65 | options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN); 66 | options.c_oflag &= (tcflag_t) ~(OPOST); 67 | options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); 68 | 69 | options.c_cc[VMIN] = 0; 70 | options.c_cc[VTIME] = 0; 71 | 72 | switch (version) { 73 | case 0: cfsetispeed(&options, B115200);break; 74 | case 3: cfsetispeed(&options, B115200);break; 75 | case 6: cfsetispeed(&options, B230400);break; 76 | case 8: cfsetispeed(&options, B115200);break; 77 | case 9: cfsetispeed(&options, B115200);break; 78 | default: cfsetispeed(&options, B115200); 79 | } 80 | 81 | if (tcsetattr(mComHandle, TCSANOW, &options) < 0) { 82 | std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl; 83 | Close(); 84 | return false; 85 | } 86 | 87 | tcflush(mComHandle, TCIFLUSH); 88 | 89 | mRxThreadExitFlag = false; 90 | mRxThread = new std::thread(mRxThreadProc, this); 91 | mIsCmdOpened = true; 92 | 93 | return true; 94 | } 95 | 96 | 97 | bool CmdInterfaceLinux::Close() 98 | { 99 | if (mIsCmdOpened == false) { 100 | return true; 101 | } 102 | 103 | mRxThreadExitFlag = true; 104 | 105 | if (mComHandle != -1) { 106 | close(mComHandle); 107 | mComHandle = -1; 108 | } 109 | 110 | if ((mRxThread != nullptr) && mRxThread->joinable()) { 111 | mRxThread->join(); 112 | delete mRxThread; 113 | mRxThread = NULL; 114 | } 115 | 116 | mIsCmdOpened = false; 117 | 118 | return true; 119 | } 120 | 121 | 122 | bool CmdInterfaceLinux::GetCmdDevices( 123 | std::vector> & device_list) 125 | { 126 | struct udev * udev; 127 | struct udev_enumerate * enumerate; 128 | struct udev_list_entry * devices, * dev_list_entry; 129 | struct udev_device * dev; 130 | 131 | udev = udev_new(); 132 | if (!udev) { 133 | return false; 134 | } 135 | enumerate = udev_enumerate_new(udev); 136 | udev_enumerate_add_match_subsystem(enumerate, "tty"); 137 | udev_enumerate_scan_devices(enumerate); 138 | devices = udev_enumerate_get_list_entry(enumerate); 139 | udev_list_entry_foreach(dev_list_entry, devices) 140 | { 141 | const char * path; 142 | 143 | path = udev_list_entry_get_name(dev_list_entry); 144 | dev = udev_device_new_from_syspath(udev, path); 145 | std::string dev_path = std::string(udev_device_get_devnode(dev)); 146 | dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device"); 147 | if (dev) { 148 | std::pair p; 149 | p.first = dev_path; 150 | p.second = udev_device_get_sysattr_value(dev, "product"); 151 | device_list.push_back(p); 152 | udev_device_unref(dev); 153 | } else { 154 | continue; 155 | } 156 | } 157 | udev_enumerate_unref(enumerate); 158 | udev_unref(udev); 159 | return true; 160 | } 161 | 162 | bool CmdInterfaceLinux::ReadFromIO(uint8_t * rx_buf, uint32_t rx_buf_len, uint32_t * rx_len) 163 | { 164 | static timespec timeout = {0, (int32_t)(100 * 1e6)}; 165 | int32_t len = -1; 166 | 167 | if (IsOpened()) { 168 | fd_set read_fds; 169 | FD_ZERO(&read_fds); 170 | FD_SET(mComHandle, &read_fds); 171 | int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL); 172 | if (r < 0) { 173 | // Select was interrupted 174 | if (errno == EINTR) { 175 | return false; 176 | } 177 | } else if (r == 0) { // timeout 178 | return false; 179 | } 180 | 181 | if (FD_ISSET(mComHandle, &read_fds)) { 182 | len = (int32_t)read(mComHandle, rx_buf, rx_buf_len); 183 | if ((len != -1) && rx_len) { 184 | *rx_len = len; 185 | } 186 | } 187 | } 188 | return len == -1 ? false : true; 189 | } 190 | 191 | 192 | bool CmdInterfaceLinux::WriteToIo(const uint8_t * tx_buf, uint32_t tx_buf_len, uint32_t * tx_len) 193 | { 194 | int32_t len = -1; 195 | 196 | if (IsOpened()) { 197 | len = (int32_t)write(mComHandle, tx_buf, tx_buf_len); 198 | if ((len != -1) && tx_len) { 199 | *tx_len = len; 200 | } 201 | } 202 | return len == -1 ? false : true; 203 | } 204 | 205 | 206 | void CmdInterfaceLinux::mRxThreadProc(void * param) 207 | { 208 | CmdInterfaceLinux * cmd_if = reinterpret_cast(param); 209 | char * rx_buf = new char[MAX_ACK_BUF_LEN + 1]; 210 | while (!cmd_if->mRxThreadExitFlag.load()) { 211 | uint32_t readed = 0; 212 | bool res = cmd_if->ReadFromIO( 213 | reinterpret_cast(rx_buf), MAX_ACK_BUF_LEN, &readed); 214 | if (res && readed) { 215 | cmd_if->mRxCount += readed; 216 | if (cmd_if->mReadCallback != nullptr) { 217 | cmd_if->mReadCallback(rx_buf, readed); 218 | } 219 | } 220 | } 221 | 222 | delete[] rx_buf; 223 | } 224 | -------------------------------------------------------------------------------- /src/lipkg.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ROBOTIS CO., LTD. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | // 15 | // Author: LD Robot, Will Son 16 | 17 | #include "../include/lipkg.hpp" 18 | #include 19 | #include 20 | 21 | 22 | #define USE_SLBF 23 | 24 | #ifdef USE_SLBI 25 | #include "slbi.hpp" 26 | #endif 27 | 28 | #ifdef USE_SLBF 29 | #include "slbf.hpp" 30 | #endif 31 | 32 | 33 | static const uint8_t CrcTable[256] = 34 | { 35 | 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 36 | 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, 37 | 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 38 | 0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, 39 | 0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, 40 | 0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, 41 | 0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, 42 | 0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, 43 | 0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, 44 | 0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, 45 | 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 46 | 0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 47 | 0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 48 | 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, 49 | 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 50 | 0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 51 | 0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 52 | 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, 53 | 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 54 | 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 55 | 0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 56 | 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8 57 | }; 58 | 59 | 60 | LiPkg::LiPkg() 61 | : mTimestamp(0), mSpeed(0), mErrorTimes(0), mIsFrameReady(false), mIsPkgReady(false) 62 | { 63 | } 64 | 65 | double LiPkg::GetSpeed(void) 66 | { 67 | return mSpeed / 360.0; 68 | } 69 | 70 | bool LiPkg::Parse(const uint8_t * data, int32_t len) 71 | { 72 | for (int i = 0; i < len; i++) { 73 | mDataTmp.push_back(*(data + i)); 74 | } 75 | 76 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) { 77 | return false; 78 | } 79 | 80 | if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100) { 81 | mErrorTimes++; 82 | mDataTmp.clear(); 83 | return false; 84 | } 85 | 86 | uint16_t start = 0; 87 | 88 | while (start < mDataTmp.size() - 2) { 89 | start = 0; 90 | while (start < mDataTmp.size() - 2) { 91 | if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN)) { 92 | break; 93 | } 94 | 95 | if ((mDataTmp[start] == PKG_HEADER) && 96 | (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5)) 97 | )) 98 | { 99 | break; 100 | } 101 | start++; 102 | } 103 | 104 | if (start != 0) { 105 | mErrorTimes++; 106 | for (int i = 0; i < start; i++) { 107 | mDataTmp.erase(mDataTmp.begin()); 108 | } 109 | } 110 | 111 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) { 112 | return false; 113 | } 114 | 115 | LiDARFrameTypeDef * pkg = reinterpret_cast(mDataTmp.data()); 116 | uint8_t crc = 0; 117 | 118 | for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++) { 119 | crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff]; 120 | } 121 | 122 | if (crc == pkg->crc8) { 123 | double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360; 124 | if (diff > static_cast(pkg->speed) * POINT_PER_PACK / 2300 * 3 / 2) { 125 | mErrorTimes++; 126 | } else { 127 | mSpeed = pkg->speed; 128 | mTimestamp = pkg->timestamp; 129 | uint32_t diff = 130 | ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000; 131 | float step = diff / (POINT_PER_PACK - 1) / 100.0; 132 | float start = static_cast(pkg->start_angle) / 100.0; 133 | float end = static_cast(pkg->end_angle % 36000) / 100.0; 134 | PointData data; 135 | for (int i = 0; i < POINT_PER_PACK; i++) { 136 | data.distance = pkg->point[i].distance; 137 | data.angle = start + i * step; 138 | if (data.angle >= 360.0) { 139 | data.angle -= 360.0; 140 | } 141 | data.confidence = pkg->point[i].confidence; 142 | mOnePkg[i] = data; 143 | mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence)); 144 | } 145 | // prevent angle invert 146 | mOnePkg.back().angle = end; 147 | 148 | mIsPkgReady = true; 149 | } 150 | 151 | for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++) { 152 | mDataTmp.erase(mDataTmp.begin()); 153 | } 154 | 155 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) { 156 | break; 157 | } 158 | } else { 159 | mErrorTimes++; 160 | // only remove header,not all frame,because lidar data may contain head*/ 161 | for (int i = 0; i < 2; i++) { 162 | mDataTmp.erase(mDataTmp.begin()); 163 | } 164 | } 165 | } 166 | 167 | return true; 168 | } 169 | 170 | bool LiPkg::AssemblePacket() 171 | { 172 | float last_angle = 0; 173 | std::vector tmp; 174 | int count = 0; 175 | for (auto n = mFrameTemp.begin(); n != mFrameTemp.end(); n++) { 176 | // wait for enough data, need enough data to show a circle*/ 177 | if ((*n).angle - last_angle < (-350.f)) { // enough data has been obtained 178 | mFrameData.len = tmp.size(); 179 | Transform(tmp); // transform raw data to stantard data 180 | 181 | #ifdef USE_SLBI 182 | Slbi sb(2300); 183 | sb.FindBarcode(tmp); 184 | #endif 185 | 186 | #ifdef USE_SLBF 187 | Slbf sb(mSpeed); 188 | tmp = sb.NearFilter(tmp); 189 | #endif 190 | 191 | std::sort( 192 | tmp.begin(), tmp.end(), [](PointData a, PointData b) {return a.angle < b.angle;} 193 | ); 194 | if (tmp.size() > 0) { 195 | ToLaserscan(tmp); 196 | mIsFrameReady = true; 197 | for (auto i = 0; i < count; i++) { 198 | mFrameTemp.erase(mFrameTemp.begin()); 199 | } 200 | return true; 201 | } 202 | } else { 203 | tmp.push_back(*n); // getting data */ 204 | } 205 | 206 | count++; 207 | last_angle = (*n).angle; 208 | } 209 | 210 | return false; 211 | } 212 | 213 | const std::array & LiPkg::GetPkgData(void) 214 | { 215 | mIsPkgReady = false; 216 | return mOnePkg; 217 | } 218 | 219 | void LiPkg::ToLaserscan(std::vector src) 220 | { 221 | float angle_min, angle_max, range_min, range_max, angle_increment; 222 | 223 | // Adjust the parameters according to the demand 224 | angle_min = ANGLE_TO_RADIAN(src.front().angle); 225 | angle_max = ANGLE_TO_RADIAN(src.back().angle); 226 | range_min = 0.0; 227 | range_max = 100.0; 228 | 229 | angle_increment = ANGLE_TO_RADIAN(mSpeed / 2300); 230 | 231 | // Calculate the number of scanning points 232 | unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment); 233 | output.angle_min = angle_min; 234 | output.angle_max = angle_max; 235 | output.range_min = range_min; 236 | output.range_max = range_max; 237 | output.angle_increment = angle_increment; 238 | // mSpeed = rotation_degree / sec 239 | output.time_increment = (360 / mSpeed) / ((angle_max - angle_min) / angle_increment); 240 | output.scan_time = 360 / mSpeed; 241 | 242 | // First fill all the data with Nan 243 | output.ranges.assign(beam_size, std::numeric_limits::quiet_NaN()); 244 | output.intensities.assign(beam_size, std::numeric_limits::quiet_NaN()); 245 | 246 | for (auto point : src) { 247 | float range = point.distance / 1000.f; 248 | float angle = ANGLE_TO_RADIAN(point.angle); 249 | 250 | float index = (angle - output.angle_min) / output.angle_increment; 251 | if (index >= 0.0 && index < beam_size) { 252 | // If the current content is Nan, it is assigned directly 253 | if (std::isnan(output.ranges[index])) { 254 | output.ranges[index] = range; 255 | } else { 256 | // Otherwise, only when the distance is less than the current value, it can be re assigned 257 | if (range < output.ranges[index]) { 258 | output.ranges[index] = range; 259 | } 260 | } 261 | output.intensities[index] = point.confidence; 262 | } 263 | } 264 | } 265 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | --------------------------------------------------------------------------------