├── .github └── workflows │ └── main.yml ├── CMakeLists.txt ├── README.md ├── launch └── ld06.launch ├── package.xml ├── renovate.json ├── rviz └── ldlidar.rviz └── src ├── cmd_interface_linux.cpp ├── cmd_interface_linux.h ├── lipkg.cpp ├── lipkg.h ├── main.cpp ├── tofbf.cpp └── tofbf.h /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: CI 4 | 5 | # Controls when the workflow will run 6 | on: 7 | # Triggers the workflow on push or pull request events but only for the main branch 8 | push: 9 | branches: [ main ] 10 | pull_request: 11 | branches: [ main ] 12 | 13 | # Allows you to run this workflow manually from the Actions tab 14 | workflow_dispatch: 15 | jobs: 16 | test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. 17 | runs-on: ubuntu-latest 18 | strategy: 19 | matrix: 20 | ros_distribution: 21 | - melodic 22 | - noetic 23 | include: 24 | # Melodic Morenia (May 2018 - May 2023) 25 | - docker_image: ros:melodic-perception-bionic 26 | ros_distribution: melodic 27 | ros_version: 1 28 | 29 | # Noetic Ninjemys (May 2020 - May 2025) 30 | - docker_image: ros:noetic-perception-focal 31 | ros_distribution: noetic 32 | ros_version: 1 33 | 34 | container: 35 | image: ${{ matrix.docker_image }} 36 | steps: 37 | - name: install Melodic additional prereq 38 | if: ${{ matrix.ros_distribution == 'melodic' }} 39 | run: | 40 | echo Setup melodic additional build requirements 41 | DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install python3-pip --no-install-recommends -y 42 | pip3 install setuptools 43 | 44 | - name: install prereq tools 45 | if: ${{ matrix.ros_version == 1 }} 46 | run: | 47 | echo Setup build requirements 48 | DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install sudo python3-pip lcov git --no-install-recommends -y 49 | pip3 install colcon-common-extensions 50 | pip3 install colcon-lcov-result 51 | pip3 install colcon-coveragepy-result 52 | pip3 install colcon-mixin 53 | pip3 install vcstool 54 | 55 | - name: build and test ROS 1 56 | if: ${{ matrix.ros_version == 1 }} 57 | uses: ros-tooling/action-ros-ci@v0.2 58 | env: 59 | DEBIAN_FRONTEND: noninteractive 60 | with: 61 | package-name: ld06_lidar 62 | target-ros1-distro: ${{ matrix.ros_distribution }} 63 | - name: build and test ROS 2 64 | if: ${{ matrix.ros_version == 2 }} 65 | uses: ros-tooling/action-ros-ci@v0.2 66 | env: 67 | DEBIAN_FRONTEND: noninteractive 68 | with: 69 | package-name: ld06_lidar 70 | target-ros2-distro: ${{ matrix.ros_distribution }} 71 | - name: asan 72 | uses: ros-tooling/action-ros-ci@v0.2 73 | with: 74 | colcon-defaults: | 75 | { 76 | "build": { 77 | "mixin": ["asan-gcc"] 78 | } 79 | } 80 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/3e627e0fa30db85aea05a50e2c61a9832664d236/index.yaml 81 | package-name: ld06_lidar 82 | target-ros1-distro: ${{ matrix.ros_distribution }} 83 | env: 84 | DEBIAN_FRONTEND: noninteractive -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ld06_lidar) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | sensor_msgs 7 | ) 8 | 9 | catkin_package( 10 | # INCLUDE_DIRS include 11 | # LIBRARIES ld06_lidar 12 | # CATKIN_DEPENDS other_catkin_pkg 13 | # DEPENDS system_lib 14 | ) 15 | 16 | ########### 17 | ## Build ## 18 | ########### 19 | 20 | ## Specify additional locations of header files 21 | ## Your package locations should be listed before other locations 22 | include_directories( 23 | # include 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | ## Get current directory files 28 | aux_source_directory(. file_name) 29 | 30 | if(file_name MATCHES "slbf.cpp") 31 | add_definitions(-D"USE_SLBF") 32 | message("-- #define USE_SLBF") 33 | endif() 34 | 35 | if(file_name MATCHES "slbi.cpp") 36 | add_definitions(-D"USE_SLBI") 37 | message("-- #define USE_SLBI") 38 | endif() 39 | 40 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 41 | 42 | file(GLOB MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 43 | 44 | add_executable(ld06_lidar ${MAIN_SRC}) 45 | target_link_libraries(ld06_lidar pthread udev ${catkin_LIBRARIES}) 46 | 47 | 48 | ## Mark executables and/or libraries for installation 49 | install(TARGETS ${PROJECT_NAME} ld06_lidar 50 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | 55 | ## Mark other files for installation (e.g. launch and bag files, etc.) 56 | install(FILES 57 | launch/ld06.launch 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LD06 Lidar ROS driver 2 | 3 | This is an attempt to fix and improve the driver provided by LDRobots in their 4 | [website](https://www.ldrobot.com/download/44) 5 | 6 | ## Parameters 7 | 8 | The ldlidar node supports the following parameters: 9 | 10 | * `serial_port` used to override the autodetect and select a specific port 11 | * `lidar_frame` used to override the defauld `lidar_frame` frame_id for the 12 | `sensor_msgs/laserscan` topic generated by this node. 13 | 14 | An example launch file including all parameters is provided below: 15 | 16 | ```xml 17 | 18 | 19 | 20 | 21 | 22 | 23 | ``` 24 | 25 | ## Building the driver 26 | 27 | ```bash 28 | # create a workspace folder (or use the existing workspace for your robot) 29 | mkdir -p lidar_driver_ws/src 30 | cd lidar_driver_ws/src 31 | # clone the repo within a workspace 32 | git clone https://github.com/AlessioMorale/ld06_lidar.git 33 | cd .. 34 | # install all prerequisites & build 35 | rosdep install --from-paths src --ignore-src -y 36 | catkin build 37 | ``` 38 | ## ROS Support 39 | 40 | - Kinetic 41 | - Melodic 42 | - Noetic 43 | -------------------------------------------------------------------------------- /launch/ld06.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ld06_lidar 4 | 0.0.0 5 | The ld06_lidar package 6 | 7 | weyne 8 | 9 | BSD 10 | 11 | catkin 12 | pcl_conversions 13 | pcl_ros 14 | roscpp 15 | sensor_msgs 16 | libudev-dev 17 | pcl_conversions 18 | pcl_ros 19 | roscpp 20 | sensor_msgs 21 | pcl_conversions 22 | pcl_ros 23 | roscpp 24 | sensor_msgs 25 | libpcl-all-dev 26 | libpcl-all 27 | udev 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /renovate.json: -------------------------------------------------------------------------------- 1 | { 2 | "extends": [ 3 | "config:base" 4 | ] 5 | } 6 | -------------------------------------------------------------------------------- /rviz/ldlidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 750 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz/LaserScan 63 | Color: 255; 255; 255 64 | Color Transformer: Intensity 65 | Decay Time: 0 66 | Enabled: true 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: 251 70 | Min Color: 0; 0; 0 71 | Min Intensity: 45 72 | Name: LaserScan 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 5 77 | Size (m): 0.00999999978 78 | Style: Points 79 | Topic: /LiDAR/LD06 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 48; 48; 48 87 | Default Light: true 88 | Fixed Frame: lidar_frame 89 | Frame Rate: 30 90 | Name: root 91 | Tools: 92 | - Class: rviz/Interact 93 | Hide Inactive Objects: true 94 | - Class: rviz/MoveCamera 95 | - Class: rviz/Select 96 | - Class: rviz/FocusCamera 97 | - Class: rviz/Measure 98 | - Class: rviz/SetInitialPose 99 | Topic: /initialpose 100 | - Class: rviz/SetGoal 101 | Topic: /move_base_simple/goal 102 | - Class: rviz/PublishPoint 103 | Single click: true 104 | Topic: /clicked_point 105 | Value: true 106 | Views: 107 | Current: 108 | Class: rviz/Orbit 109 | Distance: 12.833868980407715 110 | Enable Stereo Rendering: 111 | Stereo Eye Separation: 0.0599999987 112 | Stereo Focal Distance: 1 113 | Swap Stereo Eyes: false 114 | Value: false 115 | Field of View: 0.7853981852531433 116 | Focal Point: 117 | X: 0 118 | Y: 0 119 | Z: 0 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.0500000007 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.00999999978 125 | Pitch: -1.56979632 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 0 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: false 133 | Height: 1028 134 | Hide Left Dock: false 135 | Hide Right Dock: false 136 | QMainWindow State: 000000ff00000000fd0000000400000000000001720000037dfc0200000008fb0000001200530065006c0065006300740069006f006e0000000028000000960000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037d000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001110000037dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000037d000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003bfc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004b00000037d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 137 | Selection: 138 | collapsed: false 139 | Time: 140 | collapsed: false 141 | Tool Properties: 142 | collapsed: false 143 | Views: 144 | collapsed: false 145 | Width: 1855 146 | X: 65 147 | Y: 24 148 | -------------------------------------------------------------------------------- /src/cmd_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | #include "cmd_interface_linux.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define MAX_ACK_BUF_LEN 2304000 14 | 15 | CmdInterfaceLinux::CmdInterfaceLinux():mRxThread(nullptr), 16 | mRxCount(0), 17 | mReadCallback(nullptr) 18 | { 19 | mComHandle = -1; 20 | } 21 | 22 | 23 | CmdInterfaceLinux::~CmdInterfaceLinux() 24 | { 25 | Close(); 26 | } 27 | 28 | 29 | bool CmdInterfaceLinux::Open(std::string& port_name) 30 | { 31 | int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); 32 | 33 | mComHandle = open(port_name.c_str(), flags); 34 | if (-1 == mComHandle) 35 | { 36 | std::cout << "CmdInterfaceLinux::Open open error!" << std::endl; 37 | return false; 38 | } 39 | 40 | // get port options 41 | struct termios options; 42 | if (-1 == tcgetattr(mComHandle, &options)) 43 | { 44 | Close(); 45 | std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl; 46 | return false; 47 | } 48 | 49 | options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS); 50 | options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD); 51 | options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | 52 | ISIG | IEXTEN); //|ECHOPRT 53 | options.c_oflag &= (tcflag_t) ~(OPOST); 54 | options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); 55 | 56 | options.c_cc[VMIN] = 0; 57 | options.c_cc[VTIME] = 0; 58 | 59 | cfsetispeed(&options, B230400); 60 | 61 | if (tcsetattr(mComHandle, TCSANOW, &options) < 0) 62 | { 63 | std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl; 64 | Close(); 65 | return false; 66 | } 67 | 68 | tcflush(mComHandle, TCIFLUSH); 69 | 70 | mRxThreadExitFlag = false; 71 | mRxThread = new std::thread(mRxThreadProc, this); 72 | mIsCmdOpened = true; 73 | 74 | return true; 75 | } 76 | 77 | 78 | bool CmdInterfaceLinux::Close() 79 | { 80 | if (mIsCmdOpened == false) 81 | { 82 | return true; 83 | } 84 | 85 | mRxThreadExitFlag = true; 86 | 87 | if (mComHandle != -1) 88 | { 89 | close(mComHandle); 90 | mComHandle = -1; 91 | } 92 | 93 | if ((mRxThread!=nullptr) && mRxThread->joinable()) 94 | { 95 | mRxThread->join(); 96 | delete mRxThread; 97 | mRxThread = NULL; 98 | } 99 | 100 | mIsCmdOpened = false; 101 | 102 | return true; 103 | } 104 | 105 | 106 | bool CmdInterfaceLinux::GetCmdDevices(std::vector>& device_list) 107 | { 108 | struct udev *udev; 109 | struct udev_enumerate *enumerate; 110 | struct udev_list_entry *devices, *dev_list_entry; 111 | struct udev_device *dev; 112 | 113 | udev = udev_new(); 114 | if (!udev) 115 | { 116 | return false; 117 | } 118 | enumerate = udev_enumerate_new(udev); 119 | udev_enumerate_add_match_subsystem(enumerate, "tty"); 120 | udev_enumerate_scan_devices(enumerate); 121 | devices = udev_enumerate_get_list_entry(enumerate); 122 | udev_list_entry_foreach(dev_list_entry, devices) 123 | { 124 | const char *path; 125 | 126 | path = udev_list_entry_get_name(dev_list_entry); 127 | dev = udev_device_new_from_syspath(udev, path); 128 | std::string dev_path = std::string(udev_device_get_devnode(dev)); 129 | dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device"); 130 | if (dev) 131 | { 132 | std::pair p; 133 | p.first = dev_path; 134 | p.second = udev_device_get_sysattr_value(dev, "product"); 135 | device_list.push_back(p); 136 | udev_device_unref(dev); 137 | } 138 | else 139 | { 140 | continue; 141 | } 142 | } 143 | udev_enumerate_unref(enumerate); 144 | udev_unref(udev); 145 | return true; 146 | } 147 | 148 | bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len) 149 | { 150 | static timespec timeout = { 0, (long)(100 * 1e6) }; 151 | int32_t len = -1; 152 | 153 | if (IsOpened()) 154 | { 155 | fd_set read_fds; 156 | FD_ZERO(&read_fds); 157 | FD_SET(mComHandle, &read_fds); 158 | int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL); 159 | if (r < 0) 160 | { 161 | // Select was interrupted 162 | if (errno == EINTR) 163 | { 164 | return false; 165 | } 166 | } 167 | else if (r == 0) // timeout 168 | { 169 | return false; 170 | } 171 | 172 | if (FD_ISSET(mComHandle, &read_fds)) 173 | { 174 | len = (int32_t)read(mComHandle, rx_buf, rx_buf_len); 175 | if ((len != -1) && rx_len) 176 | { 177 | *rx_len = len; 178 | } 179 | } 180 | } 181 | return len == -1 ? false : true; 182 | } 183 | 184 | 185 | bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len) 186 | { 187 | int32_t len = -1; 188 | 189 | if (IsOpened()) 190 | { 191 | len = (int32_t)write(mComHandle, tx_buf, tx_buf_len); 192 | if ((len != -1) && tx_len) 193 | { 194 | *tx_len = len; 195 | } 196 | } 197 | return len == -1 ? false : true; 198 | } 199 | 200 | 201 | void CmdInterfaceLinux::mRxThreadProc(void * param) 202 | { 203 | CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param; 204 | char * rx_buf = new char[MAX_ACK_BUF_LEN + 1]; 205 | while (!cmd_if->mRxThreadExitFlag.load()) { 206 | uint32_t readed = 0; 207 | bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); 208 | if (res && readed) { 209 | cmd_if->mRxCount += readed; 210 | if (cmd_if->mReadCallback != nullptr) 211 | { 212 | cmd_if->mReadCallback(rx_buf, readed); 213 | } 214 | } 215 | } 216 | 217 | delete[]rx_buf; 218 | } -------------------------------------------------------------------------------- /src/cmd_interface_linux.h: -------------------------------------------------------------------------------- 1 | #ifndef __LINUX_SERIAL_PORT_H__ 2 | #define __LINUX_SERIAL_PORT_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class CmdInterfaceLinux 15 | { 16 | public: 17 | CmdInterfaceLinux(); 18 | ~CmdInterfaceLinux(); 19 | 20 | bool Open(std::string& port_name); 21 | bool Close(); 22 | bool ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len); 23 | bool WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len); 24 | bool GetCmdDevices(std::vector >& device_list); 25 | void SetReadCallback(std::function callback) { mReadCallback = callback; } 26 | bool IsOpened() { return mIsCmdOpened.load(); }; 27 | 28 | 29 | private: 30 | std::thread *mRxThread; 31 | static void mRxThreadProc(void *param); 32 | long long mRxCount; 33 | int32_t mComHandle; 34 | std::atomic mIsCmdOpened, mRxThreadExitFlag; 35 | std::function mReadCallback; 36 | }; 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/lipkg.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.cpp 3 | * @author LD Robot 4 | * @version V01 5 | * @brief 6 | * @note 7 | * @attention COPYRIGHT LDROBOT 8 | **/ 9 | 10 | #include "lipkg.h" 11 | #include 12 | #include 13 | #include "tofbf.h" 14 | 15 | 16 | static const uint8_t CrcTable[256] = 17 | { 18 | 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 19 | 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, 20 | 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 21 | 0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, 22 | 0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, 23 | 0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, 24 | 0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, 25 | 0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, 26 | 0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, 27 | 0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, 28 | 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 29 | 0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 30 | 0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 31 | 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, 32 | 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 33 | 0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 34 | 0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 35 | 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, 36 | 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 37 | 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 38 | 0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 39 | 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8 40 | }; 41 | 42 | 43 | LiPkg::LiPkg(): 44 | mTimestamp(0), 45 | mSpeed(0), 46 | mErrorTimes(0), 47 | mFrameReady(false), 48 | mIsPkgReady(false) 49 | { 50 | } 51 | 52 | double LiPkg::GetSpeed(void) 53 | { 54 | return mSpeed; 55 | } 56 | 57 | bool LiPkg::Parse(const uint8_t * data, long len) 58 | { 59 | for (int i = 0; i < len; i++) 60 | { 61 | mDataTmp.push_back(*(data + i)); 62 | } 63 | 64 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) 65 | return false; 66 | 67 | if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100) 68 | { 69 | mErrorTimes++; 70 | mDataTmp.clear(); 71 | return false; 72 | } 73 | 74 | uint16_t start = 0; 75 | 76 | while (start < mDataTmp.size() - 2) 77 | { 78 | start = 0; 79 | while (start < mDataTmp.size() - 2) 80 | { 81 | if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN)) 82 | { 83 | break; 84 | } 85 | 86 | if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5)))) 87 | { 88 | break; 89 | } 90 | start++; 91 | } 92 | 93 | if (start != 0) 94 | { 95 | mErrorTimes++; 96 | for (int i = 0; i < start; i++) 97 | { 98 | mDataTmp.erase(mDataTmp.begin()); 99 | } 100 | } 101 | 102 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) 103 | return false; 104 | 105 | 106 | LiDARFrameTypeDef* pkg = (LiDARFrameTypeDef *)mDataTmp.data(); 107 | uint8_t crc = 0; 108 | 109 | for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++) 110 | { 111 | crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff]; 112 | } 113 | 114 | if (crc == pkg->crc8) 115 | { 116 | double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360; 117 | if (diff > (double)pkg->speed*POINT_PER_PACK / 4500 * 3 / 2) 118 | { 119 | mErrorTimes++; 120 | } 121 | else 122 | { 123 | mSpeed = pkg->speed; 124 | mTimestamp = pkg->timestamp; 125 | uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000; 126 | float step = diff / (POINT_PER_PACK - 1) / 100.0; 127 | float start = (double)pkg->start_angle / 100.0; 128 | float end = (double)(pkg->end_angle % 36000) / 100.0; 129 | PointData data; 130 | for (int i = 0; i < POINT_PER_PACK; i++) 131 | { 132 | data.distance = (float)pkg->point[i].distance / 1e3f; // distance in mm 133 | data.angle = start + i * step; 134 | if (data.angle >= 360.0) 135 | { 136 | data.angle -= 360.0; 137 | } 138 | data.confidence = pkg->point[i].confidence; 139 | mOnePkg[i] = data; 140 | mFrameTmp.push_back(PointData(data.angle, data.distance, data.confidence)); 141 | } 142 | //prevent angle invert 143 | mOnePkg.back().angle = end; 144 | 145 | mIsPkgReady = true; 146 | } 147 | 148 | for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++) 149 | { 150 | mDataTmp.erase(mDataTmp.begin()); 151 | } 152 | 153 | if (mDataTmp.size() < sizeof(LiDARFrameTypeDef)) 154 | { 155 | break; 156 | } 157 | } 158 | else 159 | { 160 | mErrorTimes++; 161 | /*only remove header,not all frame,because lidar data may contain head*/ 162 | for (int i = 0; i < 2; i++) 163 | { 164 | mDataTmp.erase(mDataTmp.begin()); 165 | } 166 | } 167 | } 168 | 169 | return true; 170 | } 171 | 172 | bool LiPkg::AssemblePacket() 173 | { 174 | float last_angle = 0; 175 | std::vector tmp; 176 | int count = 0; 177 | for (auto n : mFrameTmp) 178 | { 179 | /*wait for enough data, need enough data to show a circle*/ 180 | if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */ 181 | { 182 | Tofbf tofbfLd06(GetSpeed()); 183 | //std::cout << GetSpeed() << std::endl; 184 | tmp = tofbfLd06.NearFilter(tmp); 185 | std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) {return a.angle < b.angle; }); 186 | if(tmp.size()>0) 187 | { 188 | ToLaserscan(tmp); 189 | mFrameReady = true; 190 | for(auto i=0;i& LiPkg::GetPkgData(void) 210 | { 211 | mIsPkgReady = false; 212 | return mOnePkg; 213 | } 214 | 215 | void LiPkg::ToLaserscan(std::vector src) 216 | { 217 | float angle_min, angle_max, range_min, range_max, angle_increment; 218 | 219 | /*Adjust the parameters according to the demand*/ 220 | angle_min = 0 ; 221 | angle_max = M_PI * 2; 222 | range_min = 0.005; 223 | range_max = 15.0; 224 | /*Angle resolution, the smaller the resolution, the smaller the error after conversion*/ 225 | angle_increment = ANGLE_TO_RADIAN(mSpeed/4500); 226 | /*Calculate the number of scanning points*/ 227 | unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment); 228 | output.header.stamp = ros::Time::now(); 229 | output.header.frame_id = mLidarFrame; 230 | output.angle_min = angle_min; 231 | output.angle_max = angle_max; 232 | output.range_min = range_min; 233 | output.range_max = range_max; 234 | output.angle_increment = angle_increment; 235 | output.time_increment = 0.0; 236 | output.scan_time = 0.0; 237 | 238 | /*First fill all the data with Nan*/ 239 | output.ranges.assign(beam_size, std::numeric_limits::quiet_NaN()); 240 | output.intensities.assign(beam_size, std::numeric_limits::quiet_NaN()); 241 | 242 | for (auto point : src) 243 | { 244 | float range = point.distance ; 245 | float angle = ANGLE_TO_RADIAN(point.angle); 246 | 247 | int index = (int)((output.angle_max - angle) / output.angle_increment); 248 | 249 | if (index >= 0 && index < beam_size) 250 | { 251 | /*If the current content is Nan, it is assigned directly*/ 252 | if (std::isnan(output.ranges[index])) 253 | { 254 | output.ranges[index] = range; 255 | } 256 | else 257 | {/*Otherwise, only when the distance is less than the current value, it can be re assigned*/ 258 | if (range < output.ranges[index]) 259 | { 260 | output.ranges[index] = range; 261 | } 262 | } 263 | output.intensities[index] = point.confidence; 264 | } 265 | } 266 | } 267 | 268 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ -------------------------------------------------------------------------------- /src/lipkg.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.h 3 | * @author LD Robot 4 | * @version V01 5 | 6 | * @brief 7 | * @note 8 | * @attention COPYRIGHT LDROBOT 9 | **/ 10 | 11 | #ifndef __LIPKG_H 12 | #define __LIPKG_H 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000) 20 | 21 | enum 22 | { 23 | PKG_HEADER = 0x54, 24 | PKG_VER_LEN = 0x2C, 25 | POINT_PER_PACK = 12, 26 | }; 27 | 28 | 29 | typedef struct __attribute__((packed)) 30 | { 31 | uint16_t distance; 32 | uint8_t confidence; 33 | }LidarPointStructDef; 34 | 35 | typedef struct __attribute__((packed)) 36 | { 37 | uint8_t header; 38 | uint8_t ver_len; 39 | uint16_t speed; 40 | uint16_t start_angle; 41 | LidarPointStructDef point[POINT_PER_PACK]; 42 | uint16_t end_angle; 43 | uint16_t timestamp; 44 | uint8_t crc8; 45 | }LiDARFrameTypeDef; 46 | 47 | struct PointData 48 | { 49 | float angle; 50 | float distance; 51 | uint8_t confidence; 52 | double x; 53 | double y; 54 | PointData(float angle, float distance, uint8_t confidence , double x = 0, double y = 0) 55 | { 56 | this->angle = angle; 57 | this->distance = distance; 58 | this->confidence = confidence; 59 | this->x = x; 60 | this->y = y; 61 | } 62 | PointData(){} 63 | friend std::ostream& operator<<(std::ostream &os , const PointData &data) 64 | { 65 | os << data.angle << " "<< data.distance << " " << (int)data.confidence << " "<& GetPkgData(void);/*original data package*/ 82 | bool Parse(const uint8_t* data , long len);/*parse single packet*/ 83 | bool AssemblePacket();/*combine stantard data into data frames and calibrate*/ 84 | sensor_msgs::LaserScan GetLaserScan() {return output;} 85 | void SetLidarFrame(const std::string lidar_frame) { mLidarFrame = lidar_frame;}; 86 | private: 87 | uint16_t mTimestamp; 88 | double mSpeed; 89 | std::vector mDataTmp; 90 | long mErrorTimes; 91 | std::arraymOnePkg; 92 | std::vector mFrameTmp; 93 | bool mIsPkgReady; 94 | bool mFrameReady; 95 | sensor_msgs::LaserScan output; 96 | std::string mLidarFrame; 97 | void ToLaserscan(std::vector src); 98 | }; 99 | #endif 100 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ 101 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "cmd_interface_linux.h" 3 | #include 4 | #include "lipkg.h" 5 | #include 6 | #include 7 | #include "tofbf.h" 8 | #include 9 | 10 | int main(int argc , char **argv) 11 | { 12 | ros::init(argc, argv, "ld06_lidar"); 13 | ros::NodeHandle nh; /* create a ROS Node */ 14 | ros::NodeHandle nh_private("~"); 15 | 16 | LiPkg * lidar = new LiPkg; 17 | 18 | CmdInterfaceLinux cmd_port; 19 | std::string port_name; 20 | std::string lidar_frame; 21 | 22 | nh_private.param("lidar_frame", lidar_frame, "lidar_frame"); 23 | lidar->SetLidarFrame(lidar_frame); 24 | 25 | nh_private.param("serial_port", port_name, std::string()); 26 | 27 | if (port_name.empty()) 28 | { 29 | ROS_INFO("Autodetecting serial port"); 30 | std::vector> device_list; 31 | cmd_port.GetCmdDevices(device_list); 32 | auto found = std::find_if( 33 | device_list.begin(), 34 | device_list.end(), 35 | [](std::pair n) 36 | { return strstr(n.second.c_str(), "CP2102"); }); 37 | 38 | if (found != device_list.end()) 39 | { 40 | ROS_INFO_STREAM(found->first << " " << found->second); 41 | port_name = found->first; 42 | } 43 | else 44 | { 45 | ROS_ERROR("Can't find LiDAR LD06"); 46 | return -1; 47 | } 48 | } 49 | 50 | ROS_INFO_STREAM("Using port " << port_name); 51 | 52 | cmd_port.SetReadCallback([&lidar](const char *byte, size_t len) { 53 | if(lidar->Parse((uint8_t*)byte, len)) 54 | { 55 | lidar->AssemblePacket(); 56 | } 57 | }); 58 | 59 | if(cmd_port.Open(port_name)){ 60 | ROS_INFO("LiDAR_LD06 started successfully"); 61 | } else { 62 | ROS_ERROR("Can't open the serial port"); 63 | return -1; 64 | } 65 | ros::Publisher lidar_pub = nh.advertise("LiDAR/LD06", 1); /*create a ROS topic */ 66 | 67 | ros::Rate rate(30); 68 | while (ros::ok()) 69 | { 70 | if (lidar->IsFrameReady()) 71 | { 72 | lidar_pub.publish(lidar->GetLaserScan()); // Fixed Frame: lidar_frame 73 | lidar->ResetFrameReady(); 74 | } 75 | rate.sleep(); 76 | } 77 | return 0; 78 | } 79 | 80 | -------------------------------------------------------------------------------- /src/tofbf.cpp: -------------------------------------------------------------------------------- 1 |  2 | #include "tofbf.h" 3 | #include 4 | #include 5 | #include 6 | 7 | /*! 8 | \brief Select the radar version number and set the current speed 9 | \param[in] 10 | \arg version Version number of current radar 11 | \arg speed Current radar speed 12 | \param[out] none A circle of radar data packed 13 | \retval none 14 | */ 15 | Tofbf::Tofbf(int speed) 16 | { 17 | curr_speed = speed; 18 | } 19 | 20 | Tofbf::~Tofbf() 21 | { 22 | } 23 | 24 | /*! 25 | \brief Filter within 5m to filter out unreasonable data points 26 | \param[in] 27 | \arg tmp 28 | \param[out] none 29 | \retval Normal data 30 | */ 31 | std::vector Tofbf::NearFilter(const std::vector &tmp) const 32 | { 33 | std::vector normal, pending, item; 34 | std::vector> group; 35 | 36 | // Remove points within 5m 37 | for (auto n : tmp) 38 | { 39 | if (n.distance < 5000) 40 | { 41 | pending.push_back(n); 42 | } 43 | else 44 | { 45 | normal.push_back(n); 46 | } 47 | } 48 | 49 | if (tmp.empty()) 50 | return normal; 51 | 52 | double angle_delta_up_limit = curr_speed / SCAN_FRE * 2; 53 | 54 | // std::cout < angle_delta_up_limit || abs(n.distance - last.distance) > last.distance*0.03) 64 | { 65 | if(item.empty() == false) 66 | { 67 | group.push_back(item); 68 | item.clear(); 69 | } 70 | } 71 | item.push_back(n); 72 | last = n; 73 | } 74 | // push back last item 75 | if (item.empty() == false) 76 | group.push_back(item); 77 | 78 | if (group.empty()) 79 | return normal; 80 | 81 | // Connection 0 degree and 359 degree 82 | auto first_item = group.front().front(); 83 | auto last_item = group.back().back(); 84 | if (abs(first_item.angle + 360.f - last_item.angle) < angle_delta_up_limit && abs(first_item.distance - last_item.distance) < last.distance*0.03) 85 | { 86 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 87 | group.erase(group.end() - 1); 88 | } 89 | //selection 90 | for (auto n : group) 91 | { 92 | if (n.size() == 0) 93 | continue; 94 | //No filtering if there are many points 95 | if (n.size() > 15) 96 | { 97 | normal.insert(normal.end(), n.begin(), n.end()); 98 | continue; 99 | } 100 | 101 | //Filter out those with few points 102 | if (n.size() < 3) 103 | { 104 | int c = 0; 105 | for(auto m : n) 106 | { 107 | c += m.confidence; 108 | } 109 | c /= n.size(); 110 | if(c < CONFIDENCE_SINGLE) 111 | continue; 112 | } 113 | 114 | //Calculate the mean value of distance and confidence 115 | double confidence_avg = 0; 116 | double dis_avg = 0; 117 | for (auto m : n) 118 | { 119 | confidence_avg += m.confidence; 120 | dis_avg += m.distance; 121 | } 122 | confidence_avg /= n.size(); 123 | dis_avg /= n.size(); 124 | 125 | //High confidence, no filtering 126 | if (confidence_avg > CONFIDENCE_LOW) 127 | { 128 | normal.insert(normal.end(), n.begin(), n.end()); 129 | continue; 130 | } 131 | 132 | } 133 | 134 | return normal; 135 | } 136 | 137 | -------------------------------------------------------------------------------- /src/tofbf.h: -------------------------------------------------------------------------------- 1 | #ifndef __TOFBF_H_ 2 | #define __TOFBF_H_ 3 | 4 | #include 5 | #include 6 | #include "lipkg.h" 7 | 8 | class Tofbf 9 | { 10 | private: 11 | const int CONFIDENCE_LOW = 15; /* Low confidence threshold */ 12 | const int CONFIDENCE_SINGLE = 220; /* Discrete points require higher confidence */ 13 | const int SCAN_FRE = 4500; /* Default scan frequency, to change, read according to radar protocol */ 14 | double offset_x, offset_y; 15 | double curr_speed; 16 | Tofbf() = delete; 17 | Tofbf(const Tofbf &) = delete; 18 | Tofbf &operator=(const Tofbf &) = delete; 19 | public: 20 | Tofbf(int speed); 21 | std::vector NearFilter(const std::vector &tmp) const; 22 | ~Tofbf(); 23 | }; 24 | 25 | #endif 26 | --------------------------------------------------------------------------------