├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── ros_api.h ├── launch ├── ld06.launch ├── ld19.launch ├── viewer_ld06_kinetic_melodic.launch ├── viewer_ld06_noetic.launch ├── viewer_ld19_kinetic_melodic.launch └── viewer_ld19_noetic.launch ├── ldlidar_driver ├── include │ ├── core │ │ ├── ldlidar_datatype.h │ │ └── ldlidar_driver.h │ ├── dataprocess │ │ └── lipkg.h │ ├── filter │ │ └── tofbf.h │ ├── logger │ │ └── log_module.h │ ├── networkcom │ │ └── network_socket_interface_linux.h │ └── serialcom │ │ └── serial_interface_linux.h └── src │ ├── core │ └── ldlidar_driver.cpp │ ├── dataprocess │ └── lipkg.cpp │ ├── filter │ └── tofbf.cpp │ ├── logger │ └── log_module.cpp │ ├── networkcom │ └── network_socket_interface_linux.cpp │ └── serialcom │ └── serial_interface_linux.cpp ├── package.xml ├── rules └── ldlidar.rules ├── rviz ├── ldlidar_kinetic_melodic.rviz └── ldlidar_noetic.rviz ├── scripts ├── create_udev_rules.sh └── delete_udev_rules.sh └── src ├── listen_node └── listen_node.cpp └── publish_node └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.json 2 | *.zip -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ldlidar_stl_ros) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | set (IS_WARLL "OFF") # "ON" or "OFF" 6 | if(${IS_WARLL} MATCHES "OFF") 7 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 8 | message(STATUS "optional:-std=c++11") 9 | else() 10 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wall -Wextra ${CMAKE_CXX_FLAGS}") 11 | message(STATUS "optional:-std=c++11 -Wall -Wextra ") 12 | endif() 13 | 14 | # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g ") 15 | # set (CMAKE_VERBOSE_MAKEFILE ON) 16 | 17 | ## Find catkin macros and libraries 18 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 19 | ## is used, also find other catkin packages 20 | find_package(catkin REQUIRED COMPONENTS 21 | roscpp 22 | sensor_msgs 23 | ) 24 | 25 | ## System dependencies are found with CMake's conventions 26 | # find_package(Boost REQUIRED COMPONENTS system) 27 | 28 | 29 | ## Uncomment this if the package has a setup.py. This macro ensures 30 | ## modules and global scripts declared therein get installed 31 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 32 | # catkin_python_setup() 33 | 34 | ################################################ 35 | ## Declare ROS messages, services and actions ## 36 | ################################################ 37 | 38 | ## To declare and build messages, services or actions from within this 39 | ## package, follow these steps: 40 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 41 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 42 | ## * In the file package.xml: 43 | ## * add a build_depend tag for "message_generation" 44 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 45 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 46 | ## but can be declared for certainty nonetheless: 47 | ## * add a exec_depend tag for "message_runtime" 48 | ## * In this file (CMakeLists.txt): 49 | ## * add "message_generation" and every package in MSG_DEP_SET to 50 | ## find_package(catkin REQUIRED COMPONENTS ...) 51 | ## * add "message_runtime" and every package in MSG_DEP_SET to 52 | ## catkin_package(CATKIN_DEPENDS ...) 53 | ## * uncomment the add_*_files sections below as needed 54 | ## and list every .msg/.srv/.action file to be processed 55 | ## * uncomment the generate_messages entry below 56 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 57 | 58 | ## Generate messages in the 'msg' folder 59 | # add_message_files( 60 | # FILES 61 | # Message1.msg 62 | # Message2.msg 63 | # ) 64 | 65 | ## Generate services in the 'srv' folder 66 | # add_service_files( 67 | # FILES 68 | # Service1.srv 69 | # Service2.srv 70 | # ) 71 | 72 | ## Generate actions in the 'action' folder 73 | # add_action_files( 74 | # FILES 75 | # Action1.action 76 | # Action2.action 77 | # ) 78 | 79 | ## Generate added messages and services with any dependencies listed here 80 | # generate_messages( 81 | # DEPENDENCIES 82 | # std_msgs # Or other packages containing msgs 83 | # ) 84 | 85 | ################################################ 86 | ## Declare ROS dynamic reconfigure parameters ## 87 | ################################################ 88 | 89 | ## To declare and build dynamic reconfigure parameters within this 90 | ## package, follow these steps: 91 | ## * In the file package.xml: 92 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 93 | ## * In this file (CMakeLists.txt): 94 | ## * add "dynamic_reconfigure" to 95 | ## find_package(catkin REQUIRED COMPONENTS ...) 96 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 97 | ## and list every .cfg file to be processed 98 | 99 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 100 | # generate_dynamic_reconfigure_options( 101 | # cfg/DynReconf1.cfg 102 | # cfg/DynReconf2.cfg 103 | # ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if your package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | # INCLUDE_DIRS include 116 | # LIBRARIES ldlidar 117 | # CATKIN_DEPENDS other_catkin_pkg 118 | # DEPENDS system_lib 119 | ) 120 | 121 | ########### 122 | ## Build ## 123 | ########### 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | include_directories( 128 | ${CMAKE_CURRENT_SOURCE_DIR}/include 129 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/core/ 130 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/dataprocess/ 131 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/filter/ 132 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/logger/ 133 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/networkcom/ 134 | ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/include/serialcom/ 135 | ${catkin_INCLUDE_DIRS} 136 | ) 137 | 138 | file(GLOB LDLIDAR_DRI_CORE ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/core/*.cpp) 139 | file(GLOB LDLIDAR_DRI_DATARPC ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/dataprocess/*.cpp) 140 | file(GLOB LDLIDAR_DRI_FILTER ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/filter/*.cpp) 141 | file(GLOB LDLIDAR_DRI_LOGGER ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/logger/*.cpp) 142 | file(GLOB LDLIDAR_DRI_NETWORK ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/networkcom/*.cpp) 143 | file(GLOB LDLIDAR_DRI_SERIAL ${CMAKE_CURRENT_SOURCE_DIR}/ldlidar_driver/src/serialcom/*.cpp) 144 | 145 | file(GLOB MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/publish_node/*.cpp) 146 | file(GLOB LISTEN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/listen_node/*.cpp) 147 | 148 | add_executable(${PROJECT_NAME}_node 149 | ${MAIN_SRC} 150 | ${LDLIDAR_DRI_CORE} 151 | ${LDLIDAR_DRI_DATARPC} 152 | ${LDLIDAR_DRI_FILTER} 153 | ${LDLIDAR_DRI_LOGGER} 154 | ${LDLIDAR_DRI_NETWORK} 155 | ${LDLIDAR_DRI_SERIAL} 156 | ) 157 | target_link_libraries(${PROJECT_NAME}_node pthread ${catkin_LIBRARIES}) 158 | 159 | add_executable(${PROJECT_NAME}_listen_node 160 | ${LISTEN_SRC} 161 | ) 162 | target_link_libraries(${PROJECT_NAME}_listen_node ${catkin_LIBRARIES}) 163 | 164 | ## Declare a C++ library 165 | # add_library(${PROJECT_NAME} 166 | # src/${PROJECT_NAME}/ldlidar.cpp 167 | # ) 168 | 169 | ## Add cmake target dependencies of the library 170 | ## as an example, code may need to be generated before libraries 171 | ## either from message generation or dynamic reconfigure 172 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 173 | 174 | ## Declare a C++ executable 175 | ## With catkin_make all packages are built within a single CMake context 176 | ## The recommended prefix ensures that target names across packages don't collide 177 | # add_executable(${PROJECT_NAME}_node src/ldlidar_node.cpp) 178 | 179 | ## Rename C++ executable without prefix 180 | ## The above recommended prefix causes long target names, the following renames the 181 | ## target back to the shorter version for ease of user use 182 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 183 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 184 | 185 | ## Add cmake target dependencies of the executable 186 | ## same as for the library above 187 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 188 | 189 | ## Specify libraries to link a library or executable target against 190 | # target_link_libraries(${PROJECT_NAME}_node 191 | # ${catkin_LIBRARIES} 192 | # ) 193 | 194 | ############# 195 | ## Install ## 196 | ############# 197 | 198 | # all install targets should use catkin DESTINATION variables 199 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 200 | 201 | ## Mark executable scripts (Python etc.) for installation 202 | ## in contrast to setup.py, you can choose the destination 203 | # install(PROGRAMS 204 | # scripts/my_python_script 205 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 206 | # ) 207 | install(PROGRAMS 208 | scripts/create_udev_rules.sh 209 | scripts/delete_udev_rules.sh 210 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 211 | ) 212 | 213 | ## Mark executables and/or libraries for installation 214 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 215 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 216 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 217 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 218 | # ) 219 | install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_listen_node 220 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 221 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 222 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 223 | ) 224 | 225 | ## Mark cpp header files for installation 226 | # install(DIRECTORY include/${PROJECT_NAME}/ 227 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 228 | # FILES_MATCHING PATTERN "*.h" 229 | # PATTERN ".svn" EXCLUDE 230 | # ) 231 | 232 | ## Mark other files for installation (e.g. launch and bag files, etc.) 233 | # install(FILES 234 | # # myfile1 235 | # # myfile2 236 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 237 | # ) 238 | install(DIRECTORY launch rules rviz 239 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 240 | ) 241 | 242 | ############# 243 | ## Testing ## 244 | ############# 245 | 246 | ## Add gtest based cpp test target and link libraries 247 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ldlidar.cpp) 248 | # if(TARGET ${PROJECT_NAME}-test) 249 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 250 | # endif() 251 | 252 | ## Add folders to be run by python nosetests 253 | # catkin_add_nosetests(test) 254 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 SHENZHEN LDROBOT CO., LTD. All rights 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | - [cn](#操作指南) 2 | - [en](#Instructions) 3 | # 操作指南 4 | 5 | >此SDK仅适用于深圳乐动机器人有限公司销售的激光雷达产品，产品型号为: 6 | > - LDROBOT LiDAR LD06 7 | > - LDROBOT LiDAR LD19 8 | ## 0. 获取雷达的ROS功能包 9 | ```bash 10 | cd ~ 11 | 12 | mkdir -p ldlidar_ros_ws/src 13 | 14 | cd ldlidar_ros_ws/src 15 | 16 | git clone https://github.com/ldrobotSensorTeam/ldlidar_stl_ros.git 17 | # 或者 18 | git clone https://gitee.com/ldrobotSensorTeam/ldlidar_stl_ros.git 19 | ``` 20 | 21 | ## 1. 系统设置 22 | - 第一步，通过板载串口或者USB转串口模块(例如,cp2102模块)的方式使雷达连接到你的系统主板. 23 | - 第二步，设置雷达在系统中挂载的串口设备-x权限(以/dev/ttyUSB0为例) 24 | - 实际使用时，根据雷达在你的系统中的实际挂载情况来设置，可以使用`ls -l /dev`命令查看. 25 | 26 | ``` bash 27 | cd ~/ldlidar_ros_ws 28 | 29 | sudo chmod 777 /dev/ttyUSB0 30 | ``` 31 | - 第三步，修改`launch/`目录下雷达产品型号对应的lanuch文件中的`port_name`值，以ld06.launch为例，如下所示. 32 | 33 | ```xml 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 67 | 68 | 69 | 70 | 71 | ``` 72 | ## 2. 编译方法 73 | - 安装src/目录下功能包的依赖 74 | ```bash 75 | cd ~/ldlidar_ros_ws 76 | 77 | rosdep install --from-paths src --ignore-src -r -y 78 | ``` 79 | - 使用catkin编译 80 | ```bash 81 | cd ~/ldlidar_ros_ws 82 | 83 | catkin_make 84 | ``` 85 | ## 3. 运行方法 86 | 87 | ### 3.1. 设置功能包环境变量 88 | 89 | - 编译完成后需要将编译生成的相关文件加入环境变量，便于 ROS 环境可以识别， 执行命令如下所示， 该命令是临时给终端加入环境变量，意味着您如果重 90 | 新打开新的终端，也需要重新执行如下命令. 91 | 92 | ```bash 93 | cd ~/ldlidar_ros_ws 94 | 95 | source devel/setup.bash 96 | ``` 97 | - 为了重新打开终端后，永久不用执行上述添加环境变量的命令，可以进行如下操作. 98 | 99 | ```bash 100 | echo "source ~/ldlidar_ros_ws/devel/setup.bash" >> ~/.bashrc 101 | 102 | source ~/.bashrc 103 | ``` 104 | ### 3.2. 启动激光雷达节点 105 | 106 | - 产品型号为 LDROBOT LiDAR LD06 107 | - 启动ld06 lidar node: 108 | ``` bash 109 | roslaunch ldlidar_stl_ros ld06.launch 110 | ``` 111 | - 启动ld06 lidar node并显示激光数据在Rviz上: 112 | ``` bash 113 | # if ROS_DISTRO in 'kinetic' or 'melodic' 114 | roslaunch ldlidar_stl_ros viewer_ld06_kinetic_melodic.launch 115 | # if ROS_DISTRO in 'noetic' 116 | roslaunch ldlidar_stl_ros viewer_ld06_noetic.launch 117 | ``` 118 | 119 | - 产品型号为 LDROBOT LiDAR LD19 120 | - 启动ld19 lidar node: 121 | ``` bash 122 | roslaunch ldlidar_stl_ros ld19.launch 123 | ``` 124 | - 启动ld19 lidar node并显示激光数据在Rviz上: 125 | ``` bash 126 | # if ROS_DISTRO in 'kinetic' or 'melodic' 127 | roslaunch ldlidar_stl_ros viewer_ld19_kinetic_melodic.launch 128 | # if ROS_DISTRO in 'noetic' 129 | roslaunch ldlidar_stl_ros viewer_ld19_noetic.launch 130 | ``` 131 | 132 | ## 4. 测试 133 | 134 | > 代码支持ubuntu16.04 ROS kinetic、ubuntu18.04 ROS melodic、ubuntu20.04 ROS noetic版本下测试，使用rviz可视化。 135 | 136 | - 新打开一个终端 (Ctrl + Alt + T),并通过Rviz工具打开readme文件所在目录的rviz文件夹下面的rviz配置文件 137 | ```bash 138 | rviz 139 | ``` 140 | # Instructions 141 | 142 | > This SDK is only applicable to the LiDAR products sold by Shenzhen LDROBOT Co., LTD. The product models are : 143 | > - LDROBOT LiDAR LD06 144 | > - LDROBOT LiDAR LD19 145 | ## step0: get LiDAR ROS Package 146 | ```bash 147 | cd ~ 148 | 149 | mkdir -p ldlidar_ros_ws/src 150 | 151 | cd ldlidar_ros_ws/src 152 | 153 | git clone https://github.com/ldrobotSensorTeam/ldlidar_stl_ros.git 154 | # or 155 | git clone https://gitee.com/ldrobotSensorTeam/ldlidar_stl_ros.git 156 | ``` 157 | ## step 1: system setup 158 | - Connect the LiDAR to your system motherboard via an onboard serial port or usB-to-serial module (for example, CP2102 module). 159 | 160 | - Set the -x permission for the serial port device mounted by the radar in the system (for example, /dev/ttyUSB0) 161 | 162 | - In actual use, the LiDAR can be set according to the actual mounted status of your system, you can use 'ls -l /dev' command to view. 163 | 164 | ``` bash 165 | cd ~/ldlidar_ros_ws 166 | 167 | sudo chmod 777 /dev/ttyUSB0 168 | ``` 169 | - Modify the `port_name` value in the Lanuch file corresponding to the radar product model under `launch/`, using `ld06.launch` as an example, as shown below. 170 | 171 | ``` xml 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 205 | 206 | 207 | 208 | 209 | ``` 210 | ## step 2: build 211 | - install package depend 212 | ```bash 213 | cd ~/ldlidar_ros_ws 214 | 215 | rosdep install --from-paths src --ignore-src -r -y 216 | ``` 217 | 218 | - build compile 219 | ```bash 220 | cd ~/ldlidar_ros_ws 221 | 222 | catkin_make 223 | ``` 224 | ## step 3: run 225 | 226 | ### step3.1: package environment variable settings 227 | 228 | - After the compilation is completed, you need to add the relevant files generated by the compilation to the environment variables, so that the ROS environment can recognize them. The execution command is as follows. This command is to temporarily add environment variables to the terminal, which means that if you reopen a new terminal, you also need to re-execute it. The following command. 229 | 230 | ```bash 231 | cd ~/ldlidar_ros_ws 232 | 233 | source devel/setup.bash 234 | ``` 235 | 236 | - In order to never need to execute the above command to add environment variables after reopening the terminal, you can do the following. 237 | 238 | ```bash 239 | echo "source ~/ldlidar_ros_ws/devel/setup.bash" >> ~/.bashrc 240 | 241 | source ~/.bashrc 242 | ``` 243 | ### step3.2: start LiDAR node 244 | 245 | - The product is LDROBOT LiDAR LD06 246 | - start ld06 lidar node: 247 | ``` bash 248 | roslaunch ldlidar_stl_ros ld06.launch 249 | ``` 250 | - start ld06 lidar node and show on the Rviz: 251 | ``` bash 252 | # if ROS_DISTRO in 'kinetic' or 'melodic' 253 | roslaunch ldlidar_stl_ros viewer_ld06_kinetic_melodic.launch 254 | # if ROS_DISTRO in 'noetic' 255 | roslaunch ldlidar_stl_ros viewer_ld06_noetic.launch 256 | ``` 257 | 258 | - The product is LDROBOT LiDAR LD19 259 | - start ld19 lidar node: 260 | ``` bash 261 | roslaunch ldlidar_stl_ros ld19.launch 262 | ``` 263 | - start ld19 lidar node and show on the Rviz: 264 | ``` bash 265 | # if ROS_DISTRO in 'kinetic' or 'melodic' 266 | roslaunch ldlidar_stl_ros viewer_ld19_kinetic_melodic.launch 267 | # if ROS_DISTRO in 'noetic' 268 | roslaunch ldlidar_stl_ros viewer_ld19_noetic.launch 269 | ``` 270 | 271 | ## step 4: test 272 | 273 | > The code was tested under ubuntu16.04 ROS kinetic、ubuntu18.04 ROS melodic、ubuntu20.04 ROS noetic, using rviz visualization. 274 | 275 | - new a terminal (Ctrl + Alt + T) and use Rviz tool,open the rviz config file below the rviz folder of the readme file directory 276 | ```bash 277 | rviz 278 | ``` -------------------------------------------------------------------------------- /include/ros_api.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ros_api.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ros lib interface 5 | * This code is only applicable to LDROBOT LiDAR products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __ROS_API_H__ 22 | #define __ROS_API_H__ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | struct LaserScanSetting 29 | { 30 | std::string frame_id; 31 | bool laser_scan_dir; 32 | bool enable_angle_crop_func; 33 | double angle_crop_min; 34 | double angle_crop_max; 35 | }; 36 | 37 | #endif //__ROS_API_H__ 38 | 39 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 40 | * FILE ********/ -------------------------------------------------------------------------------- /launch/ld06.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/ld19.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/viewer_ld06_kinetic_melodic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/viewer_ld06_noetic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/viewer_ld19_kinetic_melodic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/viewer_ld19_noetic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /ldlidar_driver/include/core/ldlidar_datatype.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pointdata.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief lidar point data structure 5 | * This code is only applicable to LDROBOT products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-09 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef _LDLIDAR_POINT_DATA_H_ 22 | #define _LDLIDAR_POINT_DATA_H_ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000) 30 | #define RADIAN_TO_ANGLED(angle) ((angle)*180000 / 3141.59) 31 | 32 | namespace ldlidar { 33 | 34 | enum class LDType { 35 | NO_VERSION, 36 | LD_06, 37 | LD_19, 38 | STL_06P, 39 | STL_26, 40 | STL_27L, 41 | }; 42 | 43 | enum class LidarStatus { 44 | NORMAL, // 雷达正常,可获取点云数据 45 | ERROR, // 表明雷达出现异常错误，可获取雷达反馈的错误码了解具体错误，具体错误由错误码对应二进制值的对应位决定 46 | DATA_TIME_OUT, // 雷达点云数据包发布超时 47 | DATA_WAIT, // 雷达点云数据包发布等待 48 | STOP, // 未启动Driver 49 | }; 50 | 51 | struct PointData { 52 | // Polar coordinate representation 53 | float angle; // Angle ranges from 0 to 359 degrees 54 | uint16_t distance; // Distance is measured in millimeters 55 | uint8_t intensity; // Intensity is 0 to 255 56 | //! System time when first range was measured in nanoseconds 57 | uint64_t stamp; 58 | // Cartesian coordinate representation 59 | double x; 60 | double y; 61 | PointData(float angle, uint16_t distance, uint8_t intensity, uint64_t stamp = 0, double x = 0, 62 | double y = 0) { 63 | this->angle = angle; 64 | this->distance = distance; 65 | this->intensity = intensity; 66 | this->stamp = stamp; 67 | this->x = x; 68 | this->y = y; 69 | } 70 | PointData() {} 71 | }; 72 | 73 | typedef std::vector Points2D; 74 | 75 | struct LaserScan { 76 | //! System time when first range was measured in nanoseconds 77 | uint64_t stamp; 78 | //! Array of laser point 79 | Points2D points; 80 | 81 | LaserScan &operator=(const LaserScan &data) { 82 | this->stamp = data.stamp; 83 | this->points = data.points; 84 | return *this; 85 | } 86 | }; 87 | 88 | } // namespace ldlidar 89 | 90 | #endif // _POINT_DATA_H_ 91 | 92 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 93 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/core/ldlidar_driver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar processing App 5 | * This code is only applicable to LDROBOT LiDAR LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-05-12 9 | * 10 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #ifndef __LDLIDAR_NODE_H__ 22 | #define __LDLIDAR_NODE_H__ 23 | 24 | #include 25 | #include 26 | 27 | #include "serial_interface_linux.h" 28 | #include "network_socket_interface_linux.h" 29 | #include "lipkg.h" 30 | #include "log_module.h" 31 | 32 | namespace ldlidar { 33 | 34 | typedef enum CommunicationMode { 35 | COMM_NO_NULL, 36 | COMM_SERIAL_MODE, /* serial communication */ 37 | COMM_UDP_CLIENT_MODE, /* network communication for UDP client */ 38 | COMM_UDP_SERVER_MODE, /* network communication for UDP server */ 39 | COMM_TCP_CLIENT_MODE, /* network communication for TCP client */ 40 | COMM_TCP_SERVER_MODE /* network communication for TCP server */ 41 | }CommunicationModeTypeDef; 42 | 43 | class LDLidarDriver { 44 | public: 45 | LDLidarDriver(); 46 | ~LDLidarDriver(); 47 | 48 | /** 49 | * @brief get lidar sdk version number 50 | * @param none 51 | * @retval string type, for example is "v2.3.0" 52 | */ 53 | std::string GetLidarSdkVersionNumber(void); 54 | 55 | /** 56 | * @brief start lidar device handle node 57 | * @param product_name 58 | * ldlidar product type: ldlidar::LDType, value: 59 | * - ldlidar::LDType::NOVER 60 | * - ldlidar::LDType::LD_06 61 | * .... 62 | * - else definition in "ldlidar_driver/include/ldlidar_datatype.h" 63 | * @param serial_port_name 64 | * serial device system path, eg: "/dev/ttyUSB0" 65 | * @param serial_baudrate 66 | * serial communication baudrate value(> 0), unit is bit/s. 67 | * @retval value is true, start is success; 68 | * value is false, start is failed. 69 | */ 70 | bool Start(LDType product_name, 71 | std::string serial_port_name, 72 | uint32_t serial_baudrate = 115200, 73 | CommunicationModeTypeDef comm_mode = COMM_SERIAL_MODE); 74 | 75 | bool Start(LDType product_name, 76 | const char* server_ip, 77 | const char* server_port, 78 | CommunicationModeTypeDef comm_mode = COMM_TCP_CLIENT_MODE); 79 | 80 | /** 81 | * @brief stop lidar device handle node 82 | * @param none 83 | * @retval value is true, stop is success; 84 | * value is false, stop is failed. 85 | */ 86 | bool Stop(void); 87 | 88 | /** 89 | * @brief Whether the connection of the communication channel is normal after the lidar is powered on 90 | * @param[in] 91 | * *@param timeout: Wait timeout, in milliseconds 92 | * @retval if times >= timeout, return false, communication connection is fail; 93 | * if "times < timeout", return ture, communication connection is successful. 94 | */ 95 | bool WaitLidarCommConnect(int64_t timeout); 96 | 97 | /** 98 | * @brief get lidar laser scan point cloud data 99 | * @param [output] 100 | * *@param dst: type is ldlidar::Point2D, value is laser scan point cloud data 101 | * @param [in] 102 | * *@param timeout: Wait timeout, in milliseconds 103 | * @retval value is ldlidar::LidarStatus Enum Type, definition in "ldlidar_driver/include/ldlidar_datatype.h", value is: 104 | * ldlidar::LidarStatus::NORMAL // 雷达正常 105 | * lldlidar::LidarStatus::ERROR // 雷达异常错误 106 | * .... 107 | */ 108 | LidarStatus GetLaserScanData(Points2D& dst, int64_t timeout = 1000); 109 | 110 | LidarStatus GetLaserScanData(LaserScan& dst, int64_t timeout = 1000); 111 | 112 | /** 113 | * @brief get lidar laser scan frequence 114 | * @param [output] 115 | * *@param spin_hz: value unit is hz. 116 | * @retval value is true, get lidar laser scan frequence is success; 117 | * value is false, get lidar laser scan ferquence is fail. 118 | */ 119 | bool GetLidarScanFreq(double& spin_hz); 120 | 121 | /** 122 | * @brief register get timestamp handle functional. 123 | * @param [input] 124 | * *@param get_timestamp_handle: type is `uint64_t (*)(void)`, value is pointer get timestamp fuction. 125 | * @retval none 126 | */ 127 | void RegisterGetTimestampFunctional(std::function get_timestamp_handle); 128 | 129 | /** 130 | * @brief open or close filter algorithnm process 131 | * @param [input] 132 | * *@param is_enable: value is true, open filter process; 133 | * value is false, close filter process. 134 | * @retval none 135 | */ 136 | void EnableFilterAlgorithnmProcess(bool is_enable); 137 | 138 | /** 139 | * @brief When the lidar is in an error state, get the error code fed back by the lidar 140 | * @param none 141 | * @retval errcode 142 | */ 143 | // uint8_t GetLidarErrorCode(void); 144 | 145 | static bool IsOk() { return is_ok_; } 146 | 147 | static void SetIsOkStatus(bool status) { is_ok_ = status;} 148 | 149 | private: 150 | std::string sdk_version_number_; 151 | static bool is_ok_; 152 | bool is_start_flag_; 153 | std::function register_get_timestamp_handle_; 154 | LiPkg* comm_pkg_; 155 | SerialInterfaceLinux* comm_serial_; 156 | TCPSocketInterfaceLinux* comm_tcp_network_; 157 | UDPSocketInterfaceLinux* comm_udp_network_; 158 | std::chrono::_V2::steady_clock::time_point last_pubdata_times_; 159 | }; 160 | 161 | } // namespace ldlidar 162 | 163 | #endif // __LDLIDAR_DRIVER_H__ 164 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 165 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/dataprocess/lipkg.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR data protocol processing App 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef __LIPKG_H 23 | #define __LIPKG_H 24 | 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "ldlidar_datatype.h" 33 | #include "tofbf.h" 34 | 35 | namespace ldlidar { 36 | 37 | enum { 38 | PKG_HEADER = 0x54, 39 | PKG_VER_LEN = 0x2C, 40 | POINT_PER_PACK = 12, 41 | }; 42 | 43 | typedef struct __attribute__((packed)) { 44 | uint16_t distance; 45 | uint8_t intensity; 46 | } LidarPointStructDef; 47 | 48 | typedef struct __attribute__((packed)) { 49 | uint8_t header; 50 | uint8_t ver_len; 51 | uint16_t speed; 52 | uint16_t start_angle; 53 | LidarPointStructDef point[POINT_PER_PACK]; 54 | uint16_t end_angle; 55 | uint16_t timestamp; 56 | uint8_t crc8; 57 | } LiDARFrameTypeDef; 58 | 59 | class LiPkg { 60 | public: 61 | LiPkg(); 62 | ~LiPkg(); 63 | 64 | // set product type (belong to enum class LDType) 65 | void SetProductType(LDType type_number); 66 | // get Lidar spin speed (Hz) 67 | double GetSpeed(void); 68 | // get lidar spind speed (degree per second) origin 69 | uint16_t GetSpeedOrigin(void); 70 | // get time stamp of the packet 71 | uint16_t GetTimestamp(void); 72 | // get lidar measure frequence(Hz) 73 | int GetLidarMeasurePointFrequence(void); 74 | 75 | void CommReadCallback(const char *byte, size_t len); 76 | 77 | bool GetLaserScanData(Points2D& out); 78 | 79 | void RegisterTimestampGetFunctional(std::function timestamp_handle); 80 | 81 | bool GetLidarPowerOnCommStatus(void); 82 | 83 | void EnableFilter(bool is_enable); 84 | 85 | LidarStatus GetLidarStatus(void); 86 | 87 | void ClearDataProcessStatus(void) { 88 | is_frame_ready_ = false; 89 | is_poweron_comm_normal_ = false; 90 | lidarstatus_ = LidarStatus::NORMAL; 91 | last_pkg_timestamp_ = 0; 92 | first_frame_ = true; 93 | } 94 | 95 | 96 | private: 97 | LDType product_type_; 98 | uint16_t timestamp_; 99 | double speed_; 100 | bool is_frame_ready_; 101 | bool is_poweron_comm_normal_; 102 | bool is_filter_; 103 | LidarStatus lidarstatus_; 104 | int measure_point_frequence_; 105 | std::function get_timestamp_; 106 | uint64_t last_pkg_timestamp_; 107 | bool first_frame_; 108 | 109 | LiDARFrameTypeDef pkg_; 110 | Points2D frame_tmp_; 111 | Points2D laser_scan_data_; 112 | std::mutex mutex_lock1_; 113 | std::mutex mutex_lock2_; 114 | 115 | 116 | // parse single packet 117 | bool AnalysisOne(uint8_t byte); 118 | bool Parse(const uint8_t* data, long len); 119 | // combine stantard data into data frames and calibrate 120 | bool AssemblePacket(); 121 | void SetFrameReady(void); 122 | void SetLaserScanData(Points2D& src); 123 | 124 | // Get lidar data frame ready flag 125 | bool IsFrameReady(void); 126 | // Lidar data frame readiness flag reset 127 | void ResetFrameReady(void); 128 | }; 129 | 130 | } // namespace ldlidar 131 | 132 | #endif //__LIPKG_H 133 | 134 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 135 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/filter/tofbf.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tofbf.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef __TOFBF_H_ 23 | #define __TOFBF_H_ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include "ldlidar_datatype.h" 34 | 35 | namespace ldlidar { 36 | 37 | enum class FilterType{ 38 | NO_FILTER, 39 | NEAR_FILTER, 40 | NOISE_FILTER 41 | }; 42 | 43 | class Tofbf { 44 | public: 45 | Tofbf(int speed, LDType type); 46 | ~Tofbf(); 47 | std::vector Filter(const std::vector &tmp) const; 48 | 49 | private: 50 | FilterType filter_type_; 51 | // Low intensity threshold 52 | int intensity_low_; 53 | // Discrete points require higher intensity 54 | int intensity_single_; 55 | // Default scan frequency, to change, read according to radar protocol 56 | int scan_frequency_; 57 | double curr_speed_; 58 | Tofbf() = delete; 59 | Tofbf(const Tofbf &) = delete; 60 | Tofbf &operator=(const Tofbf &) = delete; 61 | std::vector NearFilter(const std::vector &tmp) const; 62 | std::vector NoiseFilter(const std::vector &tmp) const; 63 | }; 64 | 65 | } // namespace ldlidar 66 | 67 | #endif //__TOFBF_H_ 68 | 69 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 70 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/logger/log_module.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_logger.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022.08.10 7 | * @note 8 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights reserved. 9 | * Licensed under the MIT License (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License in the file LICENSE 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | **/ 18 | #ifndef __LOGMODULE_H_ 19 | #define __LOGMODULE_H_ 20 | 21 | 22 | #define LINUX 23 | 24 | #define ENABLE_LOG_DIS_OUTPUT 25 | 26 | #define ENABLE_CONSOLE_LOG_DIS 27 | 28 | //#define ENABLE_LOG_WRITE_TO_FILE 29 | 30 | #define LOGFILEPATH "./ldlidar-driver.log" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #ifndef LINUX 38 | #include 39 | #else 40 | //#include 41 | #include 42 | #define printf_s(fileptr,str) (fprintf(fileptr,"%s",str)) 43 | #define __in 44 | #endif // ?????????????????????? 45 | 46 | 47 | struct LogVersion { 48 | int n_version; 49 | std::string str_descruble; 50 | }; 51 | 52 | 53 | class ILogRealization { 54 | public: 55 | virtual ~ILogRealization() { 56 | 57 | } 58 | virtual void Initializion(const char* path = NULL) = 0; 59 | virtual void LogPrintInf(const char* str) = 0; 60 | void free() { 61 | free(this); 62 | //this = NULL; 63 | }; 64 | private: 65 | virtual void free(ILogRealization *plogger) = 0 ; 66 | }; 67 | 68 | 69 | #define ILOGFREE(LogRealizationClass) virtual void free(ILogRealization* plogger) \ 70 | { \ 71 | LogRealizationClass* prealization = static_cast(plogger); \ 72 | if (prealization != NULL){ delete prealization;} \ 73 | } 74 | 75 | class LogPrint :public ILogRealization { 76 | public: 77 | virtual void Initializion(const char* path = NULL); 78 | virtual void free(ILogRealization *plogger); 79 | virtual void LogPrintInf(const char* str); 80 | }; 81 | 82 | #ifndef LINUX 83 | class LogOutputString :public ILogRealization { 84 | public: 85 | virtual void Initializion(const char* path = NULL) { 86 | return ; 87 | } 88 | 89 | virtual void LogPrintInf(const char* str) { 90 | OutputDebugString((LPCTSTR)str); 91 | OutputDebugString("\r\n"); 92 | } 93 | 94 | ILOGFREE(LogOutputString) 95 | /* 96 | virtual void free(ILogRealization *plogger) 97 | { 98 | LogOutputString* poutput = static_cast(plogger); 99 | if (poutput != NULL) 100 | { 101 | delete poutput; 102 | } 103 | } 104 | */ 105 | }; 106 | #endif 107 | 108 | 109 | class LogModule { 110 | public: 111 | enum LogLevel { 112 | DEBUG_LEVEL, 113 | WARNING_LEVEL, 114 | ERROR_LEVEL, 115 | INFO_LEVEL 116 | }; 117 | 118 | struct LOGMODULE_INFO { 119 | LogLevel loglevel; 120 | std::string str_filename; 121 | std::string str_funcname; 122 | int n_linenumber; 123 | }logInfo_; 124 | 125 | ILogRealization* p_realization_; 126 | public: 127 | static LogModule* GetInstance( __in const char* filename, __in const char* funcname,__in int lineno, LogLevel level, ILogRealization*plog = NULL); 128 | static LogModule* GetInstance(LogLevel level, ILogRealization*plog = NULL); 129 | 130 | void LogPrintInf(const char* format,...); 131 | void LogPrintNoLocationInf(const char* format,...); 132 | 133 | private: 134 | LogModule(); 135 | 136 | ~LogModule(); 137 | 138 | void InitLock(); 139 | 140 | void RealseLock(); 141 | 142 | void Lock(); 143 | 144 | void UnLock(); 145 | 146 | std::string GetCurrentTime(); 147 | 148 | inline uint64_t GetCurrentLocalTimeStamp() { 149 | //// 获取系统时间戳 150 | std::chrono::time_point tp = 151 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 152 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 153 | return (uint64_t)tmp.count(); 154 | } 155 | 156 | std::string GetFormatValue(std::string str_value); 157 | 158 | std::string GetFormatValue(int n_value); 159 | 160 | std::string GetLevelValue(int level); 161 | 162 | static LogModule* s_plog_module_; 163 | 164 | #ifndef LINUX 165 | CRITICAL_SECTION mutex_lock_; 166 | #else 167 | pthread_mutex_t mutex_lock_; 168 | #endif 169 | 170 | 171 | }; 172 | 173 | //// 以下功能支持所处文件、函数、行号信息的打印 174 | #define LOG(level,format,...) LogModule::GetInstance(__FILE__, __FUNCTION__, __LINE__,level)->LogPrintInf(format,__VA_ARGS__); 175 | #ifdef ENABLE_LOG_DIS_OUTPUT 176 | #define LD_LOG_DEBUG(format,...) LOG(LogModule::DEBUG_LEVEL,format,__VA_ARGS__) 177 | #define LD_LOG_INFO(format,...) LOG(LogModule::INFO_LEVEL,format,__VA_ARGS__) 178 | #define LD_LOG_WARN(format,...) LOG(LogModule::WARNING_LEVEL,format,__VA_ARGS__) 179 | #define LD_LOG_ERROR(format,...) LOG(LogModule::ERROR_LEVEL,format,__VA_ARGS__) 180 | #else 181 | #define LD_LOG_DEBUG(format,...) do {} while(0) 182 | #define LD_LOG_INFO(format,...) do {} while(0) 183 | #define LD_LOG_WARN(format,...) do {} while(0) 184 | #define LD_LOG_ERROR(format,...) do {} while(0) 185 | #endif 186 | //// 以下功能不支持所处文件、函数、行号信息的打印 187 | #ifdef ENABLE_LOG_DIS_OUTPUT 188 | #define LOG_NO_DESCRI(level,format,...) LogModule::GetInstance(level)->LogPrintNoLocationInf(format,__VA_ARGS__); 189 | #define LDS_LOG_DEBUG(format,...) LOG_NO_DESCRI(LogModule::DEBUG_LEVEL,format,__VA_ARGS__) 190 | #define LDS_LOG_INFO(format,...) LOG_NO_DESCRI(LogModule::INFO_LEVEL,format,__VA_ARGS__) 191 | #define LDS_LOG_WARN(format,...) LOG_NO_DESCRI(LogModule::WARNING_LEVEL,format,__VA_ARGS__) 192 | #define LDS_LOG_ERROR(format,...) LOG_NO_DESCRI(LogModule::ERROR_LEVEL,format,__VA_ARGS__) 193 | #else 194 | #define LDS_LOG_DEBUG(format,...) do {} while(0) 195 | #define LDS_LOG_INFO(format,...) do {} while(0) 196 | #define LDS_LOG_WARN(format,...) do {} while(0) 197 | #define LDS_LOG_ERROR(format,...) do {} while(0) 198 | #endif 199 | 200 | #endif//__LDLIDAR_LOGGER_H__ 201 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/networkcom/network_socket_interface_linux.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file network_socket_interface_linux.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux network App 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef __SOCKET_INTERFACE_LINUX_H__ 21 | #define __SOCKET_INTERFACE_LINUX_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace ldlidar { 47 | 48 | typedef enum NetworkCommDevEnum { 49 | NET_NULL, 50 | UDP_SERVER, 51 | UDP_CLIENT, 52 | TCP_SERVER, 53 | TCP_CLIENT 54 | }NetCommDevTypeDef; 55 | 56 | class UDPSocketInterfaceLinux { 57 | public: 58 | UDPSocketInterfaceLinux(); 59 | 60 | ~UDPSocketInterfaceLinux(); 61 | 62 | bool CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port); 63 | 64 | bool CloseSocket(); 65 | 66 | bool TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len); 67 | 68 | void SetRecvCallback(std::function callback); 69 | 70 | bool IsClientAck() { return is_server_recv_ack_flag_.load();} 71 | 72 | private: 73 | std::thread *recv_thread_; 74 | long long recv_count_; 75 | int32_t com_sockfd_; 76 | NetCommDevTypeDef ncd_; 77 | std::atomic is_cmd_created_, recv_thread_exit_flag_, is_server_recv_ack_flag_; 78 | std::function recv_callback_; 79 | std::string server_ip_, server_port_; 80 | std::string client_ip_, client_port_; 81 | 82 | bool IsCreated() { return is_cmd_created_.load(); } 83 | 84 | bool RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len); 85 | 86 | static void RecvThreadProc(void *param); 87 | }; 88 | 89 | 90 | class TCPSocketInterfaceLinux { 91 | public: 92 | TCPSocketInterfaceLinux(); 93 | 94 | ~TCPSocketInterfaceLinux(); 95 | 96 | bool CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port); 97 | 98 | bool CloseSocket(); 99 | 100 | bool TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len); 101 | 102 | void SetRecvCallback(std::function callback); 103 | 104 | 105 | 106 | private: 107 | std::thread *recv_thread_; 108 | long long recv_count_; 109 | int32_t com_sockfd_; 110 | int32_t listend_client_sockfd_; //// server model used 111 | NetCommDevTypeDef ncd_; 112 | std::atomic is_cmd_created_, recv_thread_exit_flag_; 113 | std::function recv_callback_; 114 | 115 | bool IsCreated() { return is_cmd_created_.load(); } 116 | 117 | bool RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len); 118 | 119 | static void RecvThreadProc(void *param); 120 | }; 121 | 122 | } // namespace ldlidar 123 | #endif // __SOCKET_INTERFACE_LINUX_H__ 124 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 125 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/include/serialcom/serial_interface_linux.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmd_interface_linux.h 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef __LINUX_SERIAL_PORT_H__ 21 | #define __LINUX_SERIAL_PORT_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | namespace asmtermios { 31 | #include 32 | } 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace ldlidar { 46 | 47 | class SerialInterfaceLinux { 48 | public: 49 | SerialInterfaceLinux(); 50 | ~SerialInterfaceLinux(); 51 | // open serial port 52 | bool Open(std::string &port_name, uint32_t com_baudrate); 53 | // close serial port 54 | bool Close(); 55 | // receive from port channel data 56 | bool ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len); 57 | // transmit data to port channel 58 | bool WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len); 59 | // set receive port channel data callback deal with fuction 60 | void SetReadCallback(std::function callback) { 61 | read_callback_ = callback; 62 | } 63 | // whether open 64 | bool IsOpened() { return is_cmd_opened_.load(); }; 65 | 66 | private: 67 | std::thread *rx_thread_; 68 | long long rx_count_; 69 | int32_t com_handle_; 70 | uint32_t com_baudrate_; 71 | std::atomic is_cmd_opened_, rx_thread_exit_flag_; 72 | std::function read_callback_; 73 | static void RxThreadProc(void *param); 74 | }; 75 | 76 | } // namespace ldlidar 77 | 78 | #endif //__LINUX_SERIAL_PORT_H__ 79 | 80 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 81 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/core/ldlidar_driver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_driver.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief ldlidar processing App 5 | * This code is only applicable to LDROBOT LiDAR LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-05-12 9 | * 10 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ldlidar_driver.h" 22 | 23 | namespace ldlidar { 24 | 25 | bool LDLidarDriver::is_ok_ = false; 26 | 27 | LDLidarDriver::LDLidarDriver() 28 | : sdk_version_number_("v3.0.3"), 29 | is_start_flag_(false), 30 | register_get_timestamp_handle_(nullptr), 31 | comm_pkg_(new LiPkg()), 32 | comm_serial_(new SerialInterfaceLinux()), 33 | comm_tcp_network_(new TCPSocketInterfaceLinux()), 34 | comm_udp_network_(new UDPSocketInterfaceLinux()) { 35 | last_pubdata_times_ = std::chrono::steady_clock::now(); 36 | } 37 | 38 | LDLidarDriver::~LDLidarDriver() { 39 | if (comm_pkg_ != nullptr) { 40 | delete comm_pkg_; 41 | } 42 | 43 | if (comm_serial_ != nullptr) { 44 | delete comm_serial_; 45 | } 46 | 47 | if (comm_tcp_network_ != nullptr) { 48 | delete comm_tcp_network_; 49 | } 50 | 51 | if (comm_udp_network_ != nullptr) { 52 | delete comm_udp_network_; 53 | } 54 | } 55 | 56 | std::string LDLidarDriver::GetLidarSdkVersionNumber(void) { 57 | return sdk_version_number_; 58 | } 59 | 60 | bool LDLidarDriver::Start(LDType product_name, 61 | std::string serial_port_name, 62 | uint32_t serial_baudrate, 63 | CommunicationModeTypeDef comm_mode) { 64 | if (is_start_flag_) { 65 | return true; 66 | } 67 | 68 | if (LDType::NO_VERSION == product_name) { 69 | LD_LOG_ERROR("input is abnormal.",""); 70 | return false; 71 | } 72 | 73 | if (serial_port_name.empty() || (0 == serial_baudrate)) { 74 | LD_LOG_ERROR("input serial param error.",""); 75 | return false; 76 | } 77 | 78 | if (register_get_timestamp_handle_ == nullptr) { 79 | LD_LOG_ERROR("get timestamp fuctional is not register.",""); 80 | return false; 81 | } 82 | 83 | comm_pkg_->ClearDataProcessStatus(); 84 | comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); 85 | comm_pkg_->SetProductType(product_name); 86 | 87 | if (COMM_SERIAL_MODE == comm_mode) { 88 | comm_serial_->SetReadCallback(std::bind(&LiPkg::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 89 | if (!(comm_serial_->Open(serial_port_name, serial_baudrate))) { 90 | LD_LOG_ERROR("serial is not open:%s", serial_port_name.c_str()); 91 | return false; 92 | } 93 | } else { 94 | LD_LOG_ERROR("input value is not \"ldlidar::COMM_SERIAL_MODE\"",""); 95 | return false; 96 | } 97 | 98 | is_start_flag_ = true; 99 | 100 | SetIsOkStatus(true); 101 | 102 | return true; 103 | } 104 | 105 | bool LDLidarDriver::Start(LDType product_name, 106 | const char* server_ip, 107 | const char* server_port, 108 | CommunicationModeTypeDef comm_mode) { 109 | 110 | if (is_start_flag_) { 111 | return true; 112 | } 113 | 114 | if (LDType::NO_VERSION == product_name) { 115 | LD_LOG_ERROR("input is abnormal.",""); 116 | return false; 117 | } 118 | 119 | if ((server_ip == nullptr) || (server_port == nullptr)) { 120 | LD_LOG_ERROR("input server_ip or server_port is null",""); 121 | return false; 122 | } 123 | 124 | if ((COMM_NO_NULL == comm_mode) || (COMM_SERIAL_MODE == comm_mode)) { 125 | LDS_LOG_ERROR("input comm_mode param is error.",""); 126 | return false; 127 | } 128 | 129 | if (register_get_timestamp_handle_ == nullptr) { 130 | LD_LOG_ERROR("get timestamp fuctional is not register.",""); 131 | return false; 132 | } 133 | 134 | comm_pkg_->ClearDataProcessStatus(); 135 | comm_pkg_->RegisterTimestampGetFunctional(register_get_timestamp_handle_); 136 | comm_pkg_->SetProductType(product_name); 137 | 138 | switch (comm_mode) { 139 | case COMM_TCP_CLIENT_MODE: { 140 | comm_tcp_network_->SetRecvCallback(std::bind(&LiPkg::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 141 | bool result = comm_tcp_network_->CreateSocket(TCP_CLIENT, server_ip, server_port); 142 | if (!result) { 143 | LD_LOG_ERROR("client host: create socket is fail.",""); 144 | return false; 145 | } 146 | LDS_LOG_INFO("client host: create socket is ok.",""); 147 | } 148 | break; 149 | case COMM_TCP_SERVER_MODE: { 150 | comm_tcp_network_->SetRecvCallback(std::bind(&LiPkg::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 151 | bool result = comm_tcp_network_->CreateSocket(TCP_SERVER, server_ip, server_port); 152 | if (!result) { 153 | LD_LOG_ERROR("server host: create socket is fail.",""); 154 | return false; 155 | } 156 | LDS_LOG_INFO("server host: create socket is ok.",""); 157 | } 158 | break; 159 | case COMM_UDP_CLIENT_MODE: { 160 | comm_udp_network_->SetRecvCallback(std::bind(&LiPkg::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 161 | bool result = comm_udp_network_->CreateSocket(UDP_CLIENT, server_ip, server_port); 162 | if (!result) { 163 | LD_LOG_ERROR("client host: create socket is fail.",""); 164 | return false; 165 | } 166 | // 主动向服务端发布消息使服务端保存客户端ip，port 信息，建立沟通渠道 167 | uint8_t trans_buf[4] = {0xa5, 0x5a, 0x00, 0x00}; 168 | uint32_t tx_len; 169 | if (!comm_udp_network_->TransToNet((uint8_t *)trans_buf, sizeof(trans_buf), &tx_len)) { 170 | LD_LOG_ERROR("client host: send request to server is fail. %s", strerror(errno)); 171 | return false; 172 | } 173 | LDS_LOG_INFO("client host: create socket is ok.",""); 174 | } 175 | break; 176 | case COMM_UDP_SERVER_MODE: { 177 | comm_udp_network_->SetRecvCallback(std::bind(&LiPkg::CommReadCallback, comm_pkg_, std::placeholders::_1, std::placeholders::_2)); 178 | bool result = comm_udp_network_->CreateSocket(UDP_SERVER, server_ip, server_port); 179 | if (!result) { 180 | LD_LOG_ERROR("server host: create socket is fail.",""); 181 | return false; 182 | } 183 | LDS_LOG_INFO("server host: create socket is ok.",""); 184 | LDS_LOG_INFO("server host: wait client ack connect..",""); 185 | while (!comm_udp_network_->IsClientAck()) { 186 | usleep(1000); 187 | } 188 | } 189 | break; 190 | default: { 191 | LDS_LOG_ERROR("input comm_mode param is error.",""); 192 | return false; 193 | } 194 | break; 195 | } 196 | 197 | is_start_flag_ = true; 198 | 199 | SetIsOkStatus(true); 200 | 201 | return true; 202 | } 203 | 204 | bool LDLidarDriver::Stop(void) { 205 | if (!is_start_flag_) { 206 | return true; 207 | } 208 | 209 | comm_serial_->Close(); 210 | comm_tcp_network_->CloseSocket(); 211 | comm_udp_network_->CloseSocket(); 212 | 213 | is_start_flag_ = false; 214 | 215 | SetIsOkStatus(false); 216 | 217 | return true; 218 | } 219 | 220 | bool LDLidarDriver::WaitLidarCommConnect(int64_t timeout) { 221 | auto last_time = std::chrono::steady_clock::now(); 222 | 223 | bool is_recvflag = false; 224 | do { 225 | if (comm_pkg_->GetLidarPowerOnCommStatus()) { 226 | is_recvflag = true; 227 | } 228 | usleep(1000); 229 | } while (!is_recvflag && (std::chrono::duration_cast( 230 | std::chrono::steady_clock::now() - last_time).count() < timeout)); 231 | 232 | if (is_recvflag) { 233 | last_pubdata_times_ = std::chrono::steady_clock::now(); 234 | return true; 235 | } else { 236 | return false; 237 | } 238 | } 239 | 240 | LidarStatus LDLidarDriver::GetLaserScanData(Points2D& dst, int64_t timeout) { 241 | if (!is_start_flag_) { 242 | return LidarStatus::STOP; 243 | } 244 | 245 | LidarStatus status = comm_pkg_->GetLidarStatus(); 246 | if (LidarStatus::NORMAL == status) { 247 | if (comm_pkg_->GetLaserScanData(dst)) { 248 | last_pubdata_times_ = std::chrono::steady_clock::now(); 249 | return LidarStatus::NORMAL; 250 | } 251 | 252 | if (std::chrono::duration_cast(std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 253 | return LidarStatus::DATA_TIME_OUT; 254 | } else { 255 | return LidarStatus::DATA_WAIT; 256 | } 257 | } else { 258 | last_pubdata_times_ = std::chrono::steady_clock::now(); 259 | return status; 260 | } 261 | } 262 | 263 | LidarStatus LDLidarDriver::GetLaserScanData(LaserScan& dst, int64_t timeout) { 264 | if (!is_start_flag_) { 265 | return LidarStatus::STOP; 266 | } 267 | 268 | LidarStatus status = comm_pkg_->GetLidarStatus(); 269 | if (LidarStatus::NORMAL == status) { 270 | Points2D recvpcd; 271 | if (comm_pkg_->GetLaserScanData(recvpcd)) { 272 | last_pubdata_times_ = std::chrono::steady_clock::now(); 273 | dst.stamp = recvpcd.front().stamp; 274 | dst.points = recvpcd; 275 | return LidarStatus::NORMAL; 276 | } 277 | 278 | if (std::chrono::duration_cast(std::chrono::steady_clock::now() - last_pubdata_times_).count() > timeout) { 279 | return LidarStatus::DATA_TIME_OUT; 280 | } else { 281 | return LidarStatus::DATA_WAIT; 282 | } 283 | } else { 284 | last_pubdata_times_ = std::chrono::steady_clock::now(); 285 | return status; 286 | } 287 | } 288 | 289 | bool LDLidarDriver::GetLidarScanFreq(double& spin_hz) { 290 | if (!is_start_flag_) { 291 | return false; 292 | } 293 | spin_hz = comm_pkg_->GetSpeed(); 294 | 295 | return true; 296 | } 297 | 298 | void LDLidarDriver::RegisterGetTimestampFunctional(std::function get_timestamp_handle) { 299 | register_get_timestamp_handle_ = get_timestamp_handle; 300 | } 301 | 302 | void LDLidarDriver::EnableFilterAlgorithnmProcess(bool is_enable) { 303 | comm_pkg_->EnableFilter(is_enable); 304 | } 305 | 306 | // uint8_t LDLidarDriver::GetLidarErrorCode(void) { 307 | // if (!is_start_flag_) { 308 | // return 0x00; 309 | // } 310 | // uint8_t errcode = comm_pkg_->GetLidarErrorCode(); 311 | // return errcode; 312 | // } 313 | 314 | } // namespace ldlidar 315 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 316 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/dataprocess/lipkg.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lipkg.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR data protocol processing App 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #include "lipkg.h" 23 | 24 | namespace ldlidar { 25 | 26 | static const uint8_t CrcTable[256] = { 27 | 0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 0xae, 0xf2, 0xbf, 0x68, 0x25, 28 | 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33, 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 29 | 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 0xf5, 0x1f, 0x52, 0x85, 0xc8, 30 | 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77, 0x3a, 0x94, 0xd9, 0x0e, 0x43, 31 | 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55, 0x18, 0x44, 0x09, 0xde, 0x93, 32 | 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4, 0xe9, 0x47, 0x0a, 0xdd, 0x90, 33 | 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f, 0x62, 0x97, 0xda, 0x0d, 0x40, 34 | 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff, 0xb2, 0x1c, 0x51, 0x86, 0xcb, 35 | 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2, 0x8f, 0xd3, 0x9e, 0x49, 0x04, 36 | 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12, 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 37 | 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 0xd4, 0x7c, 0x31, 0xe6, 0xab, 38 | 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 0x59, 0xf7, 0xba, 0x6d, 0x20, 39 | 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 40 | 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9, 0xb4, 0x1a, 0x57, 0x80, 0xcd, 41 | 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 0x3f, 0xca, 0x87, 0x50, 0x1d, 42 | 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 0xef, 0x41, 0x0c, 0xdb, 0x96, 43 | 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 0xec, 0xb0, 0xfd, 0x2a, 0x67, 44 | 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71, 0x3c, 0x92, 0xdf, 0x08, 0x45, 45 | 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 0xb7, 0x5d, 0x10, 0xc7, 0x8a, 46 | 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 0x78, 0xd6, 0x9b, 0x4c, 0x01, 47 | 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 48 | 0x7f, 0x32, 0xe5, 0xa8}; 49 | 50 | uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) { 51 | uint8_t crc = 0; 52 | while (data_len--) { 53 | crc = CrcTable[(crc ^ *data) & 0xff]; 54 | data++; 55 | } 56 | return crc; 57 | } 58 | 59 | LiPkg::LiPkg() 60 | : product_type_(LDType::NO_VERSION), 61 | timestamp_(0), 62 | speed_(0), 63 | is_frame_ready_(false), 64 | is_poweron_comm_normal_(false), 65 | is_filter_(true), 66 | lidarstatus_(LidarStatus::NORMAL), 67 | measure_point_frequence_(4500), 68 | get_timestamp_(nullptr), 69 | last_pkg_timestamp_(0), 70 | first_frame_(true) { 71 | 72 | } 73 | 74 | LiPkg::~LiPkg() { 75 | 76 | } 77 | 78 | void LiPkg::SetProductType(LDType type_number) { 79 | product_type_ = type_number; 80 | switch (type_number) { 81 | case LDType::LD_06: 82 | case LDType::LD_19: 83 | measure_point_frequence_ = 4500; 84 | break; 85 | case LDType::STL_06P: 86 | case LDType::STL_26: 87 | measure_point_frequence_ = 5000; 88 | break; 89 | case LDType::STL_27L: 90 | measure_point_frequence_ = 21600; 91 | break; 92 | default : 93 | measure_point_frequence_ = 4500; 94 | break; 95 | } 96 | } 97 | 98 | bool LiPkg::AnalysisOne(uint8_t byte) { 99 | static enum { 100 | HEADER, 101 | VER_LEN, 102 | DATA, 103 | } state = HEADER; 104 | static uint16_t count = 0; 105 | static uint8_t tmp[128] = {0}; 106 | static uint16_t pkg_count = sizeof(LiDARFrameTypeDef); 107 | 108 | switch (state) { 109 | case HEADER: 110 | if (byte == PKG_HEADER) { 111 | tmp[count++] = byte; 112 | state = VER_LEN; 113 | } 114 | break; 115 | case VER_LEN: 116 | if (byte == PKG_VER_LEN) { 117 | tmp[count++] = byte; 118 | state = DATA; 119 | } else { 120 | state = HEADER; 121 | count = 0; 122 | return false; 123 | } 124 | break; 125 | case DATA: 126 | tmp[count++] = byte; 127 | if (count >= pkg_count) { 128 | memcpy((uint8_t *)&pkg_, tmp, pkg_count); 129 | uint8_t crc = CalCRC8((uint8_t *)&pkg_, pkg_count - 1); 130 | state = HEADER; 131 | count = 0; 132 | if (crc == pkg_.crc8) { 133 | return true; 134 | } else { 135 | return false; 136 | } 137 | } 138 | break; 139 | default: 140 | break; 141 | } 142 | 143 | return false; 144 | } 145 | 146 | bool LiPkg::Parse(const uint8_t *data, long len) { 147 | for (int i = 0; i < len; i++) { 148 | if (AnalysisOne(data[i])) { 149 | is_poweron_comm_normal_ = true; 150 | // parse a package is success 151 | double diff = (pkg_.end_angle / 100 - pkg_.start_angle / 100 + 360) % 360; 152 | if (diff <= ((double)pkg_.speed * POINT_PER_PACK / measure_point_frequence_ * 1.5)) { 153 | 154 | if (0 == last_pkg_timestamp_) { 155 | last_pkg_timestamp_ = get_timestamp_(); 156 | continue; 157 | } 158 | uint64_t current_pack_stamp = get_timestamp_(); 159 | int pkg_point_number = POINT_PER_PACK; 160 | double pack_stamp_point_step = 161 | static_cast(current_pack_stamp - last_pkg_timestamp_) / static_cast(pkg_point_number - 1); 162 | 163 | speed_ = pkg_.speed; // Degrees per second 164 | timestamp_ = pkg_.timestamp; // In milliseconds 165 | uint32_t diff = ((uint32_t)pkg_.end_angle + 36000 - (uint32_t)pkg_.start_angle) % 36000; 166 | float step = diff / (POINT_PER_PACK - 1) / 100.0; 167 | float start = (double)pkg_.start_angle / 100.0; 168 | PointData data; 169 | for (int i = 0; i < POINT_PER_PACK; i++) { 170 | data.distance = pkg_.point[i].distance; 171 | data.angle = start + i * step; 172 | if (data.angle >= 360.0) { 173 | data.angle -= 360.0; 174 | } 175 | data.intensity = pkg_.point[i].intensity; 176 | data.stamp = static_cast(last_pkg_timestamp_ + (pack_stamp_point_step * i)); 177 | frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity, data.stamp)); 178 | } 179 | 180 | last_pkg_timestamp_ = current_pack_stamp; //// update last pkg timestamp 181 | } 182 | } 183 | } 184 | 185 | return true; 186 | } 187 | 188 | bool LiPkg::AssemblePacket() { 189 | float last_angle = 0; 190 | Points2D tmp, data; 191 | int count = 0; 192 | 193 | for (auto n : frame_tmp_) { 194 | // wait for enough data, need enough data to show a circle 195 | // enough data has been obtained 196 | if ((n.angle < 20.0) && (last_angle > 340.0)) { 197 | if ((count * GetSpeed()) > (measure_point_frequence_ * 1.4)) { 198 | if (count >= (int)frame_tmp_.size()) { 199 | frame_tmp_.clear(); 200 | } else { 201 | frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count); 202 | } 203 | return false; 204 | } 205 | 206 | data.insert(data.begin(), frame_tmp_.begin(), frame_tmp_.begin() + count); 207 | 208 | if (is_filter_) { 209 | Tofbf tofbfLd(speed_, product_type_); 210 | tmp = tofbfLd.Filter(data); 211 | } else { 212 | tmp = data; 213 | } 214 | 215 | std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.stamp < b.stamp; }); 216 | 217 | if (tmp.size() > 0) { 218 | if (first_frame_) { 219 | first_frame_ = false; 220 | } else { 221 | SetLaserScanData(tmp); 222 | SetFrameReady(); 223 | } 224 | 225 | if (count >= (int)frame_tmp_.size()) { 226 | frame_tmp_.clear(); 227 | } else { 228 | frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count); 229 | } 230 | return true; 231 | } 232 | } 233 | count++; 234 | 235 | if ((count * GetSpeed()) > (measure_point_frequence_ * 2)) { 236 | if (count >= (int)frame_tmp_.size()) { 237 | frame_tmp_.clear(); 238 | } else { 239 | frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count); 240 | } 241 | return false; 242 | } 243 | 244 | last_angle = n.angle; 245 | } 246 | 247 | return false; 248 | } 249 | 250 | double LiPkg::GetSpeed(void) { 251 | return (speed_ / 360.0); // unit is hz 252 | } 253 | 254 | uint16_t LiPkg::GetSpeedOrigin(void) { 255 | return speed_; 256 | } 257 | 258 | uint16_t LiPkg::GetTimestamp(void) { 259 | return timestamp_; 260 | } 261 | 262 | int LiPkg::GetLidarMeasurePointFrequence(void) { 263 | return measure_point_frequence_; 264 | } 265 | 266 | bool LiPkg::IsFrameReady(void) { 267 | std::lock_guard lg(mutex_lock1_); 268 | return is_frame_ready_; 269 | } 270 | 271 | void LiPkg::ResetFrameReady(void) { 272 | std::lock_guard lg(mutex_lock1_); 273 | is_frame_ready_ = false; 274 | } 275 | 276 | void LiPkg::SetFrameReady(void) { 277 | std::lock_guard lg(mutex_lock1_); 278 | is_frame_ready_ = true; 279 | } 280 | 281 | bool LiPkg::GetLaserScanData(Points2D& out) { 282 | if (IsFrameReady()) { 283 | ResetFrameReady(); 284 | { 285 | std::lock_guard lg(mutex_lock2_); 286 | out = laser_scan_data_; 287 | } 288 | return true; 289 | } else { 290 | return false; 291 | } 292 | } 293 | 294 | void LiPkg::SetLaserScanData(Points2D& src) { 295 | std::lock_guard lg(mutex_lock2_); 296 | laser_scan_data_ = src; 297 | } 298 | 299 | void LiPkg::RegisterTimestampGetFunctional(std::function timestamp_handle) { 300 | get_timestamp_ = timestamp_handle; 301 | } 302 | 303 | bool LiPkg::GetLidarPowerOnCommStatus(void) { 304 | if (is_poweron_comm_normal_) { 305 | is_poweron_comm_normal_ = false; 306 | return true; 307 | } else { 308 | return false; 309 | } 310 | } 311 | 312 | void LiPkg::EnableFilter(bool is_enable) { 313 | is_filter_ = is_enable; 314 | } 315 | 316 | LidarStatus LiPkg::GetLidarStatus(void) { 317 | return lidarstatus_; 318 | } 319 | 320 | void LiPkg::CommReadCallback(const char *byte, size_t len) { 321 | if (this->Parse((uint8_t *)byte, len)) { 322 | this->AssemblePacket(); 323 | } 324 | } 325 | 326 | } // namespace ldlidar 327 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 328 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/filter/tofbf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file tofbf.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief LiDAR near-range filtering algorithm 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #include "tofbf.h" 23 | 24 | namespace ldlidar { 25 | 26 | /** 27 | * @brief Construct a new Tofbf:: Tofbf object 28 | * @param [in] 29 | * @param speed current lidar speed 30 | */ 31 | Tofbf::Tofbf(int speed, LDType type) { 32 | curr_speed_ = speed; // ldliar spin speed, unit is Degrees per second 33 | switch (type) 34 | { 35 | case LDType::LD_06: 36 | case LDType::LD_19: 37 | intensity_low_ = 15; 38 | intensity_single_ = 220; 39 | scan_frequency_ = 4500; 40 | filter_type_ = FilterType::NEAR_FILTER; 41 | break; 42 | case LDType::STL_06P: 43 | case LDType::STL_26: 44 | case LDType::STL_27L: 45 | filter_type_ = FilterType::NOISE_FILTER; 46 | break; 47 | default: 48 | std::cout << "[ldrobot] tofbf input ldlidar type error!" << std::endl; 49 | filter_type_ = FilterType::NO_FILTER; 50 | break; 51 | } 52 | } 53 | 54 | Tofbf::~Tofbf() {} 55 | 56 | /** 57 | * @brief The tof lidar filter 58 | * @param [in] 59 | * @param tmp lidar point data 60 | * @return std::vector 61 | */ 62 | std::vector Tofbf::Filter( 63 | const std::vector &tmp) const { 64 | std::vector normal; 65 | 66 | switch(filter_type_){ 67 | case FilterType::NEAR_FILTER: 68 | normal = NearFilter(tmp); 69 | break; 70 | 71 | case FilterType::NOISE_FILTER: 72 | normal = NoiseFilter(tmp); 73 | break; 74 | 75 | default: 76 | normal = tmp; 77 | break; 78 | } 79 | 80 | return normal; 81 | } 82 | 83 | std::vector Tofbf::NearFilter( 84 | const std::vector &tmp) const { 85 | std::vector normal, pending, item; 86 | std::vector> group; 87 | 88 | // Remove points within 5m 89 | for (auto n : tmp) { 90 | if (n.distance < 5000) { 91 | pending.push_back(n); 92 | } else { 93 | normal.push_back(n); 94 | } 95 | } 96 | 97 | if (tmp.empty()) return normal; 98 | 99 | double angle_delta_up_limit = curr_speed_ / scan_frequency_ * 2; 100 | 101 | // sort 102 | std::sort(pending.begin(), pending.end(), 103 | [](PointData a, PointData b) { return a.angle < b.angle; }); 104 | 105 | PointData last(-10, 0, 0); 106 | // group 107 | for (auto n : pending) { 108 | if (fabs(n.angle - last.angle) > angle_delta_up_limit || 109 | fabs(n.distance - last.distance) > last.distance * 0.03) { 110 | if (item.empty() == false) { 111 | group.push_back(item); 112 | item.clear(); 113 | } 114 | } 115 | item.push_back(n); 116 | last = n; 117 | } 118 | // push back last item 119 | if (item.empty() == false) group.push_back(item); 120 | 121 | if (group.empty()) return normal; 122 | 123 | // Connection 0 degree and 359 degree 124 | auto first_item = group.front().front(); 125 | auto last_item = group.back().back(); 126 | if (fabs(first_item.angle + 360.f - last_item.angle) < angle_delta_up_limit && 127 | fabs(first_item.distance - last_item.distance) < last.distance * 0.03) { 128 | group.front().insert(group.front().begin(), group.back().begin(), group.back().end()); 129 | group.erase(group.end() - 1); 130 | } 131 | // selection 132 | for (auto n : group) { 133 | if (n.size() == 0) continue; 134 | // No filtering if there are many points 135 | if (n.size() > 15) { 136 | normal.insert(normal.end(), n.begin(), n.end()); 137 | continue; 138 | } 139 | 140 | // Filter out those with few points 141 | if (n.size() < 3) { 142 | int c = 0; 143 | for (auto m : n) { 144 | c += m.intensity; 145 | } 146 | c /= n.size(); 147 | if (c < intensity_single_) { 148 | for (auto& point: n) { 149 | point.distance = 0; 150 | point.intensity = 0; 151 | } 152 | normal.insert(normal.end(), n.begin(), n.end()); 153 | continue; 154 | } 155 | } 156 | 157 | // Calculate the mean value of distance and intensity 158 | double intensity_avg = 0; 159 | double dis_avg = 0; 160 | for (auto m : n) { 161 | intensity_avg += m.intensity; 162 | dis_avg += m.distance; 163 | } 164 | intensity_avg /= n.size(); 165 | dis_avg /= n.size(); 166 | 167 | // High intensity, no filtering 168 | if (intensity_avg > intensity_low_) { 169 | normal.insert(normal.end(), n.begin(), n.end()); 170 | continue; 171 | } else { 172 | for (auto& point : n) { 173 | point.distance = 0; 174 | point.intensity = 0; 175 | } 176 | normal.insert(normal.end(), n.begin(), n.end()); 177 | continue; 178 | } 179 | } 180 | 181 | return normal; 182 | } 183 | 184 | std::vector Tofbf::NoiseFilter( 185 | const std::vector &tmp) const { 186 | std::vector normal; 187 | PointData last_data, next_data; 188 | 189 | if (tmp.empty()) return normal; 190 | 191 | // Traversing the point data 192 | int count = 0; 193 | for (auto n : tmp) { 194 | if(count == 0) { 195 | last_data = tmp[tmp.size() - 1]; 196 | } else { 197 | last_data = tmp[count - 1]; 198 | } 199 | 200 | if(count == (int)(tmp.size() - 1)) { 201 | next_data = tmp[0]; 202 | } else { 203 | next_data =tmp[count + 1]; 204 | } 205 | count++; 206 | 207 | // Remove points with the opposite trend within 500mm 208 | if(n.distance < 500) { 209 | if((n.distance + 10 < last_data.distance && n.distance + 10 < next_data.distance) 210 | || (n.distance > last_data.distance + 10 && n.distance > next_data.distance + 10)) { 211 | if(n.intensity < 60) { 212 | n.intensity = 0; 213 | n.distance = 0; 214 | normal.push_back(n); 215 | continue; 216 | } 217 | } else if((n.distance + 7 < last_data.distance && n.distance + 7 < next_data.distance) 218 | || (n.distance > last_data.distance + 7 && n.distance > next_data.distance + 7)) { 219 | if(n.intensity < 45) { 220 | n.intensity = 0; 221 | n.distance = 0; 222 | normal.push_back(n); 223 | continue; 224 | } 225 | } else if((n.distance + 5 < last_data.distance && n.distance + 5 < next_data.distance) 226 | || (n.distance > last_data.distance + 5 && n.distance > next_data.distance + 5)) { 227 | if(n.intensity < 30) { 228 | n.intensity = 0; 229 | n.distance = 0; 230 | normal.push_back(n); 231 | continue; 232 | } 233 | } 234 | } 235 | 236 | // Remove points with very low intensity within 5m 237 | if (n.distance < 5000) { 238 | if(n.distance < 200) { 239 | if(n.intensity < 25) { 240 | n.intensity = 0; 241 | n.distance = 0; 242 | normal.push_back(n); 243 | continue; 244 | } 245 | } else { 246 | if(n.intensity < 10) { 247 | n.intensity = 0; 248 | n.distance = 0; 249 | normal.push_back(n); 250 | continue; 251 | } 252 | } 253 | 254 | if((n.distance + 30 < last_data.distance || n.distance > last_data.distance + 30) 255 | &&(n.distance + 30 < next_data.distance || n.distance > next_data.distance + 30)) { 256 | if((n.distance < 2000 && n.intensity < 30) || n.intensity < 20) { 257 | n.intensity = 0; 258 | n.distance = 0; 259 | normal.push_back(n); 260 | continue; 261 | } 262 | } 263 | } 264 | normal.push_back(n); 265 | } 266 | return normal; 267 | } 268 | 269 | } // namespace ldlidar 270 | 271 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 272 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/logger/log_module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ldlidar_logger.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022.08.10 7 | * @note 8 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights reserved. 9 | * Licensed under the MIT License (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License in the file LICENSE 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | **/ 18 | #include "log_module.h" 19 | 20 | #include 21 | #include 22 | 23 | #ifndef LINUX 24 | #include 25 | #pragma comment(lib, "comsuppw.lib") 26 | #else 27 | #include 28 | #endif 29 | 30 | //使用vswprintf会出现奔溃的情况如果，传入数据大于 VA_PARAMETER_MAX 就会出现崩溃 31 | #define VA_PARAMETER_MAX (1024 * 2) 32 | 33 | LogModule* LogModule::s_plog_module_ = NULL; 34 | 35 | 36 | LogModule* LogModule::GetInstance(__in const char* filename, __in const char* funcname, __in int lineno,LogLevel level,ILogRealization*plog) { 37 | if (s_plog_module_ == NULL) { 38 | s_plog_module_ = new LogModule(); 39 | } 40 | s_plog_module_->logInfo_.str_filename = filename; 41 | s_plog_module_->logInfo_.str_funcname = funcname; 42 | s_plog_module_->logInfo_.n_linenumber = lineno; 43 | s_plog_module_->logInfo_.loglevel = level; 44 | 45 | if (plog != NULL) { 46 | s_plog_module_->p_realization_->free(); 47 | s_plog_module_->p_realization_ = plog; 48 | } 49 | return s_plog_module_; 50 | } 51 | 52 | LogModule* LogModule::GetInstance(LogLevel level, ILogRealization*plog) { 53 | if (s_plog_module_ == NULL) { 54 | s_plog_module_ = new LogModule(); 55 | } 56 | s_plog_module_->logInfo_.loglevel = level; 57 | 58 | if (plog != NULL) { 59 | s_plog_module_->p_realization_->free(); 60 | s_plog_module_->p_realization_ = plog; 61 | } 62 | return s_plog_module_; 63 | } 64 | 65 | LogModule::LogModule() { 66 | logInfo_.n_linenumber = -1; 67 | logInfo_.str_filename = ""; 68 | logInfo_.str_funcname = ""; 69 | #ifndef LINUX 70 | p_realization = new LogOutputString(); 71 | #else 72 | p_realization_ = new LogPrint(); 73 | #endif 74 | InitLock(); 75 | } 76 | 77 | LogModule::~LogModule() { 78 | RealseLock(); 79 | } 80 | 81 | void LogModule::LogPrintInf(const char* format,...) { 82 | Lock(); 83 | if (p_realization_) { 84 | std::string str_temp; 85 | // manufacture 86 | str_temp.append("[LDS]"); 87 | //LogLevel 88 | str_temp.append(GetLevelValue(logInfo_.loglevel)); 89 | switch (logInfo_.loglevel) { 90 | case DEBUG_LEVEL: { 91 | //时间戳 uint is seconds 92 | char s_stamp[100] = {0}; 93 | uint64_t timestamp = GetCurrentLocalTimeStamp(); 94 | #ifdef __LP64__ 95 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 96 | #else 97 | #ifdef _WIN64 98 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 99 | #else 100 | snprintf(s_stamp, 100, "[%llu.%llu]", (timestamp/1000000000), (timestamp%1000000000)); 101 | #endif 102 | #endif 103 | str_temp.append(s_stamp); 104 | } 105 | break; 106 | default: { 107 | //时间 [week month day hours:minutes:seconds year] 108 | str_temp.append(GetFormatValue(GetCurrentTime())); 109 | } 110 | break; 111 | } 112 | 113 | //文件名称 114 | str_temp.append(GetFormatValue(logInfo_.str_filename)); 115 | str_temp.append(GetFormatValue(logInfo_.str_funcname)); 116 | //行号 117 | str_temp.append(GetFormatValue(logInfo_.n_linenumber)); 118 | 119 | va_list ptr; 120 | va_start(ptr, format); 121 | char c_value[VA_PARAMETER_MAX] = {0}; 122 | vsnprintf(c_value,sizeof(c_value),format,ptr); 123 | va_end(ptr); 124 | 125 | str_temp.append(GetFormatValue(c_value)); 126 | 127 | p_realization_->LogPrintInf(str_temp.c_str()); 128 | } 129 | UnLock(); 130 | } 131 | 132 | void LogModule::LogPrintNoLocationInf(const char* format,...) { 133 | Lock(); 134 | if (p_realization_) { 135 | std::string str_temp; 136 | // manufacture 137 | str_temp.append("[LDS]"); 138 | //LogLevel 139 | str_temp.append(GetLevelValue(logInfo_.loglevel)); 140 | 141 | //时间戳 uint is seconds 142 | char s_stamp[100] = {0}; 143 | uint64_t timestamp = GetCurrentLocalTimeStamp(); 144 | #ifdef __LP64__ 145 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 146 | #else 147 | #ifdef _WIN64 148 | snprintf(s_stamp, 100, "[%lu.%lu]", (timestamp/1000000000), (timestamp%1000000000)); 149 | #else 150 | snprintf(s_stamp, 100, "[%llu.%llu]", (timestamp/1000000000), (timestamp%1000000000)); 151 | #endif 152 | #endif 153 | str_temp.append(s_stamp); 154 | 155 | va_list ptr; 156 | va_start(ptr, format); 157 | char c_value[VA_PARAMETER_MAX] = {0}; 158 | vsnprintf(c_value,sizeof(c_value),format,ptr); 159 | va_end(ptr); 160 | 161 | str_temp.append(GetFormatValue(c_value)); 162 | 163 | p_realization_->LogPrintInf(str_temp.c_str()); 164 | } 165 | UnLock(); 166 | } 167 | 168 | void LogModule::InitLock() { 169 | #ifndef LINUX 170 | InitializeCriticalSection(&mutex_lock_); 171 | #else 172 | pthread_mutex_init(&mutex_lock_,NULL); 173 | #endif 174 | } 175 | 176 | void LogModule::RealseLock() { 177 | #ifndef LINUX 178 | DeleteCriticalSection(&mutex_lock_); 179 | #else 180 | pthread_mutex_unlock(&mutex_lock_); 181 | #endif 182 | } 183 | 184 | void LogModule::Lock() { 185 | #ifndef LINUX 186 | EnterCriticalSection(&mutex_lock_); 187 | #else 188 | pthread_mutex_lock(&mutex_lock_); 189 | #endif 190 | } 191 | 192 | void LogModule::UnLock() { 193 | #ifndef LINUX 194 | LeaveCriticalSection(&mutex_lock_); 195 | #else 196 | pthread_mutex_unlock(&mutex_lock_); 197 | #endif 198 | } 199 | 200 | std::string LogModule::GetCurrentTime() { 201 | std::string curr_time; 202 | //Current date/time based on current time 203 | time_t now = time(0); 204 | // Convert current time to string 205 | curr_time.assign(ctime(&now)); 206 | // Last charactor of currentTime is "\n", so remove it 207 | std::string current_time = curr_time.substr(0, curr_time.size()-1); 208 | return current_time; 209 | } 210 | 211 | std::string LogModule::GetFormatValue(std::string str_value) { 212 | std::string str_temp; 213 | str_temp.append("["); 214 | str_temp.append(str_value); 215 | str_temp.append("]"); 216 | return str_temp; 217 | } 218 | 219 | std::string LogModule::GetFormatValue(int n_value) { 220 | std::string str_temp; 221 | str_temp.append("["); 222 | char c_value[16]; 223 | sprintf(c_value,"%d",n_value); 224 | str_temp.append(c_value); 225 | str_temp.append("]"); 226 | return str_temp; 227 | } 228 | 229 | std::string LogModule::GetLevelValue(int level){ 230 | std::string tmp; 231 | switch (level) { 232 | case DEBUG_LEVEL: 233 | tmp = "DEBUG"; 234 | break; 235 | case WARNING_LEVEL: 236 | tmp = "WARNING"; 237 | break; 238 | case ERROR_LEVEL: 239 | tmp = "ERROR"; 240 | break; 241 | case INFO_LEVEL: 242 | tmp = "INFO"; 243 | break; 244 | default: 245 | tmp = "UnKnown"; 246 | break; 247 | } 248 | std::string str_temp; 249 | str_temp.append("["); 250 | str_temp.append(tmp); 251 | str_temp.append("]"); 252 | return str_temp; 253 | } 254 | 255 | void LogPrint::Initializion(const char* path) { 256 | printf("%s", path); 257 | return ; 258 | } 259 | 260 | void LogPrint::free(ILogRealization *plogger) { 261 | LogPrint* pOutput = static_cast(plogger); 262 | if (pOutput != NULL) { 263 | delete pOutput; 264 | } 265 | } 266 | 267 | void LogPrint::LogPrintInf(const char* str) { 268 | #ifdef ENABLE_CONSOLE_LOG_DIS 269 | printf("%s\r\n", str); 270 | #endif 271 | 272 | #ifdef ENABLE_LOG_WRITE_TO_FILE 273 | FILE *fp = fopen(LOGFILEPATH,"a"); 274 | if(!fp) { 275 | printf("%s open filed!\n", LOGFILEPATH); 276 | return ; 277 | } 278 | printf_s(fp,str); 279 | printf_s(fp,"\r\n"); 280 | fclose(fp); 281 | #endif 282 | } 283 | 284 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/networkcom/network_socket_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file network_socket_interface_linux.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux network App 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include "network_socket_interface_linux.h" 21 | #include "log_module.h" 22 | 23 | #define MAX_RECV_BUF_LEN 4096 24 | 25 | namespace ldlidar { 26 | 27 | UDPSocketInterfaceLinux::UDPSocketInterfaceLinux() : recv_thread_(nullptr), 28 | recv_count_(0), 29 | com_sockfd_(-1), 30 | ncd_(NET_NULL), 31 | is_cmd_created_(false), 32 | recv_thread_exit_flag_(true), 33 | is_server_recv_ack_flag_(false), 34 | recv_callback_(nullptr) { 35 | 36 | } 37 | 38 | UDPSocketInterfaceLinux::~UDPSocketInterfaceLinux() { 39 | CloseSocket(); 40 | } 41 | 42 | bool UDPSocketInterfaceLinux::CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port) { 43 | 44 | if (is_cmd_created_ == true) { 45 | return true; 46 | } 47 | 48 | if ((ip == nullptr) || (port == nullptr)) { 49 | LD_LOG_ERROR("UDP,input ip address or port number is null",""); 50 | return false; 51 | } 52 | 53 | if ((UDP_CLIENT == obj) || (UDP_SERVER == obj)) { 54 | com_sockfd_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 55 | if(com_sockfd_ == -1) { 56 | LD_LOG_DEBUG("UDP,fail to socket. %s", strerror(errno)); 57 | return false; 58 | } 59 | } else { 60 | LD_LOG_ERROR("UDP,input value is error",""); 61 | return false; 62 | } 63 | 64 | struct sockaddr_in net_addr; 65 | int addrlen = sizeof(net_addr); 66 | bzero(&net_addr, sizeof(net_addr)); 67 | net_addr.sin_family = AF_INET; 68 | net_addr.sin_addr.s_addr = inet_addr(ip); 69 | net_addr.sin_port = htons((short)atoi(port)); 70 | 71 | if(obj == UDP_SERVER){ 72 | if(-1 == bind(com_sockfd_, (const struct sockaddr*)&net_addr, (socklen_t)addrlen)) { 73 | LD_LOG_ERROR("UDP,fail to bind. %s", strerror(errno)); 74 | if (com_sockfd_ != -1) { 75 | close(com_sockfd_); 76 | com_sockfd_ = -1; 77 | } 78 | return false; 79 | } 80 | LDS_LOG_INFO("UDP,bind success..",""); 81 | } 82 | 83 | ncd_ = obj; 84 | server_ip_ = ip; 85 | server_port_ = port; 86 | 87 | // create recv process thread. 88 | recv_thread_exit_flag_ = false; 89 | recv_thread_ = new std::thread(RecvThreadProc, this); 90 | is_cmd_created_ = true; 91 | 92 | return true; 93 | } 94 | 95 | bool UDPSocketInterfaceLinux::CloseSocket() { 96 | if (is_cmd_created_ == false) { 97 | return true; 98 | } 99 | 100 | recv_thread_exit_flag_ = true; 101 | 102 | if (com_sockfd_ != -1) { 103 | close(com_sockfd_); 104 | com_sockfd_ = -1; 105 | } 106 | 107 | if ((recv_thread_ != nullptr) && recv_thread_->joinable()) { 108 | recv_thread_->join(); 109 | delete recv_thread_; 110 | recv_thread_ = nullptr; 111 | } 112 | 113 | is_cmd_created_ = false; 114 | 115 | return true; 116 | } 117 | 118 | bool UDPSocketInterfaceLinux::RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len) { 119 | static timespec timeout = {0, (long)(100 * 1e6)}; 120 | int32_t len = -1; 121 | struct sockaddr_in sender_addr_inf; 122 | bzero(&sender_addr_inf, sizeof(sender_addr_inf)); 123 | int addrlen = sizeof(struct sockaddr_in); 124 | 125 | if (IsCreated()) { 126 | //// setting and init fd table 127 | fd_set recv_fds; 128 | FD_ZERO(&recv_fds); 129 | FD_SET(com_sockfd_, &recv_fds); 130 | int ret = pselect(com_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 131 | if (ret < 0) { 132 | // select was interrputed 133 | if (errno == EINTR) { 134 | return false; 135 | } 136 | } else if (ret == 0) { 137 | // timeout 138 | return false; 139 | } 140 | 141 | if (FD_ISSET(com_sockfd_, &recv_fds)) { 142 | len = (int32_t)recvfrom(com_sockfd_, rx_buf, rx_buff_len, 0, 143 | (struct sockaddr *)&sender_addr_inf, (socklen_t *)&addrlen); 144 | if ((len != -1) && (rx_len != nullptr)) { 145 | *rx_len = len; 146 | 147 | std::string sender_ip = inet_ntoa(sender_addr_inf.sin_addr); 148 | uint16_t sender_port = ntohs(sender_addr_inf.sin_port); 149 | char sender_port_str[10] = {0}; 150 | snprintf(sender_port_str, 10, "%d", sender_port); 151 | 152 | if (ncd_ == UDP_SERVER) { 153 | if (!is_server_recv_ack_flag_.load()) { 154 | ///// 保存与服务端通信的客户端IP和端口号，仅支持一对一 155 | client_ip_ = sender_ip; 156 | client_port_ = sender_port_str; 157 | is_server_recv_ack_flag_.store(true); 158 | } 159 | } 160 | 161 | // printf("Recv lens:%d\n", len); 162 | // printf("Sender IP:%s\n", sender_ip.c_str()); 163 | // printf("Sender port:%s\n", sender_port_str); 164 | } 165 | } 166 | } 167 | 168 | return ((len == -1) ? false : true); 169 | } 170 | 171 | bool UDPSocketInterfaceLinux::TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len) { 172 | int32_t len = -1; 173 | struct sockaddr_in recver_net_addr; 174 | int addrlen = sizeof(recver_net_addr); 175 | bzero(&recver_net_addr, sizeof(recver_net_addr)); 176 | recver_net_addr.sin_family = AF_INET; 177 | 178 | if (IsCreated()) { 179 | if (ncd_ == UDP_CLIENT) { 180 | recver_net_addr.sin_addr.s_addr = inet_addr(server_ip_.c_str()); 181 | recver_net_addr.sin_port = htons((short)atoi(server_port_.c_str())); 182 | } else if (ncd_ == UDP_SERVER) { 183 | if (!is_server_recv_ack_flag_.load()) { 184 | return false; 185 | } else { 186 | recver_net_addr.sin_addr.s_addr = inet_addr(client_ip_.c_str()); 187 | recver_net_addr.sin_port = htons((short)atoi(client_port_.c_str())); 188 | } 189 | } 190 | 191 | len = (int32_t)sendto(com_sockfd_, tx_buf, tx_buff_len, 0, 192 | (struct sockaddr *)&recver_net_addr, (socklen_t)addrlen); 193 | if ((len != -1) && (tx_len != nullptr)) { 194 | *tx_len = len; 195 | // printf("send lens:%d\n", len); 196 | // printf("Reciver IP:%s\n", inet_ntoa(recver_net_addr.sin_addr)); 197 | // printf("Reciver port:%d\n", ntohs(recver_net_addr.sin_port)); 198 | } 199 | } 200 | 201 | return ((len == -1) ? false : true); 202 | } 203 | 204 | void UDPSocketInterfaceLinux::SetRecvCallback(std::function callback) { 205 | recv_callback_ = callback; 206 | } 207 | 208 | void UDPSocketInterfaceLinux::RecvThreadProc(void *param) { 209 | UDPSocketInterfaceLinux *cmd_if = (UDPSocketInterfaceLinux *)param; 210 | char *recieve_buff = new char[MAX_RECV_BUF_LEN + 1]; 211 | 212 | while (!cmd_if->recv_thread_exit_flag_.load()) { 213 | uint32_t recved = 0; 214 | bool res = cmd_if->RecvFromNet((uint8_t *)recieve_buff, MAX_RECV_BUF_LEN, &recved); 215 | if (res && recved) { 216 | cmd_if->recv_count_ += recved; 217 | if (cmd_if->recv_callback_ != nullptr) { 218 | cmd_if->recv_callback_(recieve_buff, recved); 219 | } 220 | } 221 | } 222 | 223 | delete[] recieve_buff; 224 | } 225 | 226 | 227 | TCPSocketInterfaceLinux::TCPSocketInterfaceLinux() : recv_thread_(nullptr), 228 | recv_count_(0), 229 | com_sockfd_(-1), 230 | listend_client_sockfd_(-1), 231 | is_cmd_created_(false), 232 | recv_thread_exit_flag_(false), 233 | recv_callback_(nullptr) { 234 | 235 | } 236 | 237 | TCPSocketInterfaceLinux::~TCPSocketInterfaceLinux() { 238 | CloseSocket(); 239 | } 240 | 241 | bool TCPSocketInterfaceLinux::CreateSocket(NetCommDevTypeDef obj, const char *ip, const char *port) { 242 | if (is_cmd_created_ == true) { 243 | return true; 244 | } 245 | 246 | if ((ip == nullptr) || (port == nullptr)) { 247 | LD_LOG_ERROR("TCP,input ip address or port number is null",""); 248 | return false; 249 | } 250 | 251 | if ((TCP_CLIENT == obj) || (TCP_SERVER == obj)) { 252 | com_sockfd_ = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 253 | if(com_sockfd_ == -1) { 254 | LD_LOG_DEBUG("TCP,fail to socket. %s", strerror(errno)); 255 | return false; 256 | } 257 | } else { 258 | LD_LOG_ERROR("TCP,input value is error",""); 259 | return false; 260 | } 261 | 262 | struct sockaddr_in net_addr; 263 | int addrlen = sizeof(net_addr); 264 | bzero(&net_addr, sizeof(net_addr)); 265 | net_addr.sin_family = AF_INET; 266 | net_addr.sin_addr.s_addr = inet_addr(ip); 267 | net_addr.sin_port = htons((short)atoi(port)); 268 | 269 | ncd_ = obj; 270 | 271 | if (TCP_SERVER == obj) { 272 | // bind sockfd 273 | if (-1 == bind(com_sockfd_,(const struct sockaddr*)&net_addr,(socklen_t)addrlen)) { 274 | LD_LOG_ERROR("TCP,fail to bind. %s", strerror(errno)); 275 | if (com_sockfd_ != -1) { 276 | close(com_sockfd_); 277 | com_sockfd_ = -1; 278 | } 279 | return false; 280 | } 281 | LDS_LOG_INFO("TCP,bind success..",""); 282 | 283 | // sockfd in listen 284 | if (-1 == listen(com_sockfd_, 1)) { 285 | LD_LOG_ERROR("TCP,fail to listen. %s", strerror(errno)); 286 | if (com_sockfd_ != -1) { 287 | close(com_sockfd_); 288 | com_sockfd_ = -1; 289 | } 290 | return false; 291 | } 292 | LDS_LOG_INFO("TCP,wait client connect.",""); 293 | 294 | // wait client request. one by one 295 | struct sockaddr_in client_addr; 296 | int addrlens = sizeof(client_addr); 297 | bzero(&client_addr, sizeof(client_addr)); 298 | 299 | if (-1 == (listend_client_sockfd_ = accept(com_sockfd_, (struct sockaddr*)&client_addr, (socklen_t*)&addrlens))) { 300 | LD_LOG_ERROR("TCP,fail to accept. %s", strerror(errno)); 301 | if (com_sockfd_ != -1) { 302 | close(com_sockfd_); 303 | com_sockfd_ = -1; 304 | } 305 | return false; 306 | } 307 | LDS_LOG_INFO("TCP,connect_client_fd: %d", listend_client_sockfd_); 308 | LDS_LOG_INFO("TCP,client port: %d", ntohs(client_addr.sin_port)); 309 | LDS_LOG_INFO("TCP,client ip: %s", inet_ntoa(client_addr.sin_addr)); 310 | 311 | } else if (TCP_CLIENT == obj) { 312 | // connect to server 313 | if (-1 == connect(com_sockfd_, (const struct sockaddr*)&net_addr, (socklen_t)addrlen)) { 314 | LD_LOG_ERROR("TCP,connect server error. %s", strerror(errno)); 315 | if (com_sockfd_ != -1) { 316 | close(com_sockfd_); 317 | com_sockfd_ = -1; 318 | } 319 | return false; 320 | } 321 | LDS_LOG_INFO("TCP,connect server success.",""); 322 | } 323 | 324 | // create recv process thread. 325 | recv_thread_exit_flag_ = false; 326 | recv_thread_ = new std::thread(RecvThreadProc, this); 327 | is_cmd_created_ = true; 328 | 329 | return true; 330 | } 331 | 332 | bool TCPSocketInterfaceLinux::CloseSocket() { 333 | if (is_cmd_created_ == false) { 334 | return true; 335 | } 336 | 337 | recv_thread_exit_flag_ = true; 338 | 339 | if (com_sockfd_ != -1) { 340 | close(com_sockfd_); 341 | com_sockfd_ = -1; 342 | } 343 | 344 | if (listend_client_sockfd_ != -1) { 345 | close(listend_client_sockfd_); 346 | listend_client_sockfd_ = -1; 347 | } 348 | 349 | if ((recv_thread_ != nullptr) && recv_thread_->joinable()) { 350 | recv_thread_->join(); 351 | delete recv_thread_; 352 | recv_thread_ = nullptr; 353 | } 354 | 355 | is_cmd_created_ = false; 356 | 357 | return true; 358 | } 359 | 360 | bool TCPSocketInterfaceLinux::RecvFromNet(uint8_t *rx_buf , uint32_t rx_buff_len, uint32_t *rx_len) { 361 | static timespec timeout = {0, (long)(100 * 1e6)}; 362 | int32_t len = -1; 363 | 364 | if (IsCreated()) { 365 | if (TCP_CLIENT == ncd_) { 366 | //// io multiplexing 367 | fd_set recv_fds; 368 | FD_ZERO(&recv_fds); 369 | FD_SET(com_sockfd_, &recv_fds); 370 | int ret = pselect(com_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 371 | if (ret < 0) { 372 | // select was interrputed 373 | if (errno == EINTR) { 374 | return false; 375 | } 376 | } else if (ret == 0) { 377 | // timeout 378 | return false; 379 | } 380 | 381 | if (FD_ISSET(com_sockfd_, &recv_fds)) { 382 | len = (int32_t)recv(com_sockfd_, rx_buf, rx_buff_len, 0); 383 | if ((len != -1) && (rx_len != nullptr)) { 384 | *rx_len = len; 385 | } 386 | } 387 | } else if (TCP_SERVER == ncd_) { 388 | fd_set recv_fds; 389 | FD_ZERO(&recv_fds); 390 | FD_SET(listend_client_sockfd_, &recv_fds); 391 | int ret = pselect(listend_client_sockfd_ + 1, &recv_fds, NULL, NULL, &timeout, NULL); 392 | if (ret < 0) { 393 | // select was interrputed 394 | if (errno == EINTR) { 395 | return false; 396 | } 397 | } else if (ret == 0) { 398 | // timeout 399 | return false; 400 | } 401 | 402 | if (FD_ISSET(listend_client_sockfd_, &recv_fds)) { 403 | len = (int32_t)recv(listend_client_sockfd_, rx_buf, rx_buff_len, 0); 404 | if ((len != -1) && (rx_len != nullptr)) { 405 | *rx_len = len; 406 | } 407 | } 408 | } 409 | } 410 | 411 | return ((len == -1) ? false : true); 412 | } 413 | 414 | bool TCPSocketInterfaceLinux::TransToNet(uint8_t *tx_buf, uint32_t tx_buff_len, uint32_t *tx_len) { 415 | int32_t len = 1; 416 | 417 | if (IsCreated()) { 418 | if (TCP_CLIENT == ncd_) { 419 | len = (int32_t)send(com_sockfd_, tx_buf, tx_buff_len, 0); 420 | if ((len != -1) && (tx_len == nullptr)) { 421 | *tx_len = len; 422 | } 423 | } else if (TCP_SERVER == ncd_) { 424 | len = (int32_t)send(listend_client_sockfd_, tx_buf, tx_buff_len, 0); 425 | if ((len != -1) && (tx_len != nullptr)) { 426 | *tx_len = len; 427 | } 428 | } 429 | } 430 | 431 | return ((len == -1) ? false : true); 432 | } 433 | 434 | void TCPSocketInterfaceLinux::SetRecvCallback(std::function callback) { 435 | recv_callback_ = callback; 436 | } 437 | 438 | void TCPSocketInterfaceLinux::RecvThreadProc(void *param) { 439 | TCPSocketInterfaceLinux *cmd_if = (TCPSocketInterfaceLinux *)param; 440 | char *recieve_buff = new char[MAX_RECV_BUF_LEN + 1]; 441 | 442 | while (!cmd_if->recv_thread_exit_flag_.load()) { 443 | uint32_t recved = 0; 444 | bool res = cmd_if->RecvFromNet((uint8_t *)recieve_buff, MAX_RECV_BUF_LEN, &recved); 445 | if (res && recved) { 446 | cmd_if->recv_count_ += recved; 447 | if (cmd_if->recv_callback_ != nullptr) { 448 | cmd_if->recv_callback_(recieve_buff, recved); 449 | } 450 | } 451 | } 452 | 453 | delete[] recieve_buff; 454 | } 455 | 456 | } // namespace ldlidar 457 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 458 | * FILE ********/ -------------------------------------------------------------------------------- /ldlidar_driver/src/serialcom/serial_interface_linux.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmd_interface_linux.cpp 3 | * @author LDRobot (support@ldrobot.com) 4 | * @brief linux serial port App 5 | * @version 0.1 6 | * @date 2021-10-28 7 | * 8 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 9 | * reserved. 10 | * Licensed under the MIT License (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License in the file LICENSE 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include "serial_interface_linux.h" 21 | #include "log_module.h" 22 | 23 | 24 | #define MAX_ACK_BUF_LEN 4096 25 | 26 | namespace ldlidar { 27 | 28 | SerialInterfaceLinux::SerialInterfaceLinux() 29 | : rx_thread_(nullptr), rx_count_(0), read_callback_(nullptr) { 30 | com_handle_ = -1; 31 | com_baudrate_ = 0; 32 | } 33 | 34 | SerialInterfaceLinux::~SerialInterfaceLinux() { Close(); } 35 | 36 | bool SerialInterfaceLinux::Open(std::string &port_name, uint32_t com_baudrate) { 37 | int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); 38 | 39 | com_handle_ = open(port_name.c_str(), flags); 40 | if (-1 == com_handle_) { 41 | LD_LOG_ERROR("Open open error,%s", strerror(errno)); 42 | return false; 43 | } 44 | 45 | com_baudrate_ = com_baudrate; 46 | 47 | struct asmtermios::termios2 options; 48 | if (ioctl(com_handle_, _IOC(_IOC_READ, 'T', 0x2A, sizeof(struct asmtermios::termios2)), &options)) { 49 | LD_LOG_ERROR("TCGETS2 first error,%s", strerror(errno)); 50 | if (com_handle_ != -1) { 51 | close(com_handle_); 52 | com_handle_ = -1; 53 | } 54 | return false; 55 | } 56 | 57 | options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8); 58 | options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB); 59 | options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | 60 | ISIG | IEXTEN); //|ECHOPRT 61 | options.c_oflag &= (tcflag_t) ~(OPOST); 62 | options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK); 63 | 64 | options.c_cflag &= ~CBAUD; 65 | options.c_cflag |= BOTHER; 66 | options.c_ispeed = this->com_baudrate_; 67 | options.c_ospeed = this->com_baudrate_; 68 | 69 | options.c_cc[VMIN] = 0; 70 | options.c_cc[VTIME] = 0; 71 | 72 | if (ioctl(com_handle_, _IOC(_IOC_WRITE, 'T', 0x2B, sizeof(struct asmtermios::termios2)), &options)) { 73 | LD_LOG_ERROR("TCSETS2 error,%s", strerror(errno)); 74 | if (com_handle_ != -1) { 75 | close(com_handle_); 76 | com_handle_ = -1; 77 | } 78 | return false; 79 | } 80 | 81 | if (ioctl(com_handle_, _IOC(_IOC_READ, 'T', 0x2A, sizeof(struct asmtermios::termios2)), &options)) { 82 | LD_LOG_ERROR("TCGETS2 second error,%s", strerror(errno)); 83 | if (com_handle_ != -1) { 84 | close(com_handle_); 85 | com_handle_ = -1; 86 | } 87 | return false; 88 | } 89 | 90 | LDS_LOG_INFO("Actual BaudRate reported:%d", options.c_ospeed); 91 | 92 | tcflush(com_handle_, TCIFLUSH); 93 | 94 | rx_thread_exit_flag_ = false; 95 | rx_thread_ = new std::thread(RxThreadProc, this); 96 | is_cmd_opened_ = true; 97 | 98 | return true; 99 | } 100 | 101 | bool SerialInterfaceLinux::Close() { 102 | if (is_cmd_opened_ == false) { 103 | return true; 104 | } 105 | 106 | rx_thread_exit_flag_ = true; 107 | 108 | if (com_handle_ != -1) { 109 | close(com_handle_); 110 | com_handle_ = -1; 111 | } 112 | 113 | if ((rx_thread_ != nullptr) && rx_thread_->joinable()) { 114 | rx_thread_->join(); 115 | delete rx_thread_; 116 | rx_thread_ = nullptr; 117 | } 118 | 119 | is_cmd_opened_ = false; 120 | 121 | return true; 122 | } 123 | 124 | bool SerialInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, 125 | uint32_t *rx_len) { 126 | static timespec timeout = {0, (long)(100 * 1e6)}; 127 | int32_t len = -1; 128 | 129 | if (IsOpened()) { 130 | fd_set read_fds; 131 | FD_ZERO(&read_fds); 132 | FD_SET(com_handle_, &read_fds); 133 | int r = pselect(com_handle_ + 1, &read_fds, NULL, NULL, &timeout, NULL); 134 | if (r < 0) { 135 | // Select was interrupted 136 | if (errno == EINTR) { 137 | return false; 138 | } 139 | } else if (r == 0) { // timeout 140 | return false; 141 | } 142 | 143 | if (FD_ISSET(com_handle_, &read_fds)) { 144 | len = (int32_t)read(com_handle_, rx_buf, rx_buf_len); 145 | if ((len != -1) && rx_len) { 146 | *rx_len = len; 147 | } 148 | } 149 | } 150 | return len == -1 ? false : true; 151 | } 152 | 153 | bool SerialInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, 154 | uint32_t *tx_len) { 155 | int32_t len = -1; 156 | 157 | if (IsOpened()) { 158 | len = (int32_t)write(com_handle_, tx_buf, tx_buf_len); 159 | if ((len != -1) && tx_len) { 160 | *tx_len = len; 161 | } 162 | } 163 | return len == -1 ? false : true; 164 | } 165 | 166 | void SerialInterfaceLinux::RxThreadProc(void *param) { 167 | SerialInterfaceLinux *cmd_if = (SerialInterfaceLinux *)param; 168 | char *rx_buf = new char[MAX_ACK_BUF_LEN + 1]; 169 | while (!cmd_if->rx_thread_exit_flag_.load()) { 170 | uint32_t readed = 0; 171 | bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed); 172 | if (res && readed) { 173 | cmd_if->rx_count_ += readed; 174 | if (cmd_if->read_callback_ != nullptr) { 175 | cmd_if->read_callback_(rx_buf, readed); 176 | } 177 | } 178 | } 179 | 180 | delete[] rx_buf; 181 | } 182 | 183 | } // namespace ldlidar 184 | 185 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 186 | * FILE ********/ -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ldlidar_stl_ros 4 | 3.0.0 5 | The ldlidar package 6 | 7 | 8 | 9 | 10 | SHENZHEN LDROBOT CO., LTD. 11 | 12 | 13 | 14 | 15 | 16 | MIT 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 | 52 | catkin 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | roscpp 76 | sensor_msgs 77 | tf 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /rules/ldlidar.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by ldlidar 2 | # CP210x USB Device 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="ldlidar" 4 | 5 | -------------------------------------------------------------------------------- /rviz/ldlidar_kinetic_melodic.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 613 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 20 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/Axes 55 | Enabled: false 56 | Length: 1 57 | Name: Axes 58 | Radius: 0.100000001 59 | Reference Frame: 60 | Value: false 61 | - Class: rviz/TF 62 | Enabled: true 63 | Frame Timeout: 15 64 | Frames: 65 | All Enabled: true 66 | base_laser: 67 | Value: true 68 | base_link: 69 | Value: true 70 | Marker Scale: 1 71 | Name: TF 72 | Show Arrows: true 73 | Show Axes: true 74 | Show Names: true 75 | Tree: 76 | base_link: 77 | base_laser: 78 | {} 79 | Update Interval: 0 80 | Value: true 81 | - Alpha: 1 82 | Autocompute Intensity Bounds: true 83 | Autocompute Value Bounds: 84 | Max Value: 10 85 | Min Value: -10 86 | Value: true 87 | Axis: Z 88 | Channel Name: intensity 89 | Class: rviz/LaserScan 90 | Color: 255; 255; 255 91 | Color Transformer: Intensity 92 | Decay Time: 0 93 | Enabled: true 94 | Invert Rainbow: false 95 | Max Color: 255; 255; 255 96 | Max Intensity: 249 97 | Min Color: 0; 0; 0 98 | Min Intensity: 35 99 | Name: LaserScan 100 | Position Transformer: XYZ 101 | Queue Size: 10 102 | Selectable: true 103 | Size (Pixels): 3 104 | Size (m): 0.00999999978 105 | Style: Points 106 | Topic: /scan 107 | Unreliable: false 108 | Use Fixed Frame: true 109 | Use rainbow: true 110 | Value: true 111 | Enabled: true 112 | Global Options: 113 | Background Color: 48; 48; 48 114 | Default Light: true 115 | Fixed Frame: base_laser 116 | Frame Rate: 30 117 | Name: root 118 | Tools: 119 | - Class: rviz/Interact 120 | Hide Inactive Objects: true 121 | - Class: rviz/MoveCamera 122 | - Class: rviz/Select 123 | - Class: rviz/FocusCamera 124 | - Class: rviz/Measure 125 | - Class: rviz/SetInitialPose 126 | Topic: /initialpose 127 | - Class: rviz/SetGoal 128 | Topic: /move_base_simple/goal 129 | - Class: rviz/PublishPoint 130 | Single click: true 131 | Topic: /clicked_point 132 | Value: true 133 | Views: 134 | Current: 135 | Class: rviz/Orbit 136 | Distance: 26.3351135 137 | Enable Stereo Rendering: 138 | Stereo Eye Separation: 0.0599999987 139 | Stereo Focal Distance: 1 140 | Swap Stereo Eyes: false 141 | Value: false 142 | Focal Point: 143 | X: 0 144 | Y: 0 145 | Z: 0 146 | Focal Shape Fixed Size: true 147 | Focal Shape Size: 0.0500000007 148 | Invert Z Axis: false 149 | Name: Current View 150 | Near Clip Distance: 0.00999999978 151 | Pitch: 1.56979632 152 | Target Frame: 153 | Value: Orbit (rviz) 154 | Yaw: 3.13039947 155 | Saved: ~ 156 | Window Geometry: 157 | Displays: 158 | collapsed: false 159 | Height: 894 160 | Hide Left Dock: false 161 | Hide Right Dock: false 162 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002f4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007450000003efc0100000002fb0000000800540069006d00650100000000000007450000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004c0000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 163 | Selection: 164 | collapsed: false 165 | Time: 166 | collapsed: false 167 | Tool Properties: 168 | collapsed: false 169 | Views: 170 | collapsed: false 171 | Width: 1861 172 | X: 57 173 | Y: 24 174 | -------------------------------------------------------------------------------- /rviz/ldlidar_noetic.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 | - /TF1 10 | - /LaserScan1 11 | Splitter Ratio: 0.5 12 | Tree Height: 557 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.5886790156364441 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 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 20 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/Axes 59 | Enabled: false 60 | Length: 1 61 | Name: Axes 62 | Radius: 0.05000000074505806 63 | Reference Frame: 64 | Show Trail: false 65 | Value: false 66 | - Class: rviz/TF 67 | Enabled: true 68 | Frame Timeout: 15 69 | Frames: 70 | All Enabled: true 71 | base_laser: 72 | Value: true 73 | base_link: 74 | Value: true 75 | Marker Alpha: 1 76 | Marker Scale: 1 77 | Name: TF 78 | Show Arrows: true 79 | Show Axes: true 80 | Show Names: true 81 | Tree: 82 | base_link: 83 | base_laser: 84 | {} 85 | Update Interval: 0 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: 10 91 | Min Value: -10 92 | Value: true 93 | Axis: Z 94 | Channel Name: intensity 95 | Class: rviz/LaserScan 96 | Color: 255; 255; 255 97 | Color Transformer: Intensity 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Min Color: 0; 0; 0 103 | Name: LaserScan 104 | Position Transformer: XYZ 105 | Queue Size: 10 106 | Selectable: true 107 | Size (Pixels): 3 108 | Size (m): 0.029999999329447746 109 | Style: Flat Squares 110 | Topic: /scan 111 | Unreliable: false 112 | Use Fixed Frame: true 113 | Use rainbow: true 114 | Value: true 115 | Enabled: true 116 | Global Options: 117 | Background Color: 48; 48; 48 118 | Default Light: true 119 | Fixed Frame: base_laser 120 | Frame Rate: 30 121 | Name: root 122 | Tools: 123 | - Class: rviz/Interact 124 | Hide Inactive Objects: true 125 | - Class: rviz/MoveCamera 126 | - Class: rviz/Select 127 | - Class: rviz/FocusCamera 128 | - Class: rviz/Measure 129 | - Class: rviz/SetInitialPose 130 | Theta std deviation: 0.2617993950843811 131 | Topic: /initialpose 132 | X std deviation: 0.5 133 | Y std deviation: 0.5 134 | - Class: rviz/SetGoal 135 | Topic: /move_base_simple/goal 136 | - Class: rviz/PublishPoint 137 | Single click: true 138 | Topic: /clicked_point 139 | Value: true 140 | Views: 141 | Current: 142 | Class: rviz/Orbit 143 | Distance: 11.079298973083496 144 | Enable Stereo Rendering: 145 | Stereo Eye Separation: 0.05999999865889549 146 | Stereo Focal Distance: 1 147 | Swap Stereo Eyes: false 148 | Value: false 149 | Field of View: 0.7853981852531433 150 | Focal Point: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Focal Shape Fixed Size: true 155 | Focal Shape Size: 0.05000000074505806 156 | Invert Z Axis: false 157 | Name: Current View 158 | Near Clip Distance: 0.009999999776482582 159 | Pitch: 1.4397964477539062 160 | Target Frame: 161 | Yaw: 3.135404348373413 162 | Saved: ~ 163 | Window Geometry: 164 | Displays: 165 | collapsed: false 166 | Height: 854 167 | Hide Left Dock: false 168 | Hide Right Dock: false 169 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b8000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007400000003efc0100000002fb0000000800540069006d0065010000000000000740000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cf000002b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 170 | Selection: 171 | collapsed: false 172 | Time: 173 | collapsed: false 174 | Tool Properties: 175 | collapsed: false 176 | Views: 177 | collapsed: false 178 | Width: 1856 179 | X: 62 180 | Y: 27 181 | -------------------------------------------------------------------------------- /scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "remap the device serial port(ttyUSBX) to ldlidar" 4 | echo "ldlidar usb connection as /dev/ldlidar , check it using the command : ls -l /dev|grep ttyUSB" 5 | echo "start copy ldlidar.rules to /etc/udev/rules.d/" 6 | sudo cp `rospack find ldlidar_stl_ros`/rules/ldlidar.rules /etc/udev/rules.d/ 7 | echo " " 8 | echo "Restarting udev" 9 | echo "" 10 | sudo udevadm control --reload-rules 11 | sudo service udev restart 12 | sudo udevadm trigger 13 | echo "finish " 14 | -------------------------------------------------------------------------------- /scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "delete remap the device serial port(ttyUSBX) to ldlidar" 4 | echo "sudo rm /etc/udev/rules.d/ldlidar.rules" 5 | sudo rm /etc/udev/rules.d/ldlidar.rules 6 | echo " " 7 | echo "Restarting udev" 8 | echo "" 9 | sudo udevadm control --reload-rules 10 | sudo service udev restart 11 | sudo udevadm trigger 12 | echo "finish delete" 13 | -------------------------------------------------------------------------------- /src/listen_node/listen_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file listen_node.cpp 3 | * @author LDRobot (marketing1@ldrobot.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022.04.08 7 | * @note 8 | * @copyright Copyright (c) 2020 SHENZHEN LDROBOT CO., LTD. All rights reserved. 9 | * Licensed under the MIT License (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License in the file LICENSE 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | **/ 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #define RADIAN_TO_DEGREES(angle) ((angle)*180000/3141.59) 25 | 26 | void LidarMsgCallback(const sensor_msgs::LaserScan::ConstPtr& data) 27 | { 28 | ROS_INFO_STREAM("[ldrobot]------listen lidar message-------"); 29 | unsigned int lens = static_cast((data->angle_max - data->angle_min) / data->angle_increment); 30 | ROS_INFO_STREAM("[ldrobot] angle_min: " << RADIAN_TO_DEGREES(data->angle_min) << " " 31 | << "angle_max: " << RADIAN_TO_DEGREES(data->angle_max)); 32 | ROS_INFO_STREAM("[ldrobot] point size: " << data->ranges.size()); 33 | for (unsigned int i = 0; i < lens; i++) { 34 | ROS_INFO_STREAM("[ldrobot] angle: " << RADIAN_TO_DEGREES((data->angle_min + i * data->angle_increment)) << " " 35 | << "range: " << data->ranges[i] << " " 36 | << "intensites: " << data->intensities[i]); 37 | } 38 | } 39 | 40 | int main(int argc, char **argv) 41 | { 42 | ros::init(argc, argv, "ldldiar_listen_node"); 43 | ros::NodeHandle nh; // create a ROS Node 44 | ros::NodeHandle n("~"); 45 | std::string topic_name; 46 | 47 | n.param("topic_name", topic_name, std::string("scan")); 48 | 49 | if (topic_name.empty()) { 50 | ROS_ERROR("[ldrobot] [ldldiar_listen_node] input param is null"); 51 | exit(EXIT_FAILURE); 52 | } else { 53 | ROS_INFO("[ldrobot] [ldldiar_listen_node] input param is %s", topic_name.c_str()); 54 | } 55 | 56 | ros::Subscriber msg_subs = nh.subscribe(topic_name, 10, &LidarMsgCallback); 57 | ROS_INFO("[ldrobot] start ldldiar message subscribe node"); 58 | 59 | ros::Rate loop_rate(10); 60 | while (ros::ok()) { 61 | ros::spinOnce(); 62 | loop_rate.sleep(); 63 | } 64 | 65 | return 0; 66 | } 67 | 68 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/ 69 | -------------------------------------------------------------------------------- /src/publish_node/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author LDRobot (marketing1@ldrobot.com) 4 | * @brief main process App 5 | * This code is only applicable to LDROBOT LiDAR LD06 products 6 | * sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-10-28 9 | * 10 | * @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights 11 | * reserved. 12 | * Licensed under the MIT License (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License in the file LICENSE 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | #include "ros_api.h" 22 | #include "ldlidar_driver.h" 23 | 24 | void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq, 25 | LaserScanSetting& setting, ros::Publisher& lidarpub); 26 | 27 | uint64_t GetSystemTimeStamp(void); 28 | 29 | int main(int argc, char **argv) { 30 | ros::init(argc, argv, "ldldiar_publisher"); 31 | ros::NodeHandle nh; // create a ROS Node 32 | ros::NodeHandle nh_private("~"); 33 | std::string product_name; 34 | std::string topic_name; 35 | std::string port_name; 36 | int serial_port_baudrate; 37 | LaserScanSetting setting; 38 | ldlidar::LDType type_name; 39 | 40 | nh_private.getParam("product_name", product_name); 41 | nh_private.getParam("topic_name", topic_name); 42 | nh_private.param("frame_id", setting.frame_id, std::string("base_laser")); 43 | nh_private.getParam("port_name", port_name); 44 | nh_private.param("port_baudrate", serial_port_baudrate, int(230400)); 45 | nh_private.param("laser_scan_dir", setting.laser_scan_dir, bool(true)); 46 | nh_private.param("enable_angle_crop_func", setting.enable_angle_crop_func, bool(false)); 47 | nh_private.param("angle_crop_min", setting.angle_crop_min, double(0.0)); 48 | nh_private.param("angle_crop_max", setting.angle_crop_max, double(0.0)); 49 | 50 | ldlidar::LDLidarDriver* ldlidarnode = new ldlidar::LDLidarDriver(); 51 | 52 | ROS_INFO("LDLiDAR SDK Pack Version is: %s", ldlidarnode->GetLidarSdkVersionNumber().c_str()); 53 | ROS_INFO("ROS params input:"); 54 | ROS_INFO(": %s", product_name.c_str()); 55 | ROS_INFO(": %s", topic_name.c_str()); 56 | ROS_INFO(": %s", setting.frame_id.c_str()); 57 | ROS_INFO(": %s", port_name.c_str()); 58 | ROS_INFO(": %d", serial_port_baudrate); 59 | ROS_INFO(": %s", (setting.laser_scan_dir?"Counterclockwise":"Clockwise")); 60 | ROS_INFO(": %s", (setting.enable_angle_crop_func?"true":"false")); 61 | ROS_INFO(": %f", setting.angle_crop_min); 62 | ROS_INFO(": %f", setting.angle_crop_max); 63 | 64 | if (product_name == "LDLiDAR_LD06") { 65 | type_name = ldlidar::LDType::LD_06; 66 | } else if (product_name == "LDLiDAR_LD19") { 67 | type_name = ldlidar::LDType::LD_19; 68 | } else { 69 | ROS_ERROR("Error, input is illegal."); 70 | exit(EXIT_FAILURE); 71 | } 72 | 73 | ldlidarnode->RegisterGetTimestampFunctional(std::bind(&GetSystemTimeStamp)); 74 | 75 | ldlidarnode->EnableFilterAlgorithnmProcess(true); 76 | 77 | if (ldlidarnode->Start(type_name, port_name, serial_port_baudrate, ldlidar::COMM_SERIAL_MODE)) { 78 | ROS_INFO("ldlidar node start is success"); 79 | } else { 80 | ROS_ERROR("ldlidar node start is fail"); 81 | exit(EXIT_FAILURE); 82 | } 83 | 84 | if (ldlidarnode->WaitLidarCommConnect(3000)) { 85 | ROS_INFO("ldlidar communication is normal."); 86 | } else { 87 | ROS_ERROR("ldlidar communication is abnormal."); 88 | exit(EXIT_FAILURE); 89 | } 90 | 91 | 92 | ros::Publisher lidar_pub = 93 | nh.advertise(topic_name, 10); // create a ROS topic 94 | 95 | ros::Rate r(10); //10hz 96 | ldlidar::Points2D laser_scan_points; 97 | double lidar_scan_freq; 98 | ROS_INFO("Publish topic message:ldlidar scan data ."); 99 | 100 | while (ros::ok()) { 101 | 102 | switch (ldlidarnode->GetLaserScanData(laser_scan_points, 1500)){ 103 | case ldlidar::LidarStatus::NORMAL: 104 | ldlidarnode->GetLidarScanFreq(lidar_scan_freq); 105 | ToLaserscanMessagePublish(laser_scan_points, lidar_scan_freq, setting, lidar_pub); 106 | break; 107 | case ldlidar::LidarStatus::DATA_TIME_OUT: 108 | ROS_ERROR("get ldlidar data is time out, please check your lidar device."); 109 | break; 110 | case ldlidar::LidarStatus::DATA_WAIT: 111 | break; 112 | default: 113 | break; 114 | } 115 | 116 | r.sleep(); 117 | } 118 | 119 | ldlidarnode->Stop(); 120 | 121 | delete ldlidarnode; 122 | ldlidarnode = nullptr; 123 | 124 | return 0; 125 | } 126 | 127 | void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq, 128 | LaserScanSetting& setting, ros::Publisher& lidarpub) { 129 | float angle_min, angle_max, range_min, range_max, angle_increment; 130 | float scan_time; 131 | ros::Time start_scan_time; 132 | static ros::Time end_scan_time; 133 | static bool first_scan = true; 134 | 135 | start_scan_time = ros::Time::now(); 136 | scan_time = (start_scan_time - end_scan_time).toSec(); 137 | 138 | if (first_scan) { 139 | first_scan = false; 140 | end_scan_time = start_scan_time; 141 | return; 142 | } 143 | 144 | // Adjust the parameters according to the demand 145 | angle_min = 0; 146 | angle_max = (2 * M_PI); 147 | range_min = 0.02; 148 | range_max = 12; 149 | int beam_size = static_cast(src.size()); 150 | angle_increment = (angle_max - angle_min) / (float)(beam_size -1); 151 | 152 | // Calculate the number of scanning points 153 | if (lidar_spin_freq > 0) { 154 | sensor_msgs::LaserScan output; 155 | output.header.stamp = start_scan_time; 156 | output.header.frame_id = setting.frame_id; 157 | output.angle_min = angle_min; 158 | output.angle_max = angle_max; 159 | output.range_min = range_min; 160 | output.range_max = range_max; 161 | output.angle_increment = angle_increment; 162 | if (beam_size <= 1) { 163 | output.time_increment = 0; 164 | } else { 165 | output.time_increment = scan_time / (float)(beam_size - 1); 166 | } 167 | output.scan_time = scan_time; 168 | // First fill all the data with Nan 169 | output.ranges.assign(beam_size, std::numeric_limits::quiet_NaN()); 170 | output.intensities.assign(beam_size, std::numeric_limits::quiet_NaN()); 171 | 172 | for (auto point : src) { 173 | float range = point.distance / 1000.f; // distance unit transform to meters 174 | float intensity = point.intensity; // laser receive intensity 175 | float dir_angle = point.angle; 176 | 177 | if ((point.distance == 0) && (point.intensity == 0)) { // filter is handled to 0, Nan will be assigned variable. 178 | range = std::numeric_limits::quiet_NaN(); 179 | intensity = std::numeric_limits::quiet_NaN(); 180 | } 181 | 182 | if (setting.enable_angle_crop_func) { // Angle crop setting, Mask data within the set angle range 183 | if ((dir_angle >= setting.angle_crop_min) && (dir_angle <= setting.angle_crop_max)) { 184 | range = std::numeric_limits::quiet_NaN(); 185 | intensity = std::numeric_limits::quiet_NaN(); 186 | } 187 | } 188 | 189 | float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian 190 | int index = static_cast(ceil((angle - angle_min) / angle_increment)); 191 | if (index < beam_size) { 192 | if (index < 0) { 193 | ROS_ERROR("[ldrobot] error index: %d, beam_size: %d, angle: %f, angle_min: %f, angle_increment: %f", 194 | index, beam_size, angle, angle_min, angle_increment); 195 | } 196 | 197 | if (setting.laser_scan_dir) { 198 | int index_anticlockwise = beam_size - index - 1; 199 | // If the current content is Nan, it is assigned directly 200 | if (std::isnan(output.ranges[index_anticlockwise])) { 201 | output.ranges[index_anticlockwise] = range; 202 | } else { // Otherwise, only when the distance is less than the current 203 | // value, it can be re assigned 204 | if (range < output.ranges[index_anticlockwise]) { 205 | output.ranges[index_anticlockwise] = range; 206 | } 207 | } 208 | output.intensities[index_anticlockwise] = intensity; 209 | } else { 210 | // If the current content is Nan, it is assigned directly 211 | if (std::isnan(output.ranges[index])) { 212 | output.ranges[index] = range; 213 | } else { // Otherwise, only when the distance is less than the current 214 | // value, it can be re assigned 215 | if (range < output.ranges[index]) { 216 | output.ranges[index] = range; 217 | } 218 | } 219 | output.intensities[index] = intensity; 220 | } 221 | } 222 | } 223 | lidarpub.publish(output); 224 | end_scan_time = start_scan_time; 225 | } 226 | } 227 | 228 | uint64_t GetSystemTimeStamp(void) { 229 | std::chrono::time_point tp = 230 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 231 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 232 | return ((uint64_t)tmp.count()); 233 | } 234 | 235 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 236 | * FILE ********/ 237 | --------------------------------------------------------------------------------