├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── README_CN.md ├── include └── ros2_api.h ├── launch ├── ld06.launch.py ├── ld14.launch.py ├── ld14p.launch.py ├── ld19.launch.py ├── viewer_ld06.launch.py ├── viewer_ld14.launch.py ├── viewer_ld14p.launch.py └── viewer_ld19.launch.py ├── package.xml ├── rviz2 └── ldlidar.rviz ├── scripts ├── create_udev_rules.sh ├── delete_udev_rules.sh └── ldlidar.rules └── src └── demo.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | *.json -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "sdk"] 2 | path = sdk 3 | url=https://github.com/ldrobotSensorTeam/ldlidar_sdk.git -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ldlidar_ros2) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(sensor_msgs REQUIRED) 22 | find_package(geometry_msgs REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # uncomment the line when a copyright and license is not present in all source files 28 | #set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # uncomment the line when this package is not in a git repo 31 | #set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | # user add 36 | include_directories( 37 | ${CMAKE_CURRENT_SOURCE_DIR}/include 38 | ${CMAKE_CURRENT_SOURCE_DIR}/sdk/include/ 39 | ) 40 | 41 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/sdk) 42 | 43 | add_executable(${PROJECT_NAME}_node 44 | ${CMAKE_CURRENT_SOURCE_DIR}/src/demo.cpp 45 | ) 46 | ament_target_dependencies(${PROJECT_NAME}_node rclcpp sensor_msgs geometry_msgs) 47 | target_link_libraries(${PROJECT_NAME}_node 48 | pthread 49 | ldlidar_driver 50 | ) 51 | 52 | # Install 53 | install(TARGETS ${PROJECT_NAME}_node 54 | DESTINATION lib/${PROJECT_NAME} 55 | ) 56 | 57 | install(DIRECTORY launch rviz2 scripts 58 | DESTINATION share/${PROJECT_NAME}/ 59 | ) 60 | 61 | 62 | ament_package() 63 | 64 | -------------------------------------------------------------------------------- /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 | # Instructions 2 | 3 | ## step 0: get LiDAR ROS2 Package 4 | ```bash 5 | cd ~ 6 | 7 | mkdir -p ldlidar_ros2_ws/src 8 | 9 | cd ldlidar_ros2_ws/src 10 | 11 | git clone https://github.com/ldrobotSensorTeam/ldlidar_ros2.git 12 | 13 | git submodule update --init --recursive 14 | ``` 15 | ## step 1: system setup 16 | - Connect the LiDAR to your system motherboard via an onboard serial port or usB-to-serial module (for example, CP2102 module). 17 | 18 | - Set the -x permission for the serial port device mounted by the radar in the system (for example, /dev/ttyUSB0) 19 | 20 | - 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. 21 | 22 | ``` bash 23 | cd ~/ldlidar_ros2_ws 24 | 25 | sudo chmod 777 /dev/ttyUSB0 26 | ``` 27 | - Modify the `port_name` value in the Lanuch file corresponding to the radar product model under `launch/`, using `ld14.launch.py` as an example, as shown below. 28 | 29 | ```py 30 | #!/usr/bin/env python3 31 | from launch import LaunchDescription 32 | from launch_ros.actions import Node 33 | 34 | ''' 35 | Parameter Description: 36 | --- 37 | - Set laser scan directon: 38 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 39 | 2. Set clockwise, example: {'laser_scan_dir': False} 40 | - Angle crop setting, Mask data within the set angle range: 41 | 1. Enable angle crop fuction: 42 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 43 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 44 | 2. Angle cropping interval setting: 45 | - The distance and intensity data within the set angle range will be set to 0. 46 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 47 | example: 48 | {'angle_crop_min': 135.0} 49 | {'angle_crop_max': 225.0} 50 | which is [135.0, 225.0], angle unit is degress. 51 | ''' 52 | 53 | def generate_launch_description(): 54 | # LDROBOT LiDAR publisher node 55 | ldlidar_node = Node( 56 | package='ldlidar_ros2', 57 | executable='ldlidar_ros2_node', 58 | name='ldlidar_publisher_ld14', 59 | output='screen', 60 | parameters=[ 61 | {'product_name': 'LDLiDAR_LD14'}, 62 | {'laser_scan_topic_name': 'scan'}, 63 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 64 | {'frame_id': 'base_laser'}, 65 | {'port_name': '/dev/ttyUSB0'}, 66 | {'serial_baudrate' : 115200}, 67 | {'laser_scan_dir': True}, 68 | {'enable_angle_crop_func': False}, 69 | {'angle_crop_min': 135.0}, 70 | {'angle_crop_max': 225.0} 71 | ] 72 | ) 73 | 74 | # base_link to base_laser tf node 75 | base_link_to_laser_tf_node = Node( 76 | package='tf2_ros', 77 | executable='static_transform_publisher', 78 | name='base_link_to_base_laser_ld14', 79 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 80 | ) 81 | 82 | 83 | # Define LaunchDescription variable 84 | ld = LaunchDescription() 85 | 86 | ld.add_action(ldlidar_node) 87 | ld.add_action(base_link_to_laser_tf_node) 88 | 89 | return ld 90 | ``` 91 | 92 | ## step 2: build 93 | 94 | Run the following command. 95 | 96 | ```bash 97 | cd ~/ldlidar_ros2_ws 98 | 99 | colcon build 100 | ``` 101 | ## step 3: run 102 | 103 | ### step3.1: package environment variable settings 104 | 105 | - 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. 106 | 107 | ```bash 108 | cd ~/ldlidar_ros2_ws 109 | 110 | source install/local_setup.bash 111 | ``` 112 | 113 | - In order to never need to execute the above command to add environment variables after reopening the terminal, you can do the following. 114 | 115 | ```bash 116 | echo "source ~/ldlidar_ros2_ws/install/local_setup.bash" >> ~/.bashrc 117 | 118 | source ~/.bashrc 119 | ``` 120 | ### step3.2: start LiDAR node 121 | 122 | - The product is LDROBOT LiDAR LD14 123 | - start ld14 lidar node: 124 | ``` bash 125 | ros2 launch ldlidar_ros2 ld14.launch.py 126 | ``` 127 | - start ld14 lidar node and show on the Rviz2: 128 | ``` bash 129 | ros2 launch ldlidar_ros2 viewer_ld14.launch.py 130 | ``` 131 | 132 | - The product is LDROBOT LiDAR LD14P 133 | - start ld14p lidar node: 134 | ``` bash 135 | ros2 launch ldlidar_ros2 ld14p.launch.py 136 | ``` 137 | - start ld14p lidar node and show on the Rviz2: 138 | ``` bash 139 | ros2 launch ldlidar_ros2 viewer_ld14p.launch.py 140 | ``` 141 | 142 | - The product is LDROBOT LiDAR LD06 143 | - start ld06 lidar node: 144 | ``` bash 145 | ros2 launch ldlidar_ros2 ld06.launch.py 146 | ``` 147 | - start ld06 lidar node and show on the Rviz2: 148 | ``` bash 149 | ros2 launch ldlidar_ros2 viewer_ld06.launch.py 150 | ``` 151 | 152 | - The product is LDROBOT LiDAR LD19 153 | - start ld19 lidar node: 154 | ``` bash 155 | ros2 launch ldlidar_ros2 ld19.launch.py 156 | ``` 157 | - start ld19 lidar node and show on the Rviz2: 158 | ``` bash 159 | ros2 launch ldlidar_ros2 viewer_ld19.launch.py 160 | ``` 161 | 162 | ## step 4: Data visualization 163 | 164 | > The code supports ubuntu 20.04 ros2 foxy version and above, using rviz2 visualization. 165 | 166 | - new a terminal (Ctrl + Alt + T) and use Rviz2 tool(run command: `rviz2`) ,open the `ldlidar.rviz` file below the rviz2 folder of the readme file directory 167 | ```bash 168 | rviz2 169 | ``` 170 | -------------------------------------------------------------------------------- /README_CN.md: -------------------------------------------------------------------------------- 1 | # 操作指南 2 | 3 | ## 0. 获取激光雷达的ROS2功能包 4 | ```bash 5 | cd ~ 6 | 7 | mkdir -p ldlidar_ros2_ws/src 8 | 9 | cd ldlidar_ros2_ws/src 10 | 11 | git clone https://github.com/ldrobotSensorTeam/ldlidar_ros2.git 12 | 13 | git submodule update --init --recursive 14 | ``` 15 | ## 1. 系统设置 16 | - 第一步，通过板载串口或者USB转串口模块(例如,cp2102模块)的方式使雷达连接到你的系统主板. 17 | - 第二步，设置雷达在系统中挂载的串口设备-x权限(以/dev/ttyUSB0为例) 18 | - 实际使用时，根据雷达在你的系统中的实际挂载情况来设置，可以使用`ls -l /dev`命令查看. 19 | 20 | ``` bash 21 | cd ~/ldlidar_ros2_ws 22 | 23 | sudo chmod 777 /dev/ttyUSB0 24 | ``` 25 | - 第三步，修改`launch/`目录下雷达产品型号对应的lanuch文件中的`port_name`值，以ld14.launch.py为例，如下所示. 26 | 27 | ```py 28 | #!/usr/bin/env python3 29 | from launch import LaunchDescription 30 | from launch_ros.actions import Node 31 | 32 | ''' 33 | Parameter Description: 34 | --- 35 | - Set laser scan directon: 36 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 37 | 2. Set clockwise, example: {'laser_scan_dir': False} 38 | - Angle crop setting, Mask data within the set angle range: 39 | 1. Enable angle crop fuction: 40 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 41 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 42 | 2. Angle cropping interval setting: 43 | - The distance and intensity data within the set angle range will be set to 0. 44 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 45 | example: 46 | {'angle_crop_min': 135.0} 47 | {'angle_crop_max': 225.0} 48 | which is [135.0, 225.0], angle unit is degress. 49 | ''' 50 | 51 | def generate_launch_description(): 52 | # LDROBOT LiDAR publisher node 53 | ldlidar_node = Node( 54 | package='ldlidar_ros2', 55 | executable='ldlidar_ros2_node', 56 | name='ldlidar_publisher_ld14', 57 | output='screen', 58 | parameters=[ 59 | {'product_name': 'LDLiDAR_LD14'}, 60 | {'laser_scan_topic_name': 'scan'}, 61 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 62 | {'frame_id': 'base_laser'}, 63 | {'port_name': '/dev/ttyUSB0'}, 64 | {'serial_baudrate' : 115200}, 65 | {'laser_scan_dir': True}, 66 | {'enable_angle_crop_func': False}, 67 | {'angle_crop_min': 135.0}, 68 | {'angle_crop_max': 225.0} 69 | ] 70 | ) 71 | 72 | # base_link to base_laser tf node 73 | base_link_to_laser_tf_node = Node( 74 | package='tf2_ros', 75 | executable='static_transform_publisher', 76 | name='base_link_to_base_laser_ld14', 77 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 78 | ) 79 | 80 | 81 | # Define LaunchDescription variable 82 | ld = LaunchDescription() 83 | 84 | ld.add_action(ldlidar_node) 85 | ld.add_action(base_link_to_laser_tf_node) 86 | 87 | return ld 88 | ``` 89 | ## 2. 编译方法 90 | 91 | 使用colcon编译. 92 | 93 | ```bash 94 | cd ~/ldlidar_ros2_ws 95 | 96 | colcon build 97 | ``` 98 | ## 3. 运行方法 99 | ### 3.1. 设置功能包环境变量 100 | 101 | - 编译完成后需要将编译生成的相关文件加入环境变量，便于 ROS 环境可以识别， 执行命令如下所示， 该命令是临时给终端加入环境变量，意味着您如果重新打开新的终端，也需要重新执行如下命令. 102 | 103 | ```bash 104 | cd ~/ldlidar_ros2_ws 105 | 106 | source install/local_setup.bash 107 | ``` 108 | - 为了重新打开终端后，永久不用执行上述添加环境变量的命令，可以进行如下操作. 109 | 110 | ```bash 111 | echo "source ~/ldlidar_ros2_ws/install/local_setup.bash" >> ~/.bashrc 112 | 113 | source ~/.bashrc 114 | ``` 115 | ### 3.2. 启动激光雷达节点 116 | 117 | - 产品型号为 LDROBOT LiDAR LD14 118 | - 启动ld14 lidar node: 119 | ``` bash 120 | ros2 launch ldlidar_ros2 ld14.launch.py 121 | ``` 122 | - 启动ld14 lidar node并显示激光数据在Rviz2上: 123 | ``` bash 124 | ros2 launch ldlidar_ros2 viewer_ld14.launch.py 125 | ``` 126 | 127 | - 产品型号为 LDROBOT LiDAR LD14P 128 | - 启动ld14p lidar node: 129 | ``` bash 130 | ros2 launch ldlidar_ros2 ld14p.launch.py 131 | ``` 132 | - 启动ld14p lidar node并显示激光数据在Rviz2上: 133 | ``` bash 134 | ros2 launch ldlidar_ros2 viewer_ld14p.launch.py 135 | ``` 136 | 137 | - 产品型号为 LDROBOT LiDAR LD06 138 | - 启动ld06 lidar node: 139 | ``` bash 140 | ros2 launch ldlidar_ros2 ld06.launch.py 141 | ``` 142 | - 启动ld14 lidar node并显示激光数据在Rviz2上: 143 | ``` bash 144 | ros2 launch ldlidar_ros2 viewer_ld06.launch.py 145 | ``` 146 | 147 | - 产品型号为 LDROBOT LiDAR LD19 148 | - 启动ld19 lidar node: 149 | ``` bash 150 | ros2 launch ldlidar_ros2 ld19.launch.py 151 | ``` 152 | - 启动ld19 lidar node并显示激光数据在Rviz2上: 153 | ``` bash 154 | ros2 launch ldlidar_ros2 viewer_ld19.launch.py 155 | ``` 156 | ## 4. 可视化 157 | 158 | > 代码支持ubuntu20.04 ROS2 foxy版本及以上测试，使用rviz2可视化。 159 | - 新打开一个终端 (Ctrl + Alt + T),运行命令:`rviz2`,并通过Rviz2工具打开readme文件所在目录的rviz2文件夹下面的ldlidar.rviz文件 160 | ```bash 161 | rviz2 162 | ``` -------------------------------------------------------------------------------- /include/ros2_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 | #include 28 | #include 29 | #include 30 | 31 | struct LaserScanSetting 32 | { 33 | std::string frame_id; 34 | bool laser_scan_dir; 35 | bool enable_angle_crop_func; 36 | double angle_crop_min; 37 | double angle_crop_max; 38 | double range_min; 39 | double range_max; 40 | }; 41 | 42 | #endif //__ROS_API_H__ 43 | 44 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 45 | * FILE ********/ -------------------------------------------------------------------------------- /launch/ld06.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | 5 | ''' 6 | Parameter Description: 7 | --- 8 | - Set laser scan directon: 9 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 10 | 2. Set clockwise, example: {'laser_scan_dir': False} 11 | - Angle crop setting, Mask data within the set angle range: 12 | 1. Enable angle crop fuction: 13 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 14 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 15 | 2. Angle cropping interval setting: 16 | - The distance and intensity data within the set angle range will be set to 0. 17 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 18 | example: 19 | {'angle_crop_min': 135.0} 20 | {'angle_crop_max': 225.0} 21 | which is [135.0, 225.0], angle unit is degress. 22 | ''' 23 | 24 | def generate_launch_description(): 25 | # LDROBOT LiDAR publisher node 26 | ldlidar_node = Node( 27 | package='ldlidar_ros2', 28 | executable='ldlidar_ros2_node', 29 | name='ldlidar_publisher_ld06', 30 | output='screen', 31 | parameters=[ 32 | {'product_name': 'LDLiDAR_LD06'}, 33 | {'laser_scan_topic_name': 'scan'}, 34 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 35 | {'frame_id': 'base_laser'}, 36 | {'port_name': '/dev/ttyUSB0'}, 37 | {'serial_baudrate': 230400}, 38 | {'laser_scan_dir': True}, 39 | {'enable_angle_crop_func': False}, 40 | {'angle_crop_min': 135.0}, # unit is degress 41 | {'angle_crop_max': 225.0}, # unit is degress 42 | {'range_min': 0.02}, # unit is meter 43 | {'range_max': 12.0} # unit is meter 44 | ] 45 | ) 46 | 47 | # base_link to base_laser tf node 48 | base_link_to_laser_tf_node = Node( 49 | package='tf2_ros', 50 | executable='static_transform_publisher', 51 | name='base_link_to_base_laser_ld06', 52 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 53 | ) 54 | 55 | 56 | # Define LaunchDescription variable 57 | ld = LaunchDescription() 58 | 59 | ld.add_action(ldlidar_node) 60 | ld.add_action(base_link_to_laser_tf_node) 61 | 62 | return ld -------------------------------------------------------------------------------- /launch/ld14.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | 5 | ''' 6 | Parameter Description: 7 | --- 8 | - Set laser scan directon: 9 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 10 | 2. Set clockwise, example: {'laser_scan_dir': False} 11 | - Angle crop setting, Mask data within the set angle range: 12 | 1. Enable angle crop fuction: 13 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 14 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 15 | 2. Angle cropping interval setting: 16 | - The distance and intensity data within the set angle range will be set to 0. 17 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 18 | example: 19 | {'angle_crop_min': 135.0} 20 | {'angle_crop_max': 225.0} 21 | which is [135.0, 225.0], angle unit is degress. 22 | ''' 23 | 24 | def generate_launch_description(): 25 | # LDROBOT LiDAR publisher node 26 | ldlidar_node = Node( 27 | package='ldlidar_ros2', 28 | executable='ldlidar_ros2_node', 29 | name='ldlidar_publisher_ld14', 30 | output='screen', 31 | parameters=[ 32 | {'product_name': 'LDLiDAR_LD14'}, 33 | {'laser_scan_topic_name': 'scan'}, 34 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 35 | {'frame_id': 'base_laser'}, 36 | {'port_name': '/dev/ttyUSB0'}, 37 | {'serial_baudrate' : 115200}, 38 | {'laser_scan_dir': True}, 39 | {'enable_angle_crop_func': False}, 40 | {'angle_crop_min': 135.0}, # unit is degress 41 | {'angle_crop_max': 225.0}, # unit is degress 42 | {'range_min': 0.02}, # unit is meter 43 | {'range_max': 8.0} # unit is meter 44 | ] 45 | ) 46 | 47 | # base_link to base_laser tf node 48 | base_link_to_laser_tf_node = Node( 49 | package='tf2_ros', 50 | executable='static_transform_publisher', 51 | name='base_link_to_base_laser_ld14', 52 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 53 | ) 54 | 55 | 56 | # Define LaunchDescription variable 57 | ld = LaunchDescription() 58 | 59 | ld.add_action(ldlidar_node) 60 | ld.add_action(base_link_to_laser_tf_node) 61 | 62 | return ld -------------------------------------------------------------------------------- /launch/ld14p.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | 5 | ''' 6 | Parameter Description: 7 | --- 8 | - Set laser scan directon: 9 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 10 | 2. Set clockwise, example: {'laser_scan_dir': False} 11 | - Angle crop setting, Mask data within the set angle range: 12 | 1. Enable angle crop fuction: 13 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 14 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 15 | 2. Angle cropping interval setting: 16 | - The distance and intensity data within the set angle range will be set to 0. 17 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 18 | example: 19 | {'angle_crop_min': 135.0} 20 | {'angle_crop_max': 225.0} 21 | which is [135.0, 225.0], angle unit is degress. 22 | ''' 23 | 24 | def generate_launch_description(): 25 | # LDROBOT LiDAR publisher node 26 | ldlidar_node = Node( 27 | package='ldlidar_ros2', 28 | executable='ldlidar_ros2_node', 29 | name='ldlidar_publisher_ld14p', 30 | output='screen', 31 | parameters=[ 32 | {'product_name': 'LDLiDAR_LD14P'}, 33 | {'laser_scan_topic_name': 'scan'}, 34 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 35 | {'frame_id': 'base_laser'}, 36 | {'port_name': '/dev/ttyUSB0'}, 37 | {'serial_baudrate' : 230400}, 38 | {'laser_scan_dir': True}, 39 | {'enable_angle_crop_func': False}, 40 | {'angle_crop_min': 135.0}, # unit is degress 41 | {'angle_crop_max': 225.0}, # unit is degress 42 | {'range_min': 0.02}, # unit is meter 43 | {'range_max': 8.0} # unit is meter 44 | ] 45 | ) 46 | 47 | # base_link to base_laser tf node 48 | base_link_to_laser_tf_node = Node( 49 | package='tf2_ros', 50 | executable='static_transform_publisher', 51 | name='base_link_to_base_laser_ld14p', 52 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 53 | ) 54 | 55 | 56 | # Define LaunchDescription variable 57 | ld = LaunchDescription() 58 | 59 | ld.add_action(ldlidar_node) 60 | ld.add_action(base_link_to_laser_tf_node) 61 | 62 | return ld -------------------------------------------------------------------------------- /launch/ld19.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | 5 | ''' 6 | Parameter Description: 7 | --- 8 | - Set laser scan directon: 9 | 1. Set counterclockwise, example: {'laser_scan_dir': True} 10 | 2. Set clockwise, example: {'laser_scan_dir': False} 11 | - Angle crop setting, Mask data within the set angle range: 12 | 1. Enable angle crop fuction: 13 | 1.1. enable angle crop, example: {'enable_angle_crop_func': True} 14 | 1.2. disable angle crop, example: {'enable_angle_crop_func': False} 15 | 2. Angle cropping interval setting: 16 | - The distance and intensity data within the set angle range will be set to 0. 17 | - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress. 18 | example: 19 | {'angle_crop_min': 135.0} 20 | {'angle_crop_max': 225.0} 21 | which is [135.0, 225.0], angle unit is degress. 22 | ''' 23 | 24 | def generate_launch_description(): 25 | # LDROBOT LiDAR publisher node 26 | ldlidar_node = Node( 27 | package='ldlidar_ros2', 28 | executable='ldlidar_ros2_node', 29 | name='ldlidar_publisher_ld19', 30 | output='screen', 31 | parameters=[ 32 | {'product_name': 'LDLiDAR_LD19'}, 33 | {'laser_scan_topic_name': 'scan'}, 34 | {'point_cloud_2d_topic_name': 'pointcloud2d'}, 35 | {'frame_id': 'base_laser'}, 36 | {'port_name': '/dev/ttyUSB0'}, 37 | {'serial_baudrate': 230400}, 38 | {'laser_scan_dir': True}, 39 | {'enable_angle_crop_func': False}, 40 | {'angle_crop_min': 135.0}, # unit is degress 41 | {'angle_crop_max': 225.0}, # unit is degress 42 | {'range_min': 0.02}, # unit is meter 43 | {'range_max': 12.0} # unit is meter 44 | ] 45 | ) 46 | 47 | # base_link to base_laser tf node 48 | base_link_to_laser_tf_node = Node( 49 | package='tf2_ros', 50 | executable='static_transform_publisher', 51 | name='base_link_to_base_laser_ld19', 52 | arguments=['0','0','0.18','0','0','0','base_link','base_laser'] 53 | ) 54 | 55 | 56 | # Define LaunchDescription variable 57 | ld = LaunchDescription() 58 | 59 | ld.add_action(ldlidar_node) 60 | ld.add_action(base_link_to_laser_tf_node) 61 | 62 | return ld -------------------------------------------------------------------------------- /launch/viewer_ld06.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | 9 | def generate_launch_description(): 10 | # RViZ2 settings 11 | rviz2_config = os.path.join( 12 | get_package_share_directory('ldlidar_ros2'), 13 | 'rviz2', 14 | 'ldlidar.rviz' 15 | ) 16 | 17 | rviz2_node = Node( 18 | package='rviz2', 19 | executable='rviz2', 20 | name='rviz2_show_ld06', 21 | arguments=['-d',rviz2_config], 22 | output='screen' 23 | ) 24 | 25 | #Include LDLidar launch file 26 | ldlidar_launch = IncludeLaunchDescription( 27 | launch_description_source=PythonLaunchDescriptionSource([ 28 | get_package_share_directory('ldlidar_ros2'), 29 | '/launch/ld06.launch.py' 30 | ]) 31 | ) 32 | 33 | # Define LaunchDescription variable 34 | ld = LaunchDescription() 35 | 36 | ld.add_action(ldlidar_launch) 37 | ld.add_action(rviz2_node) 38 | 39 | return ld -------------------------------------------------------------------------------- /launch/viewer_ld14.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | 9 | def generate_launch_description(): 10 | # RViZ2 settings 11 | rviz2_config = os.path.join( 12 | get_package_share_directory('ldlidar_ros2'), 13 | 'rviz2', 14 | 'ldlidar.rviz' 15 | ) 16 | 17 | rviz2_node = Node( 18 | package='rviz2', 19 | executable='rviz2', 20 | name='rviz2_show_ld14', 21 | arguments=['-d',rviz2_config], 22 | output='screen' 23 | ) 24 | 25 | #Include LDLidar launch file 26 | ldlidar_launch = IncludeLaunchDescription( 27 | launch_description_source=PythonLaunchDescriptionSource([ 28 | get_package_share_directory('ldlidar_ros2'), 29 | '/launch/ld14.launch.py' 30 | ]) 31 | ) 32 | 33 | # Define LaunchDescription variable 34 | ld = LaunchDescription() 35 | 36 | ld.add_action(ldlidar_launch) 37 | ld.add_action(rviz2_node) 38 | 39 | return ld -------------------------------------------------------------------------------- /launch/viewer_ld14p.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | 9 | def generate_launch_description(): 10 | # RViZ2 settings 11 | rviz2_config = os.path.join( 12 | get_package_share_directory('ldlidar_ros2'), 13 | 'rviz2', 14 | 'ldlidar.rviz' 15 | ) 16 | 17 | rviz2_node = Node( 18 | package='rviz2', 19 | executable='rviz2', 20 | name='rviz2_show_ld14p', 21 | arguments=['-d',rviz2_config], 22 | output='screen' 23 | ) 24 | 25 | #Include LDLidar launch file 26 | ldlidar_launch = IncludeLaunchDescription( 27 | launch_description_source=PythonLaunchDescriptionSource([ 28 | get_package_share_directory('ldlidar_ros2'), 29 | '/launch/ld14p.launch.py' 30 | ]) 31 | ) 32 | 33 | # Define LaunchDescription variable 34 | ld = LaunchDescription() 35 | 36 | ld.add_action(ldlidar_launch) 37 | ld.add_action(rviz2_node) 38 | 39 | return ld -------------------------------------------------------------------------------- /launch/viewer_ld19.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | 9 | def generate_launch_description(): 10 | # RViZ2 settings 11 | rviz2_config = os.path.join( 12 | get_package_share_directory('ldlidar_ros2'), 13 | 'rviz2', 14 | 'ldlidar.rviz' 15 | ) 16 | 17 | rviz2_node = Node( 18 | package='rviz2', 19 | executable='rviz2', 20 | name='rviz2_show_ld19', 21 | arguments=['-d',rviz2_config], 22 | output='screen' 23 | ) 24 | 25 | #Include LDLidar launch file 26 | ldlidar_launch = IncludeLaunchDescription( 27 | launch_description_source=PythonLaunchDescriptionSource([ 28 | get_package_share_directory('ldlidar_ros2'), 29 | '/launch/ld19.launch.py' 30 | ]) 31 | ) 32 | 33 | # Define LaunchDescription variable 34 | ld = LaunchDescription() 35 | 36 | ld.add_action(ldlidar_launch) 37 | ld.add_action(rviz2_node) 38 | 39 | return ld -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ldlidar_ros2 5 | 1.0.0 6 | This is LDLiDAR ROS2 Package 7 | David 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | geometry_msgs 15 | 16 | ros2launch 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /rviz2/ldlidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 167 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /LaserScan1/Topic1 11 | Splitter Ratio: 0.5 12 | Tree Height: 536 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /Interact1 18 | - /Measure1 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 100 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Autocompute Intensity Bounds: false 51 | Autocompute Value Bounds: 52 | Max Value: 10 53 | Min Value: -10 54 | Value: true 55 | Axis: Z 56 | Channel Name: intensity 57 | Class: rviz_default_plugins/LaserScan 58 | Color: 255; 255; 255 59 | Color Transformer: Intensity 60 | Decay Time: 0 61 | Enabled: true 62 | Invert Rainbow: false 63 | Max Color: 255; 255; 255 64 | Max Intensity: 255 65 | Min Color: 0; 0; 0 66 | Min Intensity: 0 67 | Name: LaserScan 68 | Position Transformer: XYZ 69 | Selectable: true 70 | Size (Pixels): 3 71 | Size (m): 0.019999999552965164 72 | Style: Flat Squares 73 | Topic: 74 | Depth: 5 75 | Durability Policy: Volatile 76 | History Policy: Keep Last 77 | Reliability Policy: Reliable 78 | Value: /scan 79 | Use Fixed Frame: true 80 | Use rainbow: true 81 | Value: true 82 | - Class: rviz_default_plugins/Axes 83 | Enabled: false 84 | Length: 0.5 85 | Name: Axes 86 | Radius: 0.029999999329447746 87 | Reference Frame: 88 | Value: false 89 | - Class: rviz_default_plugins/TF 90 | Enabled: true 91 | Frame Timeout: 15 92 | Frames: 93 | All Enabled: true 94 | base_laser: 95 | Value: true 96 | base_link: 97 | Value: true 98 | Marker Scale: 1 99 | Name: TF 100 | Show Arrows: true 101 | Show Axes: true 102 | Show Names: true 103 | Tree: 104 | base_link: 105 | base_laser: 106 | {} 107 | Update Interval: 0 108 | Value: true 109 | - Alpha: 1 110 | Autocompute Intensity Bounds: false 111 | Autocompute Value Bounds: 112 | Max Value: 10 113 | Min Value: -10 114 | Value: true 115 | Axis: Z 116 | Channel Name: intensity 117 | Class: rviz_default_plugins/PointCloud 118 | Color: 255; 255; 255 119 | Color Transformer: Intensity 120 | Decay Time: 0 121 | Enabled: false 122 | Invert Rainbow: false 123 | Max Color: 255; 255; 255 124 | Max Intensity: 255 125 | Min Color: 0; 0; 0 126 | Min Intensity: 0 127 | Name: PointCloud 128 | Position Transformer: XYZ 129 | Selectable: true 130 | Size (Pixels): 3 131 | Size (m): 0.009999999776482582 132 | Style: Flat Squares 133 | Topic: 134 | Depth: 5 135 | Durability Policy: Volatile 136 | History Policy: Keep Last 137 | Reliability Policy: Reliable 138 | Value: /pointcloud2d 139 | Use Fixed Frame: true 140 | Use rainbow: true 141 | Value: false 142 | Enabled: true 143 | Global Options: 144 | Background Color: 48; 48; 48 145 | Fixed Frame: base_link 146 | Frame Rate: 30 147 | Name: root 148 | Tools: 149 | - Class: rviz_default_plugins/Interact 150 | Hide Inactive Objects: true 151 | - Class: rviz_default_plugins/MoveCamera 152 | - Class: rviz_default_plugins/Select 153 | - Class: rviz_default_plugins/FocusCamera 154 | - Class: rviz_default_plugins/Measure 155 | Line color: 128; 128; 0 156 | - Class: rviz_default_plugins/SetInitialPose 157 | Topic: 158 | Depth: 5 159 | Durability Policy: Volatile 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /initialpose 163 | - Class: rviz_default_plugins/SetGoal 164 | Topic: 165 | Depth: 5 166 | Durability Policy: Volatile 167 | History Policy: Keep Last 168 | Reliability Policy: Reliable 169 | Value: /goal_pose 170 | - Class: rviz_default_plugins/PublishPoint 171 | Single click: true 172 | Topic: 173 | Depth: 5 174 | Durability Policy: Volatile 175 | History Policy: Keep Last 176 | Reliability Policy: Reliable 177 | Value: /clicked_point 178 | Transformation: 179 | Current: 180 | Class: rviz_default_plugins/TF 181 | Value: true 182 | Views: 183 | Current: 184 | Class: rviz_default_plugins/Orbit 185 | Distance: 5.592278003692627 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.05999999865889549 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Focal Point: 192 | X: 0 193 | Y: 0 194 | Z: 0 195 | Focal Shape Fixed Size: true 196 | Focal Shape Size: 0.05000000074505806 197 | Invert Z Axis: false 198 | Name: Current View 199 | Near Clip Distance: 0.009999999776482582 200 | Pitch: 0.9447976350784302 201 | Target Frame: 202 | Value: Orbit (rviz) 203 | Yaw: 3.0903873443603516 204 | Saved: ~ 205 | Window Geometry: 206 | Displays: 207 | collapsed: false 208 | Height: 854 209 | Hide Left Dock: false 210 | Hide Right Dock: false 211 | QMainWindow State: 000000ff00000000fd00000004000000000000020a000002fcfc0200000008fb0000001200530065006c0065006300740069006f006e000000003d000000800000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002fc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002fcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002fc000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000041b000002fc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 212 | Selection: 213 | collapsed: false 214 | Tool Properties: 215 | collapsed: false 216 | Views: 217 | collapsed: false 218 | Width: 1856 219 | X: 62 220 | Y: 27 221 | -------------------------------------------------------------------------------- /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 ./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 | -------------------------------------------------------------------------------- /scripts/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 | -------------------------------------------------------------------------------- /src/demo.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author LDRobot (contact@ldrobot.com) 4 | * @brief main process App 5 | * This code is only applicable to LDROBOT LiDAR LD00 LD03 LD08 LD14 6 | * products sold by Shenzhen LDROBOT Co., LTD 7 | * @version 0.1 8 | * @date 2021-11-10 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 "ros2_api.h" 22 | #include "ldlidar_driver/ldlidar_driver_linux.h" 23 | 24 | uint64_t GetTimestamp(void); 25 | 26 | void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq, LaserScanSetting& setting, 27 | rclcpp::Node::SharedPtr& node, rclcpp::Publisher::SharedPtr& lidarpub); 28 | 29 | void ToSensorPointCloudMessagePublish(ldlidar::Points2D& src, LaserScanSetting& setting, 30 | rclcpp::Node::SharedPtr& node, rclcpp::Publisher::SharedPtr& lidarpub); 31 | 32 | int main(int argc, char **argv) { 33 | rclcpp::init(argc, argv); 34 | 35 | // create a ROS2 Node 36 | auto node = std::make_shared("ldlidar_published"); 37 | 38 | std::string product_name; 39 | std::string laser_scan_topic_name; 40 | std::string point_cloud_2d_topic_name; 41 | std::string port_name; 42 | LaserScanSetting setting; 43 | int serial_baudrate; 44 | ldlidar::LDType lidartypename; 45 | 46 | // declare ros2 param 47 | node->declare_parameter("product_name", product_name); 48 | node->declare_parameter("laser_scan_topic_name", laser_scan_topic_name); 49 | node->declare_parameter("point_cloud_2d_topic_name", point_cloud_2d_topic_name); 50 | node->declare_parameter("frame_id", setting.frame_id); 51 | node->declare_parameter("port_name", port_name); 52 | node->declare_parameter("serial_baudrate", serial_baudrate); 53 | node->declare_parameter("laser_scan_dir", setting.laser_scan_dir); 54 | node->declare_parameter("enable_angle_crop_func", setting.enable_angle_crop_func); 55 | node->declare_parameter("angle_crop_min", setting.angle_crop_min); 56 | node->declare_parameter("angle_crop_max", setting.angle_crop_max); 57 | node->declare_parameter("range_min", setting.range_min); 58 | node->declare_parameter("range_max", setting.range_max); 59 | 60 | // get ros2 param 61 | node->get_parameter("product_name", product_name); 62 | node->get_parameter("laser_scan_topic_name", laser_scan_topic_name); 63 | node->get_parameter("point_cloud_2d_topic_name", point_cloud_2d_topic_name); 64 | node->get_parameter("frame_id", setting.frame_id); 65 | node->get_parameter("port_name", port_name); 66 | node->get_parameter("serial_baudrate", serial_baudrate); 67 | node->get_parameter("laser_scan_dir", setting.laser_scan_dir); 68 | node->get_parameter("enable_angle_crop_func", setting.enable_angle_crop_func); 69 | node->get_parameter("angle_crop_min", setting.angle_crop_min); 70 | node->get_parameter("angle_crop_max", setting.angle_crop_max); 71 | node->get_parameter("range_min", setting.range_min); 72 | node->get_parameter("range_max", setting.range_max); 73 | 74 | ldlidar::LDLidarDriverLinuxInterface* ldlidar_drv = 75 | ldlidar::LDLidarDriverLinuxInterface::Create(); 76 | 77 | RCLCPP_INFO(node->get_logger(), "LDLiDAR SDK Pack Version is:%s", ldlidar_drv->GetLidarSdkVersionNumber().c_str()); 78 | RCLCPP_INFO(node->get_logger(), "ROS2 param input:"); 79 | RCLCPP_INFO(node->get_logger(), ": %s", product_name.c_str()); 80 | RCLCPP_INFO(node->get_logger(), ": %s", laser_scan_topic_name.c_str()); 81 | RCLCPP_INFO(node->get_logger(), ": %s", point_cloud_2d_topic_name.c_str()); 82 | RCLCPP_INFO(node->get_logger(), ": %s", setting.frame_id.c_str()); 83 | RCLCPP_INFO(node->get_logger(), ": %s ", port_name.c_str()); 84 | RCLCPP_INFO(node->get_logger(), ": %d ", serial_baudrate); 85 | RCLCPP_INFO(node->get_logger(), ": %s", (setting.laser_scan_dir?"Counterclockwise":"Clockwise")); 86 | RCLCPP_INFO(node->get_logger(), ": %s", (setting.enable_angle_crop_func?"true":"false")); 87 | RCLCPP_INFO(node->get_logger(), ": %f", setting.angle_crop_min); 88 | RCLCPP_INFO(node->get_logger(), ": %f", setting.angle_crop_max); 89 | RCLCPP_INFO(node->get_logger(), ": %f", setting.range_min); 90 | RCLCPP_INFO(node->get_logger(), ": %f", setting.range_max); 91 | 92 | if (port_name.empty()) { 93 | RCLCPP_ERROR(node->get_logger(), "fail, port_name is empty!"); 94 | exit(EXIT_FAILURE); 95 | } 96 | 97 | ldlidar_drv->RegisterGetTimestampFunctional(std::bind(&GetTimestamp)); 98 | 99 | ldlidar_drv->EnablePointCloudDataFilter(true); 100 | 101 | if(!strcmp(product_name.c_str(),"LDLiDAR_LD14")) { 102 | lidartypename = ldlidar::LDType::LD_14; 103 | } else if (!strcmp(product_name.c_str(), "LDLiDAR_LD14P")) { 104 | lidartypename = ldlidar::LDType::LD_14P; 105 | } else if (!strcmp(product_name.c_str(),"LDLiDAR_LD06")) { 106 | lidartypename = ldlidar::LDType::LD_06; 107 | } else if (!strcmp(product_name.c_str(),"LDLiDAR_LD19")) { 108 | lidartypename = ldlidar::LDType::LD_19; 109 | } else { 110 | RCLCPP_ERROR(node->get_logger(),"Error, input param is fail!!"); 111 | exit(EXIT_FAILURE); 112 | } 113 | 114 | if (ldlidar_drv->Connect(lidartypename, port_name, serial_baudrate)) { 115 | RCLCPP_INFO(node->get_logger(), "ldlidar serial connect is success"); 116 | } else { 117 | RCLCPP_ERROR(node->get_logger(), "ldlidar serial connect is fail"); 118 | exit(EXIT_FAILURE); 119 | } 120 | 121 | if (ldlidar_drv->WaitLidarComm(3500)) { 122 | RCLCPP_INFO(node->get_logger(), "ldlidar communication is normal."); 123 | } else { 124 | RCLCPP_ERROR(node->get_logger(), "ldlidar communication is abnormal."); 125 | exit(EXIT_FAILURE); 126 | } 127 | 128 | if (ldlidar_drv->Start()) { 129 | RCLCPP_INFO(node->get_logger(), "ldlidar driver start is success."); 130 | } else { 131 | RCLCPP_ERROR(node->get_logger(), "ldlidar driver start is fail."); 132 | } 133 | 134 | // create ldlidar data topic and publisher 135 | rclcpp::Publisher::SharedPtr lidar_pub_laserscan = 136 | node->create_publisher(laser_scan_topic_name, 10); 137 | 138 | rclcpp::Publisher::SharedPtr lidar_pub_pointcloud = 139 | node->create_publisher(point_cloud_2d_topic_name, 10); 140 | 141 | rclcpp::WallRate r(10); //Hz 142 | 143 | ldlidar::Points2D laser_scan_points; 144 | 145 | RCLCPP_INFO(node->get_logger(), "start normal, pub lidar data"); 146 | 147 | while (rclcpp::ok() && 148 | ldlidar::LDLidarDriver::Ok()) { 149 | 150 | switch (ldlidar_drv->GetLaserScanData(laser_scan_points, 1500)){ 151 | case ldlidar::LidarStatus::NORMAL: { 152 | double lidar_scan_freq = 0; 153 | ldlidar_drv->GetLidarScanFreq(lidar_scan_freq); 154 | ToLaserscanMessagePublish(laser_scan_points, lidar_scan_freq, setting, node, lidar_pub_laserscan); 155 | ToSensorPointCloudMessagePublish(laser_scan_points, setting, node, lidar_pub_pointcloud); 156 | break; 157 | } 158 | case ldlidar::LidarStatus::DATA_TIME_OUT: { 159 | RCLCPP_ERROR(node->get_logger(), "ldlidar point cloud data publish time out, please check your lidar device."); 160 | break; 161 | } 162 | case ldlidar::LidarStatus::DATA_WAIT: { 163 | break; 164 | } 165 | default: 166 | break; 167 | } 168 | 169 | r.sleep(); 170 | } 171 | 172 | ldlidar_drv->Stop(); 173 | ldlidar_drv->Disconnect(); 174 | 175 | ldlidar::LDLidarDriverLinuxInterface::Destory(ldlidar_drv); 176 | 177 | RCLCPP_INFO(node->get_logger(), "ldlidar published is end"); 178 | rclcpp::shutdown(); 179 | 180 | return 0; 181 | } 182 | 183 | uint64_t GetTimestamp(void) { 184 | std::chrono::time_point tp = 185 | std::chrono::time_point_cast(std::chrono::system_clock::now()); 186 | auto tmp = std::chrono::duration_cast(tp.time_since_epoch()); 187 | return ((uint64_t)tmp.count()); 188 | } 189 | 190 | void ToLaserscanMessagePublish(ldlidar::Points2D& src, double lidar_spin_freq, LaserScanSetting& setting, 191 | rclcpp::Node::SharedPtr& node, rclcpp::Publisher::SharedPtr& lidarpub) { 192 | float angle_min, angle_max, range_min, range_max, angle_increment; 193 | double scan_time; 194 | rclcpp::Time start_scan_time; 195 | static rclcpp::Time end_scan_time; 196 | static bool first_scan = true; 197 | 198 | start_scan_time = node->now(); 199 | scan_time = (start_scan_time.seconds() - end_scan_time.seconds()); 200 | 201 | if (first_scan) { 202 | first_scan = false; 203 | end_scan_time = start_scan_time; 204 | return; 205 | } 206 | // Adjust the parameters according to the demand 207 | angle_min = 0; 208 | angle_max = (2 * M_PI); 209 | range_min = static_cast(setting.range_min); 210 | range_max = static_cast(setting.range_max); 211 | int beam_size = static_cast(src.size()); 212 | if (beam_size <= 1) { 213 | end_scan_time = start_scan_time; 214 | RCLCPP_ERROR(node->get_logger(), "beam_size <= 1"); 215 | return; 216 | } 217 | angle_increment = (angle_max - angle_min) / (float)(beam_size -1); 218 | // Calculate the number of scanning points 219 | if (lidar_spin_freq > 0) { 220 | sensor_msgs::msg::LaserScan output; 221 | output.header.stamp = start_scan_time; 222 | output.header.frame_id = setting.frame_id; 223 | output.angle_min = angle_min; 224 | output.angle_max = angle_max; 225 | output.range_min = range_min; 226 | output.range_max = range_max; 227 | output.angle_increment = angle_increment; 228 | if (beam_size <= 1) { 229 | output.time_increment = 0; 230 | } else { 231 | output.time_increment = static_cast(scan_time / (double)(beam_size - 1)); 232 | } 233 | output.scan_time = scan_time; 234 | // First fill all the data with Nan 235 | output.ranges.assign(beam_size, std::numeric_limits::quiet_NaN()); 236 | output.intensities.assign(beam_size, std::numeric_limits::quiet_NaN()); 237 | for (auto point : src) { 238 | float range = point.distance / 1000.f; // distance unit transform to meters 239 | float intensity = point.intensity; // laser receive intensity 240 | float dir_angle = point.angle; 241 | 242 | if ((point.distance == 0) && (point.intensity == 0)) { // filter is handled to 0, Nan will be assigned variable. 243 | range = std::numeric_limits::quiet_NaN(); 244 | intensity = std::numeric_limits::quiet_NaN(); 245 | } 246 | 247 | if (setting.enable_angle_crop_func) { // Angle crop setting, Mask data within the set angle range 248 | if ((dir_angle >= setting.angle_crop_min) && (dir_angle <= setting.angle_crop_max)) { 249 | range = std::numeric_limits::quiet_NaN(); 250 | intensity = std::numeric_limits::quiet_NaN(); 251 | } 252 | } 253 | 254 | float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian 255 | int index = static_cast(ceil((angle - angle_min) / angle_increment)); 256 | if (index < beam_size) { 257 | if (index < 0) { 258 | RCLCPP_ERROR(node->get_logger(), "error index: %d, beam_size: %d, angle: %f, output.angle_min: %f, output.angle_increment: %f", 259 | index, beam_size, angle, angle_min, angle_increment); 260 | } 261 | 262 | if (setting.laser_scan_dir) { 263 | int index_anticlockwise = beam_size - index - 1; 264 | // If the current content is Nan, it is assigned directly 265 | if (std::isnan(output.ranges[index_anticlockwise])) { 266 | output.ranges[index_anticlockwise] = range; 267 | } else { // Otherwise, only when the distance is less than the current 268 | // value, it can be re assigned 269 | if (range < output.ranges[index_anticlockwise]) { 270 | output.ranges[index_anticlockwise] = range; 271 | } 272 | } 273 | output.intensities[index_anticlockwise] = intensity; 274 | } else { 275 | // If the current content is Nan, it is assigned directly 276 | if (std::isnan(output.ranges[index])) { 277 | output.ranges[index] = range; 278 | } else { // Otherwise, only when the distance is less than the current 279 | // value, it can be re assigned 280 | if (range < output.ranges[index]) { 281 | output.ranges[index] = range; 282 | } 283 | } 284 | output.intensities[index] = intensity; 285 | } 286 | } 287 | } 288 | lidarpub->publish(output); 289 | end_scan_time = start_scan_time; 290 | } 291 | } 292 | 293 | void ToSensorPointCloudMessagePublish(ldlidar::Points2D& src, LaserScanSetting& setting, 294 | rclcpp::Node::SharedPtr& node, rclcpp::Publisher::SharedPtr& lidarpub) { 295 | 296 | rclcpp::Time start_scan_time; 297 | double scan_time; 298 | float time_increment; 299 | static rclcpp::Time end_scan_time; 300 | static bool first_scan = true; 301 | 302 | ldlidar::Points2D dst = src; 303 | 304 | start_scan_time = node->now(); 305 | scan_time = (start_scan_time.seconds() - end_scan_time.seconds()); 306 | 307 | if (first_scan) { 308 | first_scan = false; 309 | end_scan_time = start_scan_time; 310 | return; 311 | } 312 | 313 | if (setting.laser_scan_dir) { 314 | for (auto&point : dst) { 315 | point.angle = 360.f - point.angle; 316 | if (point.angle < 0) { 317 | point.angle += 360.f; 318 | } 319 | } 320 | } 321 | 322 | int frame_points_num = static_cast(dst.size()); 323 | 324 | sensor_msgs::msg::PointCloud output; 325 | 326 | output.header.stamp = start_scan_time; 327 | output.header.frame_id = setting.frame_id; 328 | 329 | sensor_msgs::msg::ChannelFloat32 defaultchannelval[3]; 330 | 331 | defaultchannelval[0].name = std::string("intensity"); 332 | defaultchannelval[0].values.assign(frame_points_num, std::numeric_limits::quiet_NaN()); 333 | // output.channels.assign(1, defaultchannelval); 334 | output.channels.push_back(defaultchannelval[0]); 335 | 336 | if (frame_points_num <= 1) { 337 | time_increment = 0; 338 | } else { 339 | time_increment = static_cast(scan_time / (double)(frame_points_num - 1)); 340 | } 341 | defaultchannelval[1].name = std::string("timeincrement"); 342 | defaultchannelval[1].values.assign(1, time_increment); 343 | output.channels.push_back(defaultchannelval[1]); 344 | 345 | defaultchannelval[2].name = std::string("scantime"); 346 | defaultchannelval[2].values.assign(1, scan_time); 347 | output.channels.push_back(defaultchannelval[2]); 348 | 349 | geometry_msgs::msg::Point32 points_xyz_defaultval; 350 | points_xyz_defaultval.x = std::numeric_limits::quiet_NaN(); 351 | points_xyz_defaultval.y = std::numeric_limits::quiet_NaN(); 352 | points_xyz_defaultval.z = std::numeric_limits::quiet_NaN(); 353 | output.points.assign(frame_points_num, points_xyz_defaultval); 354 | 355 | for (int i = 0; i < frame_points_num; i++) { 356 | float range = dst[i].distance / 1000.f; // distance unit transform to meters 357 | float intensity = dst[i].intensity; // laser receive intensity 358 | float dir_angle = ANGLE_TO_RADIAN(dst[i].angle); 359 | // 极坐标系转换为笛卡尔直角坐标系 360 | output.points[i].x = range * cos(dir_angle); 361 | output.points[i].y = range * sin(dir_angle); 362 | output.points[i].z = 0.0; 363 | output.channels[0].values[i] = intensity; 364 | } 365 | lidarpub->publish(output); 366 | end_scan_time = start_scan_time; 367 | } 368 | 369 | /********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF 370 | * FILE ********/ 371 | --------------------------------------------------------------------------------