├── .gitignore ├── PC_ROS_SDK ├── README_cn.md ├── README_en.md ├── doc │ ├── LDROBOT_LD06_Development_manual_v1.0_en.pdf │ └── LDROBOT_LD06_开发手册_v1.0.pdf ├── rviz │ └── ldlidar.rviz └── src │ └── ldlidar │ ├── CMakeLists.txt │ ├── launch │ └── ld19.launch │ ├── package.xml │ └── src │ ├── cmd_interface_linux.cpp │ ├── cmd_interface_linux.h │ ├── lipkg.cpp │ ├── lipkg.h │ ├── main.cpp │ ├── tofbf.cpp │ └── tofbf.h ├── README.md ├── SBC_ROS_SDK ├── README_cn.md ├── README_en.md ├── doc │ └── LDRobot_LD19_Raspbian_User_manual_V2.3.pdf ├── rviz │ └── ldlidar.rviz └── src │ └── ldlidar │ ├── CMakeLists.txt │ ├── launch │ └── ld19.launch │ ├── package.xml │ └── src │ ├── cmd_interface_linux.cpp │ ├── cmd_interface_linux.h │ ├── lipkg.cpp │ ├── lipkg.h │ ├── main.cpp │ ├── pid.h │ ├── tofbf.cpp │ └── tofbf.h ├── datasheet ├── LDROBOT_LD19_Datasheet_V1.0.pdf └── LDROBOT_LD19_数据手册_v1.1.pdf └── pic ├── en_conne.png ├── image-20210802170439262.png ├── image-20210803135324723.png ├── 文件结构.png └── 树莓派gpio.png /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | ws_clear.sh 3 | ws_deploy.sh 4 | TODO -------------------------------------------------------------------------------- /PC_ROS_SDK/README_cn.md: -------------------------------------------------------------------------------- 1 | # PC_ROS_SDK使用说明（LD19） 2 | 3 | --- 4 | 5 | > `PC_ROS_SDK`文件夹中包含的SDK适合应用于没有原生串口外设的平台，比如PC，需要使用CP2102等USB转串口芯片来连接LD19雷达与开发平台。 6 | > 7 | > **注意**：请确保自己的雷达是通过**CP2102**芯片的转接线与开发设备相连，其他型号的芯片并不能直接使用。 8 | 9 | ## 概述 10 | 11 | 此SDK并不是只能在PC上使用，对于任何通过USB转串口芯片连接的开发平台，此SDK都是适合的。比如使用树莓派的USB接口来连接雷达时就需要选用此版本。 12 | 13 | 在`PC_ROS_SDK`目录包含以下内容： 14 | 15 | ``` 16 | /doc/ 17 | /rviz/ 18 | /src/ 19 | /README_cn.md 20 | /README_en.md 21 | ``` 22 | 23 | - `src`包含SDK源码 24 | 25 | - `doc`下是中英文[**开发手册**](./doc/LDROBOT_LD06_开发手册_v1.0.pdf)（LD19与LD06手册通用，名称有差异） 26 | 27 | - `rviz`的内容是使用rviz进行雷达数据的可视化 28 | 29 | 本SDK使用Ubuntu18.04下的Melodic，16.04下的Kinetic以及搭载Noetic和Ubuntu20.04 Server的树莓派3B+下测试通过。 30 | 31 | ## 使用方法 32 | 33 | ### 硬件准备 34 | 35 | > LDRobot LD19激光雷达、CP2102串口转USB线缆 36 | 37 | 按照下图（见开发手册“通讯接口”一节）连接线缆与雷达： 38 | 39 | ![LD19通讯接口图](../pic/image-20210802170439262.png) 40 | 41 | LD19采用ZH1.5T-4P 1.5mm接口。其中PWM信号**可以悬空不接**，此时雷达将以默认转速运行。 42 | 43 | 最后，将CP2102插入上位机USB口，并确保正确识别了它。 44 | 45 | ### 软件准备 46 | 47 | 此SDK需要上位机安装好ROS，推荐使用Ubuntu18.04、16.04，ROS：Melodic、Kinetic。 48 | 49 | 整个过程可以使用普通用户来操作，无需以root身份登入。但要确保所使用的用户拥有对应串口的读写权限。 50 | 51 | 设置权限参见开发手册[第五章第一节：设置权限](./doc/LDROBOT_LD06_开发手册_v1.0.pdf) 52 | 53 | 至此，软硬件准备完毕。 54 | 55 | ### 使用流程 56 | 57 | 1. 将文件夹`PC_ROS_SDK/src`与`PC_ROS_SDK/rviz`复制保存在某空白目录A下。 58 | 2. 在目录A下打开终端，运行`catkin_make`命令进行编译。 59 | 3. 在编译成功后运行`source ./devel/setup.bash`添加环境变量。 60 | 4. 调用命令`roslaunch ldlidar ld19.launch`运行SDK，如果显示successfully则成功。 61 | 5. 开启成功后需要保持程序运行才能在ROS里使用雷达，`Ctrl+C`或者关闭终端将中止程序。 62 | 63 | **注意：** 64 | 65 | 在每次打开新的终端时，都需要先按照第三条添加环境变量才能进行第四条。如果希望省略第三步，可以将相应命令添加到`~/.bashrc`文件中。 66 | 67 | 在运行SDK后如果连接不到雷达，则会输出`open error`字样。 68 | 69 | 如果希望关闭终端后依然运行，可以配合`nohup`命令使之后台运行。 70 | 71 | ## 使用实例 72 | 73 | 一个使用rviz将雷达数据实时可视化的例子，请参阅[开发手册第五章](./doc/LDROBOT_LD06_开发手册_v1.0.pdf)。 74 | 75 | -------------------------------------------------------------------------------- /PC_ROS_SDK/README_en.md: -------------------------------------------------------------------------------- 1 | # PC_ROS_SDK Direction Of Use(LD19) 2 | 3 | --- 4 | 5 | > Directory `PC_ROS_SDK` contains the SDK that is suitable for the platform without native serial peripheral, such as PC, it requires an extra CP2102 USB to UART bridge to connect LD19 and the upper platform. 6 | > 7 | > **Notice**: Make sure that the USB to UART chip you use is **CP2102**, other chips of the same function CAN NOT be recognized by the SDK without any modification. 8 | 9 | ## General Description 10 | 11 | This SDK could be used on any platform that is connected to LD19 through CP2102, not confined to PC. 12 | 13 | The contents of directory `PC_ROS_SDK` are listed below: 14 | 15 | ``` 16 | /doc/ 17 | /rviz/ 18 | /src/ 19 | /README_cn.md 20 | /README_en.md 21 | ``` 22 | 23 | + `src` contains the source code of SDK 24 | + `doc` contains the user manual in English and Chinese (LD19 shares the same documents with LD06, pls be awarded) 25 | + `rviz` contains the file which is used to configure the visualization of radar data 26 | 27 | This SDK has been tested in Ubuntu 18.04 with Melodic, Ubuntu 16.04 with Kinetic on PC and Noetic in Ubuntu 20.04 Server on Raspberry Pi3B+. 28 | 29 | ## Usage 30 | 31 | ### Hardware Preparation 32 | 33 | > LDRobot LD19 radar, CP2102 USB to UART Bridge 34 | 35 | Link LD19 to the upper computer according to the diagram below(could be found in the section Communication Interface of [user manual](./doc/LDROBOT_LD06_Development_manual_v1.0_en.pdf)): 36 | 37 | ![schematic](../pic/en_conne.png) 38 | 39 | LD19 adopt ZH1.5T-4P 1.5mm connector. The PWM signal, namely the second pin, need not to be attached to certain GPIO if you're not going to control the revolving speed. 40 | 41 | Then plug the CP2102 to the USB port of upper computer, check if it's recognized. 42 | 43 | ### Software Preparation 44 | 45 | The SDK requires installed ROS, Ubuntu:18.04、16.04，ROS: Melodic、Kinetic are recommended. 46 | 47 | The whole process could be conducted with a normal user's account, however, the **authority of using the ttyUSB device is a necessity**. 48 | 49 | For setting the access please refer to the [User Manual Section 5.1: Set Access](./doc/LDROBOT_LD06_Development_manual_v1.0_en.pdf) 50 | 51 | So far, all the preparation is done. 52 | 53 | ### Procedure Of Use 54 | 55 | 1. Copy folder `PC_ROS_SDK/src` and `PC_ROS_SDK/rviz` to certain empty directory A. 56 | 2. Open a terminal in A, run `catkin_make` to compile the SDK. 57 | 3. If the target has been built, run `source ./devel/setup.bash` to add some environmental variables. 58 | 4. Call `roslaunch ldlidar ld19.alaunch` at any directory to run the SDK, if there's a `Successfully` in the end of output, the LD19 has been found. 59 | 5. Keep the terminal open, the SDK should be kept running when other ROS packets 60 | 61 | **Notice** 62 | 63 | Every time you open a terminal, step 3 should be performed before step 4. There's a way to simplify it, add `source ./devel/setup.bash` to `~/.bashrc`, then you can ignore step 3 every time start a new terminal. 64 | 65 | If the SDK can't find the radar, it will print `open error`, but it won't exit. 66 | 67 | You can make use of `nohup` to let the SDK runs in the background. 68 | 69 | ## Example 70 | 71 | An example of using rviz to visualize real time data from LD19 is in the [chapter 5 of User Manual](./doc/LDROBOT_LD06_Development_manual_v1.0_en.pdf). 72 | 73 | -------------------------------------------------------------------------------- /PC_ROS_SDK/doc/LDROBOT_LD06_Development_manual_v1.0_en.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/PC_ROS_SDK/doc/LDROBOT_LD06_Development_manual_v1.0_en.pdf -------------------------------------------------------------------------------- /PC_ROS_SDK/doc/LDROBOT_LD06_开发手册_v1.0.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/PC_ROS_SDK/doc/LDROBOT_LD06_开发手册_v1.0.pdf -------------------------------------------------------------------------------- /PC_ROS_SDK/rviz/ldlidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 121 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /LaserScan1 11 | Splitter Ratio: 0.5 12 | Tree Height: 735 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 1000 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 10 59 | Min Value: -10 60 | Value: true 61 | Axis: Z 62 | Channel Name: intensity 63 | Class: rviz/LaserScan 64 | Color: 255; 255; 255 65 | Color Transformer: Intensity 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 234 71 | Min Color: 0; 0; 0 72 | Min Intensity: 3 73 | Name: LaserScan 74 | Position Transformer: XYZ 75 | Queue Size: 100 76 | Selectable: true 77 | Size (Pixels): 5 78 | Size (m): 0.00999999978 79 | Style: Points 80 | Topic: /LiDAR/LD19 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | Enabled: true 86 | Global Options: 87 | Background Color: 48; 48; 48 88 | Default Light: true 89 | Fixed Frame: lidar_frame 90 | Frame Rate: 30 91 | Name: root 92 | Tools: 93 | - Class: rviz/Interact 94 | Hide Inactive Objects: true 95 | - Class: rviz/MoveCamera 96 | - Class: rviz/Select 97 | - Class: rviz/FocusCamera 98 | - Class: rviz/Measure 99 | - Class: rviz/SetInitialPose 100 | Topic: /initialpose 101 | - Class: rviz/SetGoal 102 | Topic: /move_base_simple/goal 103 | - Class: rviz/PublishPoint 104 | Single click: true 105 | Topic: /clicked_point 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz/Orbit 110 | Distance: 13920.4805 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.0599999987 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 0 118 | Y: 0 119 | Z: 0 120 | Focal Shape Fixed Size: false 121 | Focal Shape Size: 0.0500000007 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.00999999978 125 | Pitch: 1.26039791 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 1.45039761 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: false 133 | Height: 1056 134 | Hide Left Dock: false 135 | Hide Right Dock: false 136 | QMainWindow State: 000000ff00000000fd0000000400000000000001c500000399fc0200000008fb0000001200530065006c0065006300740069006f006e0000000028000000960000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000399000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000016200000399fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000399000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007550000003bfc0100000002fb0000000800540069006d00650100000000000007550000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004220000039900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 137 | Selection: 138 | collapsed: false 139 | Time: 140 | collapsed: false 141 | Tool Properties: 142 | collapsed: false 143 | Views: 144 | collapsed: false 145 | Width: 1877 146 | X: 43 147 | Y: 24 148 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ldlidar) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g ") 8 | # set (CMAKE_VERBOSE_MAKEFILE ON) 9 | 10 | ## Find catkin macros and libraries 11 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 12 | ## is used, also find other catkin packages 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | sensor_msgs 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs # Or other packages containing msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES ldlidar 110 | # CATKIN_DEPENDS other_catkin_pkg 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Get current directory files 126 | aux_source_directory(. file_name) 127 | 128 | if(file_name MATCHES "slbf.cpp") 129 | add_definitions(-D"USE_SLBF") 130 | message("-- #define USE_SLBF") 131 | endif() 132 | 133 | if(file_name MATCHES "slbi.cpp") 134 | add_definitions(-D"USE_SLBI") 135 | message("-- #define USE_SLBI") 136 | endif() 137 | 138 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 139 | 140 | file(GLOB MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 141 | 142 | add_executable(ldlidar ${MAIN_SRC}) 143 | target_link_libraries(ldlidar pthread udev ${catkin_LIBRARIES}) 144 | 145 | 146 | ## Declare a C++ library 147 | # add_library(${PROJECT_NAME} 148 | # src/${PROJECT_NAME}/ldlidar.cpp 149 | # ) 150 | 151 | ## Add cmake target dependencies of the library 152 | ## as an example, code may need to be generated before libraries 153 | ## either from message generation or dynamic reconfigure 154 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Declare a C++ executable 157 | ## With catkin_make all packages are built within a single CMake context 158 | ## The recommended prefix ensures that target names across packages don't collide 159 | # add_executable(${PROJECT_NAME}_node src/ldlidar_node.cpp) 160 | 161 | ## Rename C++ executable without prefix 162 | ## The above recommended prefix causes long target names, the following renames the 163 | ## target back to the shorter version for ease of user use 164 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 165 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 166 | 167 | ## Add cmake target dependencies of the executable 168 | ## same as for the library above 169 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 170 | 171 | ## Specify libraries to link a library or executable target against 172 | # target_link_libraries(${PROJECT_NAME}_node 173 | # ${catkin_LIBRARIES} 174 | # ) 175 | 176 | ############# 177 | ## Install ## 178 | ############# 179 | 180 | # all install targets should use catkin DESTINATION variables 181 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 182 | 183 | ## Mark executable scripts (Python etc.) for installation 184 | ## in contrast to setup.py, you can choose the destination 185 | # install(PROGRAMS 186 | # scripts/my_python_script 187 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 188 | # ) 189 | 190 | ## Mark executables and/or libraries for installation 191 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 192 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 193 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 194 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 195 | # ) 196 | 197 | ## Mark cpp header files for installation 198 | # install(DIRECTORY include/${PROJECT_NAME}/ 199 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 200 | # FILES_MATCHING PATTERN "*.h" 201 | # PATTERN ".svn" EXCLUDE 202 | # ) 203 | 204 | ## Mark other files for installation (e.g. launch and bag files, etc.) 205 | # install(FILES 206 | # # myfile1 207 | # # myfile2 208 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 209 | # ) 210 | 211 | ############# 212 | ## Testing ## 213 | ############# 214 | 215 | ## Add gtest based cpp test target and link libraries 216 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ldlidar.cpp) 217 | # if(TARGET ${PROJECT_NAME}-test) 218 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 219 | # endif() 220 | 221 | ## Add folders to be run by python nosetests 222 | # catkin_add_nosetests(test) 223 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/launch/ld19.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ldlidar 4 | 0.0.0 5 | The ldlidar package 6 | 7 | 8 | 9 | 10 | weyne 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | pcl_conversions 53 | pcl_ros 54 | roscpp 55 | sensor_msgs 56 | pcl_conversions 57 | pcl_ros 58 | roscpp 59 | sensor_msgs 60 | pcl_conversions 61 | pcl_ros 62 | roscpp 63 | sensor_msgs 64 | libpcl-all-dev 65 | libpcl-all 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | } -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 = pkg->point[i].distance; 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 = 3.14159*2; 222 | range_min = 0.5; 223 | range_max = 15000; 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 = "lidar_frame"; 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)((angle - output.angle_min) / output.angle_increment); 248 | if (index >= 0 && index < beam_size) 249 | { 250 | /*If the current content is Nan, it is assigned directly*/ 251 | if (std::isnan(output.ranges[index])) 252 | { 253 | output.ranges[index] = range; 254 | } 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 | { 259 | output.ranges[index] = range; 260 | } 261 | } 262 | output.intensities[index] = point.confidence; 263 | } 264 | } 265 | } 266 | 267 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | uint16_t distance; 51 | uint8_t confidence; 52 | double x; 53 | double y; 54 | PointData(float angle, uint16_t 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 | 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 | void ToLaserscan(std::vector src); 97 | }; 98 | #endif 99 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ 100 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | #define RADIAN_TO_ANGLED(angle) ((angle)*180000/3141.59) 11 | 12 | int main(int argc , char **argv) 13 | { 14 | ros::init(argc, argv, "product"); 15 | ros::NodeHandle nh; /* create a ROS Node */ 16 | 17 | 18 | LiPkg * lidar = new LiPkg; 19 | 20 | CmdInterfaceLinux cmd_port; 21 | std::vector > device_list; 22 | std::string port_name; 23 | cmd_port.GetCmdDevices(device_list); 24 | for (auto n : device_list) 25 | { 26 | std::cout << n.first << " " << n.second << std::endl; 27 | if(strstr(n.second.c_str(),"CP2102")) 28 | { 29 | port_name = n.first; 30 | } 31 | } 32 | 33 | if(port_name.empty()) 34 | { 35 | std::cout<<"Can't find LiDAR LD19"<< std::endl; 36 | } 37 | 38 | std::cout<<"FOUND LiDAR_LD19" <Parse((uint8_t*)byte, len)) 41 | { 42 | lidar->AssemblePacket(); 43 | } 44 | }); 45 | 46 | if(cmd_port.Open(port_name)) 47 | std::cout<<"LiDAR_LD19 started successfully " <("LiDAR/LD19", 1); /*create a ROS topic */ 50 | 51 | while (ros::ok()) 52 | { 53 | if (lidar->IsFrameReady()) 54 | { 55 | lidar_pub.publish(lidar->GetLaserScan()); // Fixed Frame: lidar_frame 56 | lidar->ResetFrameReady(); 57 | #if 1 58 | sensor_msgs::LaserScan data = lidar->GetLaserScan(); 59 | unsigned int lens = (data.angle_max - data.angle_min) / data.angle_increment; 60 | std::cout << "current_speed: " << lidar->GetSpeed() << " " 61 | << "len: " << lens << " " 62 | << "angle_min: " << RADIAN_TO_ANGLED(data.angle_min) << " " 63 | << "angle_max: " << RADIAN_TO_ANGLED(data.angle_max) << std::endl; 64 | std::cout << "----------------------------" << std::endl; 65 | for (int i = 0; i < lens; i++) 66 | { 67 | std::cout << "range: " << data.ranges[i] << " " 68 | << "intensites: " << data.intensities[i] << std::endl; 69 | } 70 | std::cout << "----------------------------" << std::endl; 71 | #endif 72 | } 73 | } 74 | return 0; 75 | } 76 | 77 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | -------------------------------------------------------------------------------- /PC_ROS_SDK/src/ldlidar/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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LD19 DTOF ROS_SDK 2 | 3 | --- 4 | 5 | > 本项目是激光TOF雷达LD19的开发SDK。 6 | > 7 | > 基于ROS，此SDK让LD19雷达可以在多平台（PC、SBC）下进行开发。 8 | > 9 | > [English Version Here](https://github.com/HuXioAn/ld19#english-version) 10 | 11 | # 概述 12 | 13 | 项目主体结构如图： 14 | 15 | ![文件结构图](./pic/文件结构.png) 16 | 17 | `datasheet`目录下为中英文LD19雷达数据手册，描述其硬件参数。 18 | 19 | `PC_ROS_SDK`和`SBC_ROS_SDK`目录分别存放着SDK的两个版本，使用者需要根据情况来**选择其中一个**使用。两个目录下分别有中英文README进行更详细的说明。 20 | 21 | 链接在此：[PC_README](./PC_ROS_SDK/README_cn.md)、[SBC_README](./SBC_ROS_SDK/README_cn.md) 22 | 23 | ## PC与SBC版本的区别 24 | 25 | 使用PC与SBC(Single Board Computer)进行两个版本的划分实际上只是体现一种**代表性**。 26 | 27 | 两个版本的区别在于其寻找雷达设备时，PC版会去寻找`CP2102`转换芯片的USB串口，SBC则会直接调用设备上的串口外设。因为绝大部分电脑已经不配备串口，连接雷达时需要使用转换芯片（CP2102）。而单板计算机，以树莓派为例，它们具有原生串口外设，不需要转换芯片。 28 | 29 | 举个例子，如果要在树莓派的USB口上外接ld19雷达，那么需要选择`PC_ROS_SDK`。 30 | 31 | 如果选择了不合适的版本，编译并不会出现任何错误，但是运行时就无法找到雷达。 32 | 33 | ## 使用流程 34 | 35 | 1. 根据开发平台，结合前文所述确定使用的版本。 36 | 2. 进入所选版本对应的文件夹，查看`README`获取更详细的步骤。[PC_README](./PC_ROS_SDK/README_cn.md)、[SBC_README](./SBC_ROS_SDK/README_cn.md) 37 | 38 | ## 注意！ 39 | 40 | 由于LD19雷达与LD06雷达基本一致，SDK内部分手册为LD06手册，请注意需要改变内容的地方。 41 | 42 | ---- 43 | 44 | ## English Version: 45 | 46 | > This repositorise contains the SDK for LD19 DTOF radar. 47 | > 48 | > Basing on ROS, the SDK allows the development of LD19 on different platform(PC, SBC). 49 | 50 | ## General discription 51 | 52 | The file structure of this repositories is shown in the graph below: 53 | 54 | ![file structure](./pic/文件结构.png) 55 | 56 | You could find three directories: `datasheet`, `PC_ROS_SDK`, `SBC_ROS_SDK`. 57 | 58 | In the `datasheet`, there're datasheet files of the radar LD19 in both English and Chinese. 59 | 60 | The two versions of SDK are separately stored in `PC_ROS_SDK`and `SBC_ROS_SDK`, you have to **choose one version from the two** accroding to the next section. What's more, the detailed instructions are also in the two directories. 61 | 62 | Links are here:[PC_README](./PC_ROS_SDK/README_en.md),[SBC_README](./SBC_ROS_SDK/README_en.md) 63 | 64 | ## Difference betweent PC and SBC versions 65 | 66 | **Please read this section with carefulness, the version of your choice could be decisive to the whole process.** 67 | 68 | We use names "PC" and "SBC(Single Board Computer)" to indicate the features of the two versions, for the PC version was programmed to search the `CP2102` device, the functions of which are converting the UART signal to USB and simulating a virtual COM, while the SBC version will directly use the serial on the board. 69 | 70 | Owing to the fact that most of PC do not have any Serial port, extra hardware(CP2102) is needed. On the contrary, SBCs like Raspberry Pi are equipped with Serial peripheral, radar can be linked to the board through GPIOs. 71 | 72 | For example, if you are going to connect your radar and Raspberry Pi through the USB port, then you need to use PC version rather than SBC version. 73 | 74 | If a inappropriate SDK is put into use, there won't be any abnormity but it just can't find the radar. 75 | 76 | ## Usage 77 | 78 | 1. Read the section above then choose the proper version for your hardware. 79 | 2. Please turn to the README contained in the directory of the version you chose for further instructions in detail.[PC_README](./PC_ROS_SDK/README_en.md),[SBC_README](./SBC_ROS_SDK/README_en.md) 80 | 81 | ## Notice 82 | 83 | LD19 is basically the same as LD06 in all aspects, so some manual books are based on LD06, please be aware of the contents that need modifying. 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/README_cn.md: -------------------------------------------------------------------------------- 1 | # SBC_ROS_SDK使用说明（LD19） 2 | 3 | > `SBC_ROS_SDK`文件夹中包含的SDK版本适合应用于带有**原生串口外设**的平台，比如树莓派，可以直接连接LD19雷达与开发平台，这也是此版本称作SBC版的原因。 4 | 5 | ## 概述 6 | 7 | 在`SBC_ROS_SDK`目录包含以下内容： 8 | 9 | ``` 10 | /doc/ 11 | /rviz/ 12 | /src/ 13 | /README_cn.md 14 | /README_en.md 15 | ``` 16 | 17 | - `src`包含SDK源码 18 | 19 | - `doc`下是Raspbian_ROS[开发手册](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf)（LD19与LD06手册通用，名称有差异） 20 | 21 | - `rviz`的内容是使用rviz进行雷达数据的可视化 22 | 23 | 本SDK使用搭载Kinetic和Raspbian 9以及Noetic和Ubuntu20.04 Server的树莓派3B+上测试通过。 24 | 25 | ## 使用方法 26 | 27 | ### 硬件准备 28 | 29 | > LDRobot LD19激光雷达、连接线 30 | 31 | 按照下图（见开发手册“通讯接口”一节）连接GPIO与雷达： 32 | 33 | ![LD19通讯接口图](../pic/image-20210802170439262.png) 34 | 35 | LD19采用ZH1.5T-4P 1.5mm接口。其中PWM信号**可以悬空不接**，此时雷达将以默认转速运行。 36 | 37 | 将供电与地连接到SBC的5V与GND，将雷达的TX线连接到SBC串口的RX引脚上，这里以树莓派3B+为例： 38 | 39 | ![树莓派GPIO](../pic/树莓派gpio.png) 40 | 41 | > 1(Tx)------------->10(RXD) 42 | > 43 | > 2(PWM)---------悬空 44 | > 45 | > 3(GND)--------->6(GND) 46 | > 47 | > 4(P5V)---------->(5V Power) 48 | 49 | ### 软件准备 50 | 51 | 此SDK需要上位机安装好ROS，由于种种原因，在SBC上安装ROS会遇到比较多的困难，可以考虑使用Ubuntu系统，或者直接使用他人编译的已经安装ROS的系统镜像。开发手册内描述的ROS安装方法为下载源码本地编译，需要较好的网络条件。 52 | 53 | 树莓派需要在系统配置页面使能串口，不然将不会在`/dev/`下看到对应的tty设备。具体操作参见`doc`目录下使用手册第四章：[START SERIAL ttyS0](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf)。 54 | 55 | 开发手册中提到的`WiringPi`与`udev`库并不是必须的，可以跳过安装。 56 | 57 | 接下来的过程可以使用普通用户来操作，无需以root身份登入。但要确保所使用的用户拥有对应串口的**读写权限**。 58 | 59 | 至此，软硬件准备完毕。 60 | 61 | ### 使用流程 62 | 63 | 1. 将文件夹`SBC_ROS_SDK/src`与`SBC_ROS_SDK/rviz`复制保存在某空白目录A下。 64 | 2. 在目录A下打开终端，运行`catkin_make`命令进行编译。 65 | 3. 在编译成功后运行`source ./devel/setup.bash`添加环境变量。 66 | 4. 调用命令`roslaunch ldlidar ld19.launch`运行SDK，如果显示successfully则成功。 67 | 5. 开启成功后需要保持程序运行才能在ROS里使用雷达，`Ctrl+C`或者关闭终端将中止程序。 68 | 69 | **注意：** 70 | 71 | 在每次打开新的终端时，都需要先按照第三条添加环境变量才能进行第四条。如果希望省略第三步，可以将相应命令添加到`~/.bashrc`文件中。 72 | 73 | 在运行SDK后如果连接不到雷达，则会输出`open error`字样。 74 | 75 | 如果希望关闭终端后依然运行，可以配合`nohup`命令使之后台运行。 76 | 77 | ## 使用实例 78 | 79 | 一个使用rviz将雷达数据实时可视化的例子，请参阅[开发手册第五章](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf)。 80 | 81 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/README_en.md: -------------------------------------------------------------------------------- 1 | # SBC_ROS_SDK Direction Of Use(LD19) 2 | 3 | --- 4 | 5 | > Directory `SBC_ROS_SDK` contains the SDK that is suitable for the platform with native serial peripheral, such as Raspberry Pi, whose GPIOs can be used to power LD19 and recieve data directly. That's the reason why this version is named "SBC(Single Board Computer)". 6 | > 7 | 8 | ## General Description 9 | 10 | The contents of directory SBC_ROS_SDK` are listed below: 11 | 12 | ``` 13 | /doc/ 14 | /rviz/ 15 | /src/ 16 | /README_cn.md 17 | /README_en.md 18 | ``` 19 | 20 | + `src` contains the source code of SDK 21 | + `doc` contains the [Raspbian User manual](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf) in English (LD19 shares the same documents with LD06, pls be awarded) 22 | + `rviz` contains the file which is used to configure the visualization of radar data 23 | 24 | This SDK has been tested on Raspberry Pi 3B+ with Kinetic in Raspbian 9 and Noetic in Ubuntu 20.04 Server. 25 | 26 | ## Usage 27 | 28 | ### Hardware Preparation 29 | 30 | > LDRobot LD19 radar, wires 31 | 32 | Link LD19 to the upper computer according to the diagram below: 33 | 34 | ![schematic](../pic/en_conne.png) 35 | 36 | LD19 adopt ZH1.5T-4P 1.5mm connector. The PWM signal, namely the second pin, need not to be attached to certain GPIO if you're not going to control the revolving speed. 37 | 38 | Attatch `GND` and `P5V` to corresponding Pins on the SBC, `TX` to the UART RX GPIO of the SBC, here takes Raspberry Pi 3B+ for an example: 39 | 40 | ![rp3b+io](../pic/树莓派gpio.png) 41 | 42 | > 1(Tx)------------->10(RXD) 43 | > 44 | > 2(PWM)---------floating 45 | > 46 | > 3(GND)--------->6(GND) 47 | > 48 | > 4(P5V)---------->(5V Power) 49 | 50 | ### Software Preparation 51 | 52 | The SDK requires installed ROS, but you may encounter some problems if you manually install ROS on SBC. Advice is that you can try Ubuntu Mate or download system imager with compiled ROS. You can refer to the Manual for detailed direction of installation. 53 | 54 | Raspberry Pi's Serial peripheral needs to be enabled, otherwise there won't be corresponding tty device in directory `/dev`, refer to [chapter 4 of Manual](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf) for details. 55 | 56 | The `wiringPi` and `udev` libraries mentioned in the Manual are unnecessary. They could be ignored. 57 | 58 | The whole process could be conducted with a normal Linux user's account, however, the **authority of using the Serial device is a necessity**. 59 | 60 | 61 | 62 | So far, all the preparation is done. 63 | 64 | ### Procedure Of Use 65 | 66 | 1. Copy folder SBC_ROS_SDK/src` and `SBC_ROS_SDK/rviz` to certain empty directory A. 67 | 2. Open a terminal in A, run `catkin_make` to compile the SDK. 68 | 3. If the target has been built, run `source ./devel/setup.bash` to add some environmental variables. 69 | 4. Call `roslaunch ldlidar ld19.alaunch` at any directory to run the SDK, if there's a `Successfully` in the end of output, the LD19 has been found. 70 | 5. Keep the terminal open, the SDK should be kept running when other ROS packets 71 | 72 | **Notice** 73 | 74 | Every time you open a terminal, step 3 should be performed before step 4. There's a way to simplify it, add `source ./devel/setup.bash` to `~/.bashrc`, then you can ignore step 3 every time start a new terminal. 75 | 76 | If the SDK can't find the radar, it will print `open error`, but it won't exit. 77 | 78 | You can make use of `nohup` to let the SDK runs in the background. 79 | 80 | ## Example 81 | 82 | An example of using rviz to visualize real time data from LD19 is in the [chapter 5 of User Manual](./doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf). 83 | 84 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/SBC_ROS_SDK/doc/LDRobot_LD19_Raspbian_User_manual_V2.3.pdf -------------------------------------------------------------------------------- /SBC_ROS_SDK/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 | - /Grid1 10 | - /LaserScan1 11 | Splitter Ratio: 0.5 12 | Tree Height: 449 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 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 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1000 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Autocompute Intensity Bounds: true 54 | Autocompute Value Bounds: 55 | Max Value: 10 56 | Min Value: -10 57 | Value: true 58 | Axis: Z 59 | Channel Name: intensity 60 | Class: rviz/LaserScan 61 | Color: 255; 255; 255 62 | Color Transformer: Intensity 63 | Decay Time: 0 64 | Enabled: true 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 244 68 | Min Color: 0; 0; 0 69 | Min Intensity: 171 70 | Name: LaserScan 71 | Position Transformer: XYZ 72 | Queue Size: 10 73 | Selectable: true 74 | Size (Pixels): 5 75 | Size (m): 0.009999999776482582 76 | Style: Points 77 | Topic: /LiDAR/LD19 78 | Unreliable: false 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | Enabled: true 83 | Global Options: 84 | Background Color: 48; 48; 48 85 | Default Light: true 86 | Fixed Frame: lidar_frame 87 | Frame Rate: 30 88 | Name: root 89 | Tools: 90 | - Class: rviz/MoveCamera 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/Select 94 | - Class: rviz/SetInitialPose 95 | Topic: /initialpose 96 | - Class: rviz/SetGoal 97 | Topic: /move_base_simple/goal 98 | Value: true 99 | Views: 100 | Current: 101 | Class: rviz/Orbit 102 | Distance: 6565.43994140625 103 | Enable Stereo Rendering: 104 | Stereo Eye Separation: 0.05999999865889549 105 | Stereo Focal Distance: 1 106 | Swap Stereo Eyes: false 107 | Value: false 108 | Focal Point: 109 | X: 0 110 | Y: 0 111 | Z: 0 112 | Focal Shape Fixed Size: true 113 | Focal Shape Size: 0.05000000074505806 114 | Invert Z Axis: false 115 | Name: Current View 116 | Near Clip Distance: 0.009999999776482582 117 | Pitch: -1.5697963237762451 118 | Target Frame: 119 | Value: Orbit (rviz) 120 | Yaw: 0.234999418258667 121 | Saved: ~ 122 | Window Geometry: 123 | Displays: 124 | collapsed: false 125 | Height: 700 126 | Hide Left Dock: false 127 | Hide Right Dock: false 128 | QMainWindow State: 000000ff00000000fd00000004000000000000019500000257fc0200000006fb0000001200530065006c0065006300740069006f006e0000000028000000850000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000257000000de00fffffffb0000000a0049006d006100670065000000028d000000c60000000000000000000000010000010f00000257fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000257000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000fb0000000a007000680061007300650000000000ffffffff0000000000000000fb000000120061006d0070006c006900740075006400650000000000ffffffff00000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000028f00fffffffb0000000800540069006d00650100000000000004500000000000000000000001500000025700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 129 | Selection: 130 | collapsed: false 131 | Time: 132 | collapsed: false 133 | Tool Properties: 134 | collapsed: false 135 | Views: 136 | collapsed: false 137 | Width: 1024 138 | X: 0 139 | Y: 36 140 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ldlidar) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | sensor_msgs 13 | ) 14 | 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES ldlidar 108 | # CATKIN_DEPENDS other_catkin_pkg 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Get current directory files 124 | aux_source_directory(./src file_name) 125 | 126 | message(${file_name}) 127 | 128 | if(file_name MATCHES "slbf.cpp") 129 | add_definitions(-D"USE_SLBF") 130 | message("-- #define USE_SLBF") 131 | endif() 132 | 133 | if(file_name MATCHES "slbi.cpp") 134 | add_definitions(-D"USE_SLBI") 135 | message("-- #define USE_SLBI") 136 | endif() 137 | 138 | SET(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 139 | 140 | file(GLOB MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 141 | 142 | add_executable(ldlidar ${MAIN_SRC}) 143 | target_link_libraries(ldlidar pthread ${catkin_LIBRARIES}) 144 | 145 | 146 | ## Declare a C++ library 147 | # add_library(${PROJECT_NAME} 148 | # src/${PROJECT_NAME}/ldlidar.cpp 149 | # ) 150 | 151 | ## Add cmake target dependencies of the library 152 | ## as an example, code may need to be generated before libraries 153 | ## either from message generation or dynamic reconfigure 154 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Declare a C++ executable 157 | ## With catkin_make all packages are built within a single CMake context 158 | ## The recommended prefix ensures that target names across packages don't collide 159 | # add_executable(${PROJECT_NAME}_node src/ldlidar_node.cpp) 160 | 161 | ## Rename C++ executable without prefix 162 | ## The above recommended prefix causes long target names, the following renames the 163 | ## target back to the shorter version for ease of user use 164 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 165 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 166 | 167 | ## Add cmake target dependencies of the executable 168 | ## same as for the library above 169 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 170 | 171 | ## Specify libraries to link a library or executable target against 172 | # target_link_libraries(${PROJECT_NAME}_node 173 | # ${catkin_LIBRARIES} 174 | # ) 175 | 176 | ############# 177 | ## Install ## 178 | ############# 179 | 180 | # all install targets should use catkin DESTINATION variables 181 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 182 | 183 | ## Mark executable scripts (Python etc.) for installation 184 | ## in contrast to setup.py, you can choose the destination 185 | # install(PROGRAMS 186 | # scripts/my_python_script 187 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 188 | # ) 189 | 190 | ## Mark executables and/or libraries for installation 191 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 192 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 193 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 194 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 195 | # ) 196 | 197 | ## Mark cpp header files for installation 198 | # install(DIRECTORY include/${PROJECT_NAME}/ 199 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 200 | # FILES_MATCHING PATTERN "*.h" 201 | # PATTERN ".svn" EXCLUDE 202 | # ) 203 | 204 | ## Mark other files for installation (e.g. launch and bag files, etc.) 205 | # install(FILES 206 | # # myfile1 207 | # # myfile2 208 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 209 | # ) 210 | 211 | ############# 212 | ## Testing ## 213 | ############# 214 | 215 | ## Add gtest based cpp test target and link libraries 216 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ldlidar.cpp) 217 | # if(TARGET ${PROJECT_NAME}-test) 218 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 219 | # endif() 220 | 221 | ## Add folders to be run by python nosetests 222 | # catkin_add_nosetests(test) 223 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/launch/ld19.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ldlidar 4 | 0.0.0 5 | The ldlidar package 6 | 7 | 8 | 9 | 10 | weyne 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | pcl_conversions 53 | pcl_ros 54 | roscpp 55 | sensor_msgs 56 | pcl_conversions 57 | pcl_ros 58 | roscpp 59 | sensor_msgs 60 | pcl_conversions 61 | pcl_ros 62 | roscpp 63 | sensor_msgs 64 | libpcl-all-dev 65 | libpcl-all 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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 | #if 0 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 | #endif 148 | 149 | bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len) 150 | { 151 | static timespec timeout = { 0, (long)(100 * 1e6) }; 152 | int32_t len = -1; 153 | 154 | if (IsOpened()) 155 | { 156 | fd_set read_fds; 157 | FD_ZERO(&read_fds); 158 | FD_SET(mComHandle, &read_fds); 159 | int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL); 160 | if (r < 0) 161 | { 162 | // Select was interrupted 163 | if (errno == EINTR) 164 | { 165 | return false; 166 | } 167 | } 168 | else if (r == 0) // timeout 169 | { 170 | return false; 171 | } 172 | 173 | if (FD_ISSET(mComHandle, &read_fds)) 174 | { 175 | len = (int32_t)read(mComHandle, rx_buf, rx_buf_len); 176 | if ((len != -1) && rx_len) 177 | { 178 | *rx_len = len; 179 | } 180 | } 181 | } 182 | return len == -1 ? false : true; 183 | } 184 | 185 | 186 | bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len) 187 | { 188 | int32_t len = -1; 189 | 190 | if (IsOpened()) 191 | { 192 | len = (int32_t)write(mComHandle, tx_buf, tx_buf_len); 193 | if ((len != -1) && tx_len) 194 | { 195 | *tx_len = len; 196 | } 197 | } 198 | return len == -1 ? false : true; 199 | } 200 | 201 | 202 | void CmdInterfaceLinux::mRxThreadProc(void * param) 203 | { 204 | CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param; 205 | char * rx_buf = new char[MAX_ACK_BUF_LEN + 1]; 206 | 207 | while (!cmd_if->mRxThreadExitFlag.load()) { 208 | uint32_t readed = 0; 209 | bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); 210 | if (res && readed) { 211 | cmd_if->mRxCount += readed; 212 | if (cmd_if->mReadCallback != nullptr) 213 | { 214 | cmd_if->mReadCallback(rx_buf, readed); 215 | } 216 | } 217 | } 218 | 219 | delete[]rx_buf; 220 | } -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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 / 2300 * 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 = pkg->point[i].distance; 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 = 3.14159*2; 222 | range_min = 0.5; 223 | range_max = 15000; 224 | /*Angle resolution, the smaller the resolution, the smaller the error after conversion*/ 225 | angle_increment = ANGLE_TO_RADIAN(mSpeed/2300); 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 = "lidar_frame"; 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)((angle - output.angle_min) / output.angle_increment); 248 | if (index >= 0 && index < beam_size) 249 | { 250 | /*If the current content is Nan, it is assigned directly*/ 251 | if (std::isnan(output.ranges[index])) 252 | { 253 | output.ranges[index] = range; 254 | } 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 | { 259 | output.ranges[index] = range; 260 | } 261 | } 262 | output.intensities[index] = point.confidence; 263 | } 264 | } 265 | } 266 | 267 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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 | uint16_t distance; 51 | uint8_t confidence; 52 | double x; 53 | double y; 54 | PointData(float angle, uint16_t 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 | 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 | void ToLaserscan(std::vector src); 97 | }; 98 | #endif 99 | /********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/ 100 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | //#include 6 | //#include 7 | #include "cmd_interface_linux.h" 8 | #include 9 | #include 10 | #include "lipkg.h" 11 | #include "tofbf.h" 12 | #include "signal.h" 13 | #include "pid.h" 14 | 15 | int main(int argc, char **argv) 16 | { 17 | ros::init(argc, argv, "product"); 18 | ros::NodeHandle nh; /* create a ROS Node */ 19 | 20 | 21 | //int32_t pwm_out = 0; 22 | //PIDObjTyp LidarMotorPID(500, 50, 200, 0, 100, 0); 23 | //wiringPiSetup(); 24 | //set pwm out frequency 24kHz 25 | //pinMode(LIDAR_PWM, PWM_OUTPUT); 26 | //pwmSetClock(8); 27 | //pwmSetMode(PWM_MODE_MS); 28 | //pwmSetRange(100); 29 | //pwmWrite(LIDAR_PWM, 70); 30 | 31 | LiPkg * lidar = new LiPkg; 32 | 33 | CmdInterfaceLinux cmd_port; 34 | std::string port_name("/dev/ttyS0"); 35 | cmd_port.SetReadCallback([&lidar](const char *byte, size_t len) { 36 | if (lidar->Parse((uint8_t*)byte, len)) 37 | { 38 | lidar->AssemblePacket(); 39 | } 40 | }); 41 | 42 | if(cmd_port.Open(port_name)) 43 | std::cout<<"LiDAR_LD19 started successfully " <("LiDAR/LD19", 1); /*create a ROS topic */ 46 | 47 | while (ros::ok()) 48 | { 49 | if (lidar->IsFrameReady()) 50 | { 51 | lidar_pub.publish(lidar->GetLaserScan()); // Fixed Frame: lidar_frame 52 | lidar->ResetFrameReady(); 53 | } 54 | 55 | } 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/src/pid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pid.h 3 | * @author LD Robot 4 | * @version V01 5 | 6 | * @brief 7 | * @note 8 | * @attention COPYRIGHT LDROBOT 9 | **/ 10 | 11 | #ifndef __PID_H__ 12 | #define __PID_H__ 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "lipkg.h" 18 | #include 19 | #include 20 | #include 21 | 22 | #define LIDAR_PWM 1 23 | 24 | struct PIDObjTyp { 25 | int32_t kp; 26 | int32_t ki; 27 | int32_t kd; 28 | int64_t ki_plus; 29 | int32_t outabslimit; 30 | int32_t remainder; 31 | int32_t lasterr[2]; 32 | int64_t max_ki_plus; 33 | 34 | PIDObjTyp() 35 | { 36 | memset(this, 0, sizeof(PIDObjTyp)); 37 | } 38 | 39 | PIDObjTyp(int32_t kp, int32_t ki, int32_t kd, int32_t ki_plus, 40 | int32_t outabslimit, int64_t max_ki_plus) 41 | { 42 | memset(this, 0, sizeof(PIDObjTyp)); 43 | 44 | this->kp = kp; 45 | this->ki = ki; 46 | this->kd = kd; 47 | this->ki_plus = ki_plus; 48 | this->outabslimit = outabslimit; 49 | this->max_ki_plus = max_ki_plus; 50 | } 51 | 52 | void Reset() 53 | { 54 | lasterr[0] = 0; 55 | lasterr[1] = 0; 56 | ki_plus = 0; 57 | remainder = 0; 58 | } 59 | 60 | int32_t PIDRegulatorS32(int32_t aim, int32_t cur, int32_t out) 61 | { 62 | int64_t out_value = 0; 63 | int32_t err = 0, ret = 0; 64 | 65 | err = aim - cur; 66 | 67 | out_value = (int64_t)kp * (int64_t)(err - lasterr[0]); 68 | out_value += (int64_t)ki * (int64_t)(err); 69 | out_value += (int64_t)kd * (int64_t)(err - 2 * lasterr[0] + lasterr[1]); 70 | 71 | lasterr[1] = lasterr[0]; 72 | lasterr[0] = err; 73 | 74 | out_value += remainder; 75 | 76 | if (out_value < 0) 77 | { 78 | ret = (out_value >> 16) + 1 + out; 79 | remainder = (int32_t)out_value | 0xffff0000; 80 | } 81 | else 82 | { 83 | ret = (out_value >> 16) + out; 84 | remainder = (int32_t)out_value & 0x0000ffff; 85 | } 86 | 87 | 88 | if (ret > outabslimit) 89 | { 90 | return outabslimit; 91 | } 92 | else if (ret < -outabslimit) 93 | { 94 | return -outabslimit; 95 | } 96 | else 97 | { 98 | return ret; 99 | } 100 | } 101 | }; 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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 | if(n.distance > 20) 46 | { 47 | normal.push_back(n); 48 | } 49 | } 50 | } 51 | 52 | if (tmp.empty()) 53 | return normal; 54 | 55 | double angle_delta_up_limit = curr_speed / SCAN_FRE * 2; 56 | 57 | 58 | std::sort(pending.begin(), pending.end(), [](PointData a, PointData b) { return a.angle < b.angle; }); 59 | 60 | PointData last(-10, 0, 0); 61 | 62 | for (auto n : pending) 63 | { 64 | if (abs(n.angle - last.angle) > angle_delta_up_limit || abs(n.distance - last.distance) > last.distance*0.03) 65 | { 66 | if(item.empty() == false) 67 | { 68 | group.push_back(item); 69 | item.clear(); 70 | } 71 | } 72 | item.push_back(n); 73 | last = n; 74 | } 75 | 76 | if (item.empty() == false) 77 | group.push_back(item); 78 | 79 | if (group.empty()) 80 | return normal; 81 | 82 | 83 | auto first_item = group.front().front(); 84 | auto last_item = group.back().back(); 85 | 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) 86 | { 87 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 88 | group.erase(group.end() - 1); 89 | } 90 | 91 | for (auto n : group) 92 | { 93 | if (n.size() == 0) 94 | continue; 95 | 96 | if (n.size() > 15) 97 | { 98 | normal.insert(normal.end(), n.begin(), n.end()); 99 | continue; 100 | } 101 | 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 | double confidence_avg = 0; 115 | double dis_avg = 0; 116 | for (auto m : n) 117 | { 118 | confidence_avg += m.confidence; 119 | dis_avg += m.distance; 120 | } 121 | confidence_avg /= n.size(); 122 | dis_avg /= n.size(); 123 | 124 | if(dis_avg < 250 && confidence_avg < CONFIDENCE_LOW_NEAR) 125 | { 126 | continue; 127 | } 128 | 129 | if (confidence_avg > CONFIDENCE_LOW) 130 | { 131 | normal.insert(normal.end(), n.begin(), n.end()); 132 | continue; 133 | } 134 | 135 | } 136 | 137 | return normal; 138 | } 139 | 140 | -------------------------------------------------------------------------------- /SBC_ROS_SDK/src/ldlidar/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_LOW_NEAR = 100; /* Low confidence threshold */ 13 | const int CONFIDENCE_SINGLE = 220; /* Discrete points require higher confidence */ 14 | const int SCAN_FRE = 4500; /* Default scan frequency, to change, read according to radar protocol */ 15 | double offset_x, offset_y; 16 | double curr_speed; 17 | Tofbf() = delete; 18 | Tofbf(const Tofbf &) = delete; 19 | Tofbf &operator=(const Tofbf &) = delete; 20 | public: 21 | Tofbf(int speed); 22 | std::vector NearFilter(const std::vector &tmp) const; 23 | ~Tofbf(); 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /datasheet/LDROBOT_LD19_Datasheet_V1.0.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/datasheet/LDROBOT_LD19_Datasheet_V1.0.pdf -------------------------------------------------------------------------------- /datasheet/LDROBOT_LD19_数据手册_v1.1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/datasheet/LDROBOT_LD19_数据手册_v1.1.pdf -------------------------------------------------------------------------------- /pic/en_conne.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/pic/en_conne.png -------------------------------------------------------------------------------- /pic/image-20210802170439262.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/pic/image-20210802170439262.png -------------------------------------------------------------------------------- /pic/image-20210803135324723.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/pic/image-20210803135324723.png -------------------------------------------------------------------------------- /pic/文件结构.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/pic/文件结构.png -------------------------------------------------------------------------------- /pic/树莓派gpio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ldrobotSensorTeam/ld19/505da4769b04b190062807039a16eb3a154dbe1e/pic/树莓派gpio.png --------------------------------------------------------------------------------