diff --git a/ros2/src/ydlidar_ros2_driver/CMakeLists.txt b/ros2/src/ydlidar_ros2_driver/CMakeLists.txt new file mode 100644 index 0000000..af4e2b2 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/CMakeLists.txt @@ -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() + diff --git a/ros2/src/ydlidar_ros2_driver/LICENSE.txt b/ros2/src/ydlidar_ros2_driver/LICENSE.txt new file mode 100644 index 0000000..e69de29 diff --git a/ros2/src/ydlidar_ros2_driver/README.md b/ros2/src/ydlidar_ros2_driver/README.md new file mode 100644 index 0000000..9b5d49b --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/README.md @@ -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") + + >Note: If the following error occurs, Please install [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) first. + + ![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
Publish LaserScan message on `scan` topic | +| ydlidar_launch.py | Connect ydlidar.yaml Lidar specified by configuration parameters
Publish LaserScan message on `scan` topic | +| ydlidar_launch_view.py | Connect ydlidar.yaml Lidar specified by configuration parameters and setup RVIZ
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
it can be set to `/dev/ttyUSB0`, `192.168.1.11`, etc.
default: `/dev/ydlidar` | +| frame_id | string | Lidar TF coordinate system name.
default: `laser_frame` | +| ignore_array | string | LiDAR filtering angle area
eg: `-90, -80, 30, 40` | +| baudrate | int | Lidar baudrate or network port.
default: `230400` | +| lidar_type | int | Set lidar type
0 -- TYPE_TOF
1 -- TYPE_TRIANGLE
2 -- TYPE_TOF_NET
default: `1` | +| device_type | int | Set device type
0 -- YDLIDAR_TYPE_SERIAL
1 -- YDLIDAR_TYPE_TCP
2 -- YDLIDAR_TYPE_UDP
default: `0` | +| sample_rate | int | Set Lidar Sample Rate.
default: `9` | +| abnormal_check_count | int | Set the number of abnormal startup data attempts.
default: `4` | +| fixed_resolution | bool | Fixed angluar resolution.
default: `true` | +| reversion | bool | Reversion LiDAR.
default: `true` | +| inverted | bool | Inverted LiDAR.
false -- ClockWise.
true -- CounterClockWise
default: `true` | +| auto_reconnect | bool | Automatically reconnect the LiDAR.
true -- hot plug.
default: `true` | +| isSingleChannel | bool | Whether LiDAR is a single-channel.
default: `false` | +| intensity | bool | Whether LiDAR has intensity.
true -- G2 LiDAR.
default: `false` | +| support_motor_dtr | bool | Whether the Lidar can be started and stopped by Serial DTR.
default: `false` | +| angle_min | float | Minimum Valid Angle.
default: `-180` | +| angle_max | float | Maximum Valid Angle.
default: `180` | +| range_min | float | Minimum Valid range.
default: `0.1` | +| range_max | float | Maximum Valid range.
default: `16.0` | +| frequency | float | Set Scanning Frequency.
default: `10.0` | +| invalid_range_is_inf | bool | Invalid Range is inf.
true -- inf.
false -- 0.0.
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) + + + + + + diff --git a/ros2/src/ydlidar_ros2_driver/config/ydlidar.rviz b/ros2/src/ydlidar_ros2_driver/config/ydlidar.rviz new file mode 100644 index 0000000..7c9e799 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/config/ydlidar.rviz @@ -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: + 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: + 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 diff --git a/ros2/src/ydlidar_ros2_driver/details.md b/ros2/src/ydlidar_ros2_driver/details.md new file mode 100644 index 0000000..04b1edb --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/details.md @@ -0,0 +1,133 @@ +# ROS Paramters Table + +## Dataset + +
LIDAR Model Baudrate SampleRate(K) Range(m) Frequency(HZ) Intenstiy(bit) SingleChannel voltage(V) +
F4 1 115200 4 0.12~12 5~12 false false 4.8~5.2 +
S4 4 115200 4 0.10~8.0 5~12 (PWM) false false 4.8~5.2 +
S4B 4/11 153600 4 0.10~8.0 5~12(PWM) true(8) false 4.8~5.2 +
S2 4/12 115200 3 0.10~8.0 4~8(PWM) false true 4.8~5.2 +
G4 5 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
X4 6 128000 5 0.12~10 5~12(PWM) false false 4.8~5.2 +
X2/X2L 6 115200 3 0.10~8.0 4~8(PWM) false true 4.8~5.2 +
G4PRO 7 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
F4PRO 8 230400 4/6 0.12~12 5~12 false false 4.8~5.2 +
R2 9 230400 5 0.12~16 5~12 false false 4.8~5.2 +
G6 13 512000 18/16/8 0.28/0.26/0.1~25 5~12 false false 4.8~5.2 +
G2A 14 230400 5 0.12~12 5~12 false false 4.8~5.2 +
G2 15 230400 5 0.28~16 5~12 true(8) false 4.8~5.2 +
G2C 16 115200 4 0.1~12 5~12 false false 4.8~5.2 +
G4B 17 512000 10 0.12~16 5~12 true(10) false 4.8~5.2 +
G4C 18 115200 4 0.1~12 5~12 false false 4.8~5.2 +
G1 19 230400 9 0.28~16 5~12 false false 4.8~5.2 +
G5 20 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
G7 21 512000 18/16/8 0.28/0.26/0.1~25 5~12 false false 4.8~5.2 +
TX8 　 100 115200 4 0.05~8 4~8(PWM) false true 4.8~5.2 +
TX20 　 100 115200 4 0.05~20 4~8(PWM) false true 4.8~5.2 +
TG15 　 100 512000 20/18/10 0.05~30 3~16 false false 4.8~5.2 +
TG30 　 101 512000 20/18/10 0.05~30 3~16 false false 4.8~5.2 +
TG50 　 102 512000 20/18/10 0.05~50 3~16 false false 4.8~5.2 +
T15 　 200 8000 20 0.05~30 5~35 true false 4.8~5.2 +
+ +## 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 + +
LiDAR reversion +
G1/G2/G2A/G2C/F4/F4PRO/R2 true +
G4/G5/G4PRO/G4B/G4C/G6/G7 true +
TG15/TG30/TG50 true +
T5/T15 true +
S2/X2/X2L/X4/S4/S4B false +
TX8/TX20 false +
+ +## Intensity Table + +
LiDAR intensity +
S4B/G2/G4B true +
G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 false +
G1/G2A/G2C/R2 false +
S2/X2/X2L/X4 false +
TG15/TG30/TG50 false +
TX8/TX20 false +
T5/T15 true +
false +
+ +## DTR Support Table + + +
LiDAR support_motor_dtr +
S4/S4B/S2/X2/X2L/X4 true +
TX8/TX20 true +
G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 false +
G1/G2A/G2C/R2/G2/G4B false +
TG15/TG30/TG50 false +
T5/T15 false +
diff --git a/ros2/src/ydlidar_ros2_driver/images/EAI.png b/ros2/src/ydlidar_ros2_driver/images/EAI.png new file mode 100644 index 0000000..5dee9e2 Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/EAI.png differ diff --git a/ros2/src/ydlidar_ros2_driver/images/YDLidar.jpg b/ros2/src/ydlidar_ros2_driver/images/YDLidar.jpg new file mode 100644 index 0000000..d764165 Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/YDLidar.jpg differ diff --git a/ros2/src/ydlidar_ros2_driver/images/cmake_error.png b/ros2/src/ydlidar_ros2_driver/images/cmake_error.png new file mode 100644 index 0000000..169970a Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/cmake_error.png differ diff --git a/ros2/src/ydlidar_ros2_driver/images/finished.png b/ros2/src/ydlidar_ros2_driver/images/finished.png new file mode 100644 index 0000000..260c306 Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/finished.png differ diff --git a/ros2/src/ydlidar_ros2_driver/images/rviz.png b/ros2/src/ydlidar_ros2_driver/images/rviz.png new file mode 100644 index 0000000..c2a016b Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/rviz.png differ diff --git a/ros2/src/ydlidar_ros2_driver/images/view.png b/ros2/src/ydlidar_ros2_driver/images/view.png new file mode 100644 index 0000000..ad821de Binary files /dev/null and b/ros2/src/ydlidar_ros2_driver/images/view.png differ diff --git a/ros2/src/ydlidar_ros2_driver/launch/ydlidar.py b/ros2/src/ydlidar_ros2_driver/launch/ydlidar.py new file mode 100644 index 0000000..3a80766 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/launch/ydlidar.py @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch.py b/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch.py new file mode 100644 index 0000000..0e61325 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch.py @@ -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, + ]) diff --git a/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py b/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py new file mode 100644 index 0000000..b6306a2 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py @@ -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, + ]) diff --git a/ros2/src/ydlidar_ros2_driver/package.xml b/ros2/src/ydlidar_ros2_driver/package.xml new file mode 100644 index 0000000..d2f73cc --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/package.xml @@ -0,0 +1,33 @@ + + + + ydlidar_ros2_driver + 1.0.1 + + The ROS2 device driver for YDLIDAR LIDARS + + Tony + MIT + + ament_cmake + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2/src/ydlidar_ros2_driver/params/G1.yaml b/ros2/src/ydlidar_ros2_driver/params/G1.yaml new file mode 100644 index 0000000..5366f27 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/G1.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/G2.yaml b/ros2/src/ydlidar_ros2_driver/params/G2.yaml new file mode 100644 index 0000000..7a28662 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/G2.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/G6.yaml b/ros2/src/ydlidar_ros2_driver/params/G6.yaml new file mode 100644 index 0000000..da6d3db --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/G6.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/GS2.yaml b/ros2/src/ydlidar_ros2_driver/params/GS2.yaml new file mode 100644 index 0000000..8dea53f --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/GS2.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/TEA.yaml b/ros2/src/ydlidar_ros2_driver/params/TEA.yaml new file mode 100644 index 0000000..da25c8b --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/TEA.yaml @@ -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 \ No newline at end of file diff --git a/ros2/src/ydlidar_ros2_driver/params/TG.yaml b/ros2/src/ydlidar_ros2_driver/params/TG.yaml new file mode 100644 index 0000000..cf65e8c --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/TG.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/TminiPro.yaml b/ros2/src/ydlidar_ros2_driver/params/TminiPro.yaml new file mode 100644 index 0000000..595155e --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/TminiPro.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/X2.yaml b/ros2/src/ydlidar_ros2_driver/params/X2.yaml new file mode 100644 index 0000000..961a451 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/X2.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/X4-Pro.yaml b/ros2/src/ydlidar_ros2_driver/params/X4-Pro.yaml new file mode 100644 index 0000000..8e49fb9 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/X4-Pro.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/X4.yaml b/ros2/src/ydlidar_ros2_driver/params/X4.yaml new file mode 100644 index 0000000..716f2f8 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/X4.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/params/ydlidar.yaml b/ros2/src/ydlidar_ros2_driver/params/ydlidar.yaml new file mode 100644 index 0000000..c576896 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/params/ydlidar.yaml @@ -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 diff --git a/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp b/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp new file mode 100644 index 0000000..7bac433 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp @@ -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 + +#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( + "scan", rclcpp::SensorDataQoS(), scanCb); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + + return 0; +} diff --git a/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp b/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp new file mode 100644 index 0000000..144ecd4 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp @@ -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 +#include +#include +#include +#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 +#include +#include +#include +#include + +#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("scan", rclcpp::SensorDataQoS()); + auto pc_pub = node->create_publisher("point_cloud", rclcpp::SensorDataQoS()); + + auto stop_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOff(); + }; + + auto stop_service = node->create_service("stop_scan",stop_scan_service); + + auto start_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOn(); + }; + + auto start_service = node->create_service("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(); + auto pc_msg = std::make_shared(); + 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::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; +} diff --git a/ros2/src/ydlidar_ros2_driver/startup/initenv.sh b/ros2/src/ydlidar_ros2_driver/startup/initenv.sh new file mode 100755 index 0000000..9b5c1d0 --- /dev/null +++ b/ros2/src/ydlidar_ros2_driver/startup/initenv.sh @@ -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 +