fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add YDLIDAR ROS2 driver package (ported from move_try)

Browse Source 
TminiPro LiDAR driver with multi-model parameter configs (X4, G1, G2, G6, TminiPro, etc.), ROS2 lifecycle management, and RViz debug config.

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
FallenSigh
2026-06-19 06:30:19 +08:00
parent da97c1068c
commit c12a35a4a2
29 changed files with 1342 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
ros2/src/ydlidar_ros2_driver/CMakeLists.txt Normal file
View File

@@ -0,0 +1,88 @@
# Copyright(c) 2020 eaibot limited.
cmake_minimum_required(VERSION 3.5)
project(ydlidar_ros2_driver C CXX)
##################ros2#############################################
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
####################find package#####################################
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rmw REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
############## YDLIDAR SDK START#####################################
#find ydlidar_sdk package
find_package(ydlidar_sdk REQUIRED)
############## YDLIDAR SDK END#####################################
#Include directories
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/src
${YDLIDAR_SDK_INCLUDE_DIRS})
#link library directories
link_directories(${YDLIDAR_SDK_LIBRARY_DIRS})
#---------------------------------------------------------------------------------------
# generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_node
src/${PROJECT_NAME}_node.cpp)
#---------------------------------------------------------------------------------------
# link libraries
#--------------------------------------------------------------------------------------
ament_target_dependencies(${PROJECT_NAME}_node
"rclcpp"
"sensor_msgs"
"visualization_msgs"
"geometry_msgs"
"std_srvs"
)
target_link_libraries(${PROJECT_NAME}_node
${YDLIDAR_SDK_LIBRARIES})
#---------------------------------------------------------------------------------------
# generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_client
src/${PROJECT_NAME}_client.cpp)
#---------------------------------------------------------------------------------------
# link libraries
#--------------------------------------------------------------------------------------
ament_target_dependencies(${PROJECT_NAME}_client
"rclcpp"
"sensor_msgs"
"visualization_msgs"
"geometry_msgs"
"std_srvs"
)
#---------------------------------------------------------------------------------------
# Install
#---------------------------------------------------------------------------------------
install(TARGETS
${PROJECT_NAME}_node ${PROJECT_NAME}_client
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch params startup config
DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

0
ros2/src/ydlidar_ros2_driver/LICENSE.txt Normal file
View File

187
ros2/src/ydlidar_ros2_driver/README.md Normal file
View File

@@ -0,0 +1,187 @@
![YDLIDAR](images/YDLidar.jpg "YDLIDAR")
# YDLIDAR ROS2 Driver
ydlidar_ros2_driver is a new ros package, which is designed to gradually become the standard driver package for ydlidar devices in the ros2 environment.
## How to [install ROS2](https://index.ros.org/doc/ros2/Installation)
[ubuntu](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/)
[windows](https://index.ros.org/doc/ros2/Installation/Dashing/Windows-Install-Binary/)
## How to Create a ROS2 workspace
[Create a workspace](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#create-a-workspace)
## Compile & Install YDLidar SDK
ydlidar_ros2_driver depends on YDLidar-SDK library. If you have never installed YDLidar-SDK library or it is out of date, you must first install YDLidar-SDK library. If you have installed the latest version of YDLidar-SDK, skip this step and go to the next step.
1. Download or clone the [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) repository on GitHub.
2. Compile and install the YDLidar-SDK under the ***build*** directory following `README.md` of YDLIDAR/YDLidar-SDK.
## Clone ydlidar_ros2_driver
1. Clone ydlidar_ros2_driver package for github :
`git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver`
2. Build ydlidar_ros2_driver package :
```
cd ydlidar_ros2_ws
colcon build --symlink-install
```
Note: install colcon [see](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#install-colcon)
![CMAKE Finished](images/finished.png "CMAKE Finished")
<font color=Red size=4>>Note: If the following error occurs, Please install [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) first.</font>
![CMAKE ERROR](images/cmake_error.png "CMAKE ERROR")
3. Package environment setup :
`source ./install/setup.bash`
Note: Add permanent workspace environment variables.
It's convenientif the ROS2 environment variables are automatically added to your bash session every time a new shell is launched:
```
$echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc
$source ~/.bashrc
```
4. Confirmation
To confirm that your package path has been set, printenv the `grep -i ROS` variable.
```
$ printenv | grep -i ROS
```
You should see something similar to:
`OLDPWD=/home/tony/ydlidar_ros2_ws/install`
5. Create serial port Alias [optional]
```
$chmod 0777 src/ydlidar_ros2_driver/startup/*
$sudo sh src/ydlidar_ros2_driver/startup/initenv.sh
```
Note: After completing the previous operation, replug the LiDAR again.
## Configure LiDAR [paramters](params/ydlidar.yaml)
```
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
lidar_type: 1
device_type: 0
sample_rate: 9
abnormal_check_count: 4
resolution_fixed: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 64.0
range_min: 0.01
frequency: 10.0
invalid_range_is_inf: false
```
## Run ydlidar_ros2_driver
##### Run ydlidar_ros2_driver using launch file
The command format is :
`ros2 launch ydlidar_ros2_driver [launch file].py`
1. Connect LiDAR uint(s).
```
ros2 launch ydlidar_ros2_driver ydlidar_launch.py
```
or
```
launch $(ros2 pkg prefix ydlidar_ros2_driver)/share/ydlidar_ros2_driver/launch/ydlidar.py
```
2. RVIZ
```
ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py
```
![View](images/view.png "View")
3. echo scan topic
```
ros2 run ydlidar_ros2_driver ydlidar_ros2_driver_client or ros2 topic echo /scan
```
##### Launch file introduction
The driver offers users a wealth of options when using different launch file. The launch file directory
is `"ydlidar_ros2_ws/src/ydlidar_ros2_driver/launch"`. All launch files are listed as below :
| launch file | features |
| ------------------------- | ------------------------------------------------------------ |
| ydlidar.py | Connect to defualt paramters<br/>Publish LaserScan message on `scan` topic |
| ydlidar_launch.py | Connect ydlidar.yaml Lidar specified by configuration parameters<br/>Publish LaserScan message on `scan` topic |
| ydlidar_launch_view.py | Connect ydlidar.yaml Lidar specified by configuration parameters and setup RVIZ<br/>Publish LaserScan message on `scan` topic |
## Publish Topic
| Topic | Type | Description |
|----------------------|-------------------------|--------------------------------------------------|
| `scan` | sensor_msgs/LaserScan | 2D laser scan of the 0-angle ring |
## Subscribe Service
| Service | Type | Description |
|----------------------|-------------------------|--------------------------------------------------|
| `stop_scan` | std_srvs::Empty | turn off lidar |
| `start_scan` | std_srvs::Empty | turn on lidar |
## Configure ydlidar_ros_driver internal parameter
The ydlidar_ros2_driver internal parameters are in the launch file, they are listed as below :
| Parameter name | Data Type | detail |
| -------------- | ------- | ------------------------------------------------------------ |
| port | string | Set Lidar the serial port or IP address <br/>it can be set to `/dev/ttyUSB0`, `192.168.1.11`, etc. <br/>default: `/dev/ydlidar` |
| frame_id | string | Lidar TF coordinate system name. <br/>default: `laser_frame` |
| ignore_array | string | LiDAR filtering angle area<br/>eg: `-90, -80, 30, 40` |
| baudrate | int | Lidar baudrate or network port. <br/>default: `230400` |
| lidar_type | int | Set lidar type <br/>0 -- TYPE_TOF<br/>1 -- TYPE_TRIANGLE<br/>2 -- TYPE_TOF_NET <br/>default: `1` |
| device_type | int | Set device type <br/>0 -- YDLIDAR_TYPE_SERIAL<br/>1 -- YDLIDAR_TYPE_TCP<br/>2 -- YDLIDAR_TYPE_UDP <br/>default: `0` |
| sample_rate | int | Set Lidar Sample Rate. <br/>default: `9` |
| abnormal_check_count | int | Set the number of abnormal startup data attempts. <br/>default: `4` |
| fixed_resolution | bool | Fixed angluar resolution. <br/>default: `true` |
| reversion | bool | Reversion LiDAR. <br/>default: `true` |
| inverted | bool | Inverted LiDAR.<br/>false -- ClockWise.<br/>true -- CounterClockWise <br/>default: `true` |
| auto_reconnect | bool | Automatically reconnect the LiDAR.<br/>true -- hot plug. <br/>default: `true` |
| isSingleChannel | bool | Whether LiDAR is a single-channel.<br/>default: `false` |
| intensity | bool | Whether LiDAR has intensity.<br/>true -- G2 LiDAR.<br/>default: `false` |
| support_motor_dtr | bool | Whether the Lidar can be started and stopped by Serial DTR.<br/>default: `false` |
| angle_min | float | Minimum Valid Angle.<br/>default: `-180` |
| angle_max | float | Maximum Valid Angle.<br/>default: `180` |
| range_min | float | Minimum Valid range.<br/>default: `0.1` |
| range_max | float | Maximum Valid range.<br/>default: `16.0` |
| frequency | float | Set Scanning Frequency.<br/>default: `10.0` |
| invalid_range_is_inf | bool | Invalid Range is inf.<br/>true -- inf.<br/>false -- 0.0.<br/>default: `false` |
More paramters details, see [here](details.md)
## Contact EAI
![Development Path](images/EAI.png)
If you have any extra questions, please feel free to [contact us](http://www.ydlidar.cn/cn/contact)

177
ros2/src/ydlidar_ros2_driver/config/ydlidar.rviz Normal file
View File

@@ -0,0 +1,177 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /LaserScan1/Topic1
Splitter Ratio: 0.5
Tree Height: 617
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/LaserScan
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 1012
Min Color: 0; 0; 0
Min Intensity: 1008
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05999999865889549
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: System Default
Value: /scan
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
laser_frame:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
base_link:
laser_frame:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: laser_frame
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5103975534439087
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.163581848144531
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 67
Y: 60

133
ros2/src/ydlidar_ros2_driver/details.md Normal file
View File

@@ -0,0 +1,133 @@
# ROS Paramters Table
## Dataset
<table>
<tr><th>LIDAR <th> Model <th> Baudrate <th> SampleRate(K) <th> Range(m) <th> Frequency(HZ) <th> Intenstiy(bit) <th> SingleChannel<th> voltage(V)
<tr><th> F4 <td> 1 <td> 115200 <td> 4 <td> 0.12~12 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> S4 <td> 4 <td> 115200 <td> 4 <td> 0.10~8.0 <td> 5~12 (PWM) <td> false <td> false <td> 4.8~5.2
<tr><th> S4B <td> 4/11 <td> 153600 <td> 4 <td> 0.10~8.0 <td> 5~12(PWM) <td> true(8) <td> false <td> 4.8~5.2
<tr><th> S2 <td> 4/12 <td> 115200 <td> 3 <td> 0.10~8.0 <td> 4~8(PWM) <td> false <td> true <td> 4.8~5.2
<tr><th> G4 <td> 5 <td> 230400 <td> 9/8/4 <td> 0.28/0.26/0.1~16<td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> X4 <td> 6 <td> 128000 <td> 5 <td> 0.12~10 <td> 5~12(PWM) <td> false <td> false <td> 4.8~5.2
<tr><th> X2/X2L <td> 6 <td> 115200 <td> 3 <td> 0.10~8.0 <td> 4~8(PWM) <td> false <td> true <td> 4.8~5.2
<tr><th> G4PRO <td> 7 <td> 230400 <td> 9/8/4 <td> 0.28/0.26/0.1~16<td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> F4PRO <td> 8 <td> 230400 <td> 4/6 <td> 0.12~12 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> R2 <td> 9 <td> 230400 <td> 5 <td> 0.12~16 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G6 <td> 13 <td> 512000 <td> 18/16/8 <td> 0.28/0.26/0.1~25<td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G2A <td> 14 <td> 230400 <td> 5 <td> 0.12~12 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G2 <td> 15 <td> 230400 <td> 5 <td> 0.28~16 <td> 5~12 <td> true(8) <td> false <td> 4.8~5.2
<tr><th> G2C <td> 16 <td> 115200 <td> 4 <td> 0.1~12 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G4B <td> 17 <td> 512000 <td> 10 <td> 0.12~16 <td> 5~12 <td> true(10) <td> false <td> 4.8~5.2
<tr><th> G4C <td> 18 <td> 115200 <td> 4 <td> 0.1~12 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G1 <td> 19 <td> 230400 <td> 9 <td> 0.28~16 <td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G5 <td> 20 <td> 230400 <td> 9/8/4 <td> 0.28/0.26/0.1~16<td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> G7 <td> 21 <td> 512000 <td> 18/16/8 <td> 0.28/0.26/0.1~25<td> 5~12 <td> false <td> false <td> 4.8~5.2
<tr><th> TX8 　 <td> 100 <td> 115200 <td> 4 <td> 0.05~8 <td> 4~8(PWM) <td> false <td> true <td> 4.8~5.2
<tr><th> TX20 　<td> 100 <td> 115200 <td> 4 <td> 0.05~20 <td> 4~8(PWM) <td> false <td> true <td> 4.8~5.2
<tr><th> TG15 　<td> 100 <td> 512000 <td> 20/18/10 <td> 0.05~30 <td> 3~16 <td> false <td> false <td> 4.8~5.2
<tr><th> TG30 　<td> 101 <td> 512000 <td> 20/18/10 <td> 0.05~30 <td> 3~16 <td> false <td> false <td> 4.8~5.2
<tr><th> TG50 　<td> 102 <td> 512000 <td> 20/18/10 <td> 0.05~50 <td> 3~16 <td> false <td> false <td> 4.8~5.2
<tr><th> T15 　<td> 200 <td> 8000 <td> 20 <td> 0.05~30 <td> 5~35 <td> true <td> false <td> 4.8~5.2
</table>
## Baudrate Table
| LiDAR | baudrate |
|-----------------------------------------------|-----------------------|
|F4/S2/X2/X2L/S4/TX8/TX20/G4C | 115200 |
|X4 | 128000 |
|S4B | 153600 |
|G1/G2/R2/G4/G5/G4PRO/F4PRO | 230400 |
|G6/G7/TG15/TG30/TG50 | 512000 |
|T5/T15 | 8000 (network port) |
## SingleChannel Table
| LiDAR | isSingleChannel |
|-----------------------------------------------------------|-----------------------|
|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C | false |
|S2/X2/X2L | true |
|TG15/TG30/TG50 | false |
|TX8/TX20 | true |
|T5/T15 | false (optional) |
## LidarType Table
| LiDAR | lidar_type |
|-----------------------------------------------------------------------|-----------------------|
|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | TYPE_TRIANGLE |
|TG15/TG30/TG50/TX8/TX20 | TYPE_TOF |
|T5/T15 | TYPE_TOF_NET |
## DeviceType Table
| LiDAR | lidar_type |
|-----------------------------------------------------------------------|-----------------------|
|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | YDLIDAR_TYPE_SERIAL |
|TG15/TG30/TG50/TX8/TX20 | YDLIDAR_TYPE_SERIAL |
|T5/T15 | YDLIDAR_TYPE_TCP |
## Sampling Rate Table
| LiDAR | sample_rate |
|-----------------------------|------------------------|
|G4/G5/F4 | 4,8,9 |
|F4PRO | 4,6 |
|G6/G7 | 8,16,18 |
|G2/R2/X4 | 5 |
|G1 | 9 |
|S4/S4B/G4C/TX8/TX20 | 4 |
|S2 | 3 |
|TG15/TG30/TG50 | 10,18,20 |
|T5/T15 | 20 |
## Frequency Table
| LiDAR | frequency |
|-----------------------------------------------|------------------------|
|G1/G2/R2/G6/G7/G4/G5/G4PRO/F4/F4PRO | 5-12Hz |
|S4/S4B/S2/TX8/TX20/X4 | Not Support |
|TG15/TG30/TG50 | 3-16Hz |
|T5/T15 | 5-35Hz |
Note: For unsupported LiDARs, adjusting the scanning frequency requires external access to PWM speed control.
## Reversion Table
<table>
<tr><th>LiDAR <th>reversion
<tr><th>G1/G2/G2A/G2C/F4/F4PRO/R2 <td>true
<tr><th>G4/G5/G4PRO/G4B/G4C/G6/G7 <td>true
<tr><th>TG15/TG30/TG50 <td>true
<tr><th>T5/T15 <td>true
<tr><th>S2/X2/X2L/X4/S4/S4B <td>false
<tr><th>TX8/TX20 <td>false
</table>
## Intensity Table
<table>
<tr><th>LiDAR <th>intensity
<tr><th>S4B/G2/G4B <td>true
<tr><th>G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 <td>false
<tr><th>G1/G2A/G2C/R2 <td>false
<tr><th>S2/X2/X2L/X4 <td>false
<tr><th>TG15/TG30/TG50 <td>false
<tr><th>TX8/TX20 <td>false
<tr><th>T5/T15 <td>true
<tr><th> <td>false
</table>
## DTR Support Table
<table>
<tr><th>LiDAR <th>support_motor_dtr
<tr><th>S4/S4B/S2/X2/X2L/X4 <td>true
<tr><th>TX8/TX20 <td>true
<tr><th>G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 <td>false
<tr><th>G1/G2A/G2C/R2/G2/G4B <td>false
<tr><th>TG15/TG30/TG50 <td>false
<tr><th>T5/T15 <td>false
</table>

BIN
ros2/src/ydlidar_ros2_driver/images/EAI.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 515 KiB

BIN
ros2/src/ydlidar_ros2_driver/images/YDLidar.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
ros2/src/ydlidar_ros2_driver/images/cmake_error.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 83 KiB

BIN
ros2/src/ydlidar_ros2_driver/images/finished.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 19 KiB

BIN
ros2/src/ydlidar_ros2_driver/images/rviz.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 40 KiB

BIN
ros2/src/ydlidar_ros2_driver/images/view.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 128 KiB

29
ros2/src/ydlidar_ros2_driver/launch/ydlidar.py Normal file
View File

@@ -0,0 +1,29 @@
from launch.exit_handler import ignore_exit_handler, restart_exit_handler
from ros2run.api import get_executable_path
def launch(launch_descriptor, argv):
ld = launch_descriptor
package = 'ydlidar_ros2_driver'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='ydlidar_ros2_driver_node')],
name='ydlidar_ros2_driver_node',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for base_link -> laser_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'0', '0', '0.02',
'0', '0', '0', '1',
'base_link',
'laser_frame'
],
name='static_tf_pub_laser',
exit_handler=restart_exit_handler,
)
return ld

56
ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch.py Normal file
View File

@@ -0,0 +1,56 @@
#!/usr/bin/python3
# Copyright 2020, EAIBOT
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo
import lifecycle_msgs.msg
import os
def generate_launch_description():
share_dir = get_package_share_directory('ydlidar_ros2_driver')
parameter_file = LaunchConfiguration('params_file')
node_name = 'ydlidar_ros2_driver_node'
params_declare = DeclareLaunchArgument('params_file',
default_value=os.path.join(
share_dir, 'params', 'TminiPro.yaml'),
description='FPath to the ROS2 parameters file to use.')
driver_node = LifecycleNode(package='ydlidar_ros2_driver',
executable='ydlidar_ros2_driver_node',
name='ydlidar_ros2_driver_node',
output='screen',
emulate_tty=True,
parameters=[parameter_file],
namespace='/',
)
# tf2_node = Node(package='tf2_ros',
# executable='static_transform_publisher',
# name='static_tf_pub_laser',
# arguments=['0', '0', '0.00','0', '0', '0', '1','base_link','laser_frame'],
# )
return LaunchDescription([
params_declare,
driver_node,
# tf2_node,
])

63
ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py Normal file
View File

@@ -0,0 +1,63 @@
#!/usr/bin/python3
# Copyright 2020, EAIBOTd
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import LogInfo
import lifecycle_msgs.msg
import os
def generate_launch_description():
share_dir = get_package_share_directory('ydlidar_ros2_driver')
rviz_config_file = os.path.join(share_dir, 'config','ydlidar.rviz')
parameter_file = LaunchConfiguration('params_file')
node_name = 'ydlidar_ros2_driver_node'
params_declare = DeclareLaunchArgument('params_file',
default_value=os.path.join(
share_dir, 'params', 'TminiPro.yaml'),
description='FPath to the ROS2 parameters file to use.')
driver_node = LifecycleNode(package='ydlidar_ros2_driver',
executable='ydlidar_ros2_driver_node',
name='ydlidar_ros2_driver_node',
output='screen',
emulate_tty=True,
parameters=[parameter_file],
namespace='/',
)
tf2_node = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_tf_pub_laser',
arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'],
)
# rviz2_node = Node(package='rviz2',
# executable='rviz2',
# name='rviz2',
# arguments=['-d', rviz_config_file],
# )
return LaunchDescription([
params_declare,
driver_node,
tf2_node,
#rviz2_node,
])

33
ros2/src/ydlidar_ros2_driver/package.xml Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>ydlidar_ros2_driver</name>
<version>1.0.1</version>
<description>
The ROS2 device driver for YDLIDAR LIDARS
</description>
<maintainer email="support@ydlidar.com">Tony</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

23
ros2/src/ydlidar_ros2_driver/params/G1.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
lidar_type: 1
device_type: 0
sample_rate: 9
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 16.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

24
ros2/src/ydlidar_ros2_driver/params/G2.yaml Normal file
View File

@@ -0,0 +1,24 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
lidar_type: 1
device_type: 0
sample_rate: 4
intensity_bit: 10
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: true
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 16.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

23
ros2/src/ydlidar_ros2_driver/params/G6.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 512000
lidar_type: 1
device_type: 0
sample_rate: 18
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 25.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

24
ros2/src/ydlidar_ros2_driver/params/GS2.yaml Normal file
View File

@@ -0,0 +1,24 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 921600
lidar_type: 3
device_type: 0
sample_rate: 9
intensity_bit: 0
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 1.0
range_min: 0.025
frequency: 10.0
invalid_range_is_inf: false

23
ros2/src/ydlidar_ros2_driver/params/TEA.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: 192.168.0.11
frame_id: laser_frame
ignore_array: ""
baudrate: 8090
lidar_type: 0
device_type: 1
sample_rate: 20
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: true
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 50.0
range_min: 0.01
frequency: 20.0
invalid_range_is_inf: false

23
ros2/src/ydlidar_ros2_driver/params/TG.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 512000
lidar_type: 0
device_type: 0
sample_rate: 20
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 50.0
range_min: 0.01
frequency: 10.0
invalid_range_is_inf: false

24
ros2/src/ydlidar_ros2_driver/params/TminiPro.yaml Normal file
View File

@@ -0,0 +1,24 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttylzulaser
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
lidar_type: 1
device_type: 0
sample_rate: 4
intensity_bit: 8
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: true
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 12.0
range_min: 0.10
frequency: 10.0
invalid_range_is_inf: true

23
ros2/src/ydlidar_ros2_driver/params/X2.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 115200
lidar_type: 1
device_type: 0
sample_rate: 3
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: true
intensity: false
support_motor_dtr: true
angle_max: 180.0
angle_min: -180.0
range_max: 12.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

23
ros2/src/ydlidar_ros2_driver/params/X4-Pro.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 128000
lidar_type: 1
device_type: 0
sample_rate: 5
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: true
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 12.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

23
ros2/src/ydlidar_ros2_driver/params/X4.yaml Normal file
View File

@@ -0,0 +1,23 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 128000
lidar_type: 1
device_type: 0
sample_rate: 5
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: true
angle_max: 180.0
angle_min: -180.0
range_max: 12.0
range_min: 0.1
frequency: 10.0
invalid_range_is_inf: false

24
ros2/src/ydlidar_ros2_driver/params/ydlidar.yaml Normal file
View File

@@ -0,0 +1,24 @@
ydlidar_ros2_driver_node:
ros__parameters:
port: /dev/ttyUSB0
frame_id: laser_frame
ignore_array: ""
baudrate: 230400
lidar_type: 1
device_type: 0
sample_rate: 9
intensity_bit: 0
abnormal_check_count: 4
fixed_resolution: true
reversion: true
inverted: true
auto_reconnect: true
isSingleChannel: false
intensity: false
support_motor_dtr: false
angle_max: 180.0
angle_min: -180.0
range_max: 64.0
range_min: 0.01
frequency: 10.0
invalid_range_is_inf: false

42
ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*
* YDLIDAR SYSTEM
* YDLIDAR ROS 2 Node Client
*
* Copyright 2017 - 2020 EAI TEAM
* http://www.eaibot.com
*
*/
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <math.h>
#define RAD2DEG(x) ((x)*180./M_PI)
static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) {
int count = scan->scan_time / scan->time_increment;
printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count);
printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min),
RAD2DEG(scan->angle_max));
for (int i = 0; i < count; i++) {
float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
printf("[YDLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]);
}
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_client");
auto lidar_info_sub = node->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", rclcpp::SensorDataQoS(), scanCb);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

266
ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp Normal file
View File

@@ -0,0 +1,266 @@
/*
* YDLIDAR SYSTEM
* YDLIDAR ROS 2 Node
*
* Copyright 2017 - 2020 EAI TEAM
* http://www.eaibot.com
*
*/
#ifdef _MSC_VER
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#endif
#include "src/CYdLidar.h"
#include <math.h>
#include <chrono>
#include <iostream>
#include <memory>
#include "sensor_msgs/msg/point_cloud.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time_source.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
#include <vector>
#include <iostream>
#include <string>
#include <signal.h>
#include <limits>
#define ROS2Verision "1.0.1"
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node");
RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str());
CYdLidar laser;
std::string str_optvalue = "/dev/ydlidar";
node->declare_parameter("port", str_optvalue);
node->get_parameter("port", str_optvalue);
///lidar port
laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size());
///ignore array
str_optvalue = "";
node->declare_parameter("ignore_array", str_optvalue);
node->get_parameter("ignore_array", str_optvalue);
laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size());
std::string frame_id = "laser_frame";
node->declare_parameter("frame_id", frame_id);
node->get_parameter("frame_id", frame_id);
//////////////////////int property/////////////////
/// lidar baudrate
int optval = 230400;
node->declare_parameter("baudrate", optval);
node->get_parameter("baudrate", optval);
laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
/// tof lidar
optval = TYPE_TRIANGLE;
node->declare_parameter("lidar_type", optval);
node->get_parameter("lidar_type", optval);
laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
/// device type
optval = YDLIDAR_TYPE_SERIAL;
node->declare_parameter("device_type", optval);
node->get_parameter("device_type", optval);
laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
/// sample rate
optval = 9;
node->declare_parameter("sample_rate", optval);
node->get_parameter("sample_rate", optval);
laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
/// abnormal count
optval = 4;
node->declare_parameter("abnormal_check_count", optval);
node->get_parameter("abnormal_check_count", optval);
laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));
/// Intenstiy bit count
optval = 8;
node->declare_parameter("intensity_bit", optval);
node->get_parameter("intensity_bit", optval);
laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int));
//////////////////////bool property/////////////////
/// fixed angle resolution
bool b_optvalue = false;
node->declare_parameter("fixed_resolution", b_optvalue);
node->get_parameter("fixed_resolution", b_optvalue);
laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
/// rotate 180
b_optvalue = true;
node->declare_parameter("reversion", b_optvalue);
node->get_parameter("reversion", b_optvalue);
laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
/// Counterclockwise
b_optvalue = true;
node->declare_parameter("inverted", b_optvalue);
node->get_parameter("inverted", b_optvalue);
laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
b_optvalue = true;
node->declare_parameter("auto_reconnect", b_optvalue);
node->get_parameter("auto_reconnect", b_optvalue);
laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
/// one-way communication
b_optvalue = false;
node->declare_parameter("isSingleChannel", b_optvalue);
node->get_parameter("isSingleChannel", b_optvalue);
laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
/// intensity
b_optvalue = false;
node->declare_parameter("intensity", b_optvalue);
node->get_parameter("intensity", b_optvalue);
laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
b_optvalue = false;
node->declare_parameter("support_motor_dtr", b_optvalue);
node->get_parameter("support_motor_dtr", b_optvalue);
laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
//////////////////////float property/////////////////
/// unit: °
float f_optvalue = 180.0f;
node->declare_parameter("angle_max", f_optvalue);
node->get_parameter("angle_max", f_optvalue);
laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
f_optvalue = -180.0f;
node->declare_parameter("angle_min", f_optvalue);
node->get_parameter("angle_min", f_optvalue);
laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
/// unit: m
f_optvalue = 64.f;
node->declare_parameter("range_max", f_optvalue);
node->get_parameter("range_max", f_optvalue);
laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
f_optvalue = 0.1f;
node->declare_parameter("range_min", f_optvalue);
node->get_parameter("range_min", f_optvalue);
laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
/// unit: Hz
f_optvalue = 10.f;
node->declare_parameter("frequency", f_optvalue);
node->get_parameter("frequency", f_optvalue);
laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));
bool invalid_range_is_inf = false;
node->declare_parameter("invalid_range_is_inf", invalid_range_is_inf);
node->get_parameter("invalid_range_is_inf", invalid_range_is_inf);
bool ret = laser.initialize();
if (ret) {
ret = laser.turnOn();
} else {
RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError());
}
auto laser_pub = node->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS());
auto pc_pub = node->create_publisher<sensor_msgs::msg::PointCloud>("point_cloud", rclcpp::SensorDataQoS());
auto stop_scan_service =
[&laser](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> req,
std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
{
return laser.turnOff();
};
auto stop_service = node->create_service<std_srvs::srv::Empty>("stop_scan",stop_scan_service);
auto start_scan_service =
[&laser](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> req,
std::shared_ptr<std_srvs::srv::Empty::Response> response) -> bool
{
return laser.turnOn();
};
auto start_service = node->create_service<std_srvs::srv::Empty>("start_scan",start_scan_service);
rclcpp::WallRate loop_rate(20);
while (ret && rclcpp::ok()) {
LaserScan scan;//
if (laser.doProcessSimple(scan)) {
auto scan_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
auto pc_msg = std::make_shared<sensor_msgs::msg::PointCloud>();
scan_msg->header.stamp = node->now();//this->now();
//scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp);
//scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec);
scan_msg->header.frame_id = frame_id;
pc_msg->header = scan_msg->header;
scan_msg->angle_min = scan.config.min_angle;
scan_msg->angle_max = scan.config.max_angle;
scan_msg->angle_increment = scan.config.angle_increment;
scan_msg->scan_time = scan.config.scan_time;
scan_msg->time_increment = scan.config.time_increment;
scan_msg->range_min = scan.config.min_range;
scan_msg->range_max = scan.config.max_range;
int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1;
const float invalid_range_value =
invalid_range_is_inf ? std::numeric_limits<float>::infinity() : 0.0f;
scan_msg->ranges.assign(size, invalid_range_value);
scan_msg->intensities.assign(size, 0.0f);
pc_msg->channels.resize(2);
int idx_intensity = 0;
pc_msg->channels[idx_intensity].name = "intensities";
int idx_timestamp = 1;
pc_msg->channels[idx_timestamp].name = "stamps";
for(size_t i=0; i < scan.points.size(); i++) {
int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment);
if(index >=0 && index < size) {
if (scan.points[i].range >= scan.config.min_range) {
scan_msg->ranges[index] = scan.points[i].range;
scan_msg->intensities[index] = scan.points[i].intensity;
}
}
if (scan.points[i].range >= scan.config.min_range &&
scan.points[i].range <= scan.config.max_range) {
geometry_msgs::msg::Point32 point;
point.x = scan.points[i].range * cos(scan.points[i].angle);
point.y = scan.points[i].range * sin(scan.points[i].angle);
point.z = 0.0;
pc_msg->points.push_back(point);
pc_msg->channels[idx_intensity].values.push_back(scan.points[i].intensity);
pc_msg->channels[idx_timestamp].values.push_back(i * scan.config.time_increment);
}
}
laser_pub->publish(*scan_msg);
pc_pub->publish(*pc_msg);
} else {
RCLCPP_ERROR(node->get_logger(), "Failed to get scan");
}
if(!rclcpp::ok()) {
break;
}
rclcpp::spin_some(node);
loop_rate.sleep();
}
RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping .......");
laser.turnOff();
laser.disconnecting();
rclcpp::shutdown();
return 0;
}

11
ros2/src/ydlidar_ros2_driver/startup/initenv.sh Executable file
View File

@@ -0,0 +1,11 @@
#!/bin/bash
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules
echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules
service udev reload
sleep 2
service udev restart