From 57d0fa0cbf5806bc06d252dbb13039b5b425276a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 2 Jun 2025 13:30:37 +0200 Subject: [PATCH 01/12] Add timestamp fallback to packet time if no table is available --- velodyne_pointcloud/src/lib/rawdata.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/velodyne_pointcloud/src/lib/rawdata.cpp b/velodyne_pointcloud/src/lib/rawdata.cpp index 15f116bba..6e11e82ba 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cpp +++ b/velodyne_pointcloud/src/lib/rawdata.cpp @@ -329,6 +329,8 @@ void RawData::unpack( if (timing_offsets_.size()) { time = timing_offsets_[i][j] + time_diff_start_to_this_packet; } + else + time = time_diff_start_to_this_packet; if (tmp.uint == 0) { // no valid laser beam return // call to addPoint is still required since output could be organized @@ -547,6 +549,8 @@ void RawData::unpack_vls128( time = timing_offsets_[block / 4][firing_order + laser_number / 64] + time_diff_start_to_this_packet; } + else + time = time_diff_start_to_this_packet; velodyne_pointcloud::LaserCorrection & corrections = calibration_->laser_corrections[laser_number]; @@ -812,6 +816,8 @@ void RawData::unpack_vlp16( if (timing_offsets_.size()) { time = timing_offsets_[block][firing * 16 + dsr] + time_diff_start_to_this_packet; } + else + time = time_diff_start_to_this_packet; data.addPoint( x_coord, y_coord, z_coord, corrections.laser_ring, From 5ae386643513ec614e7ae28e09f0a89b965e47b1 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 12:00:20 +0200 Subject: [PATCH 02/12] Bump to retrigger checks From 3fadcc3b2c46d9dd28af38802ec64192784a2a60 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 12:12:13 +0200 Subject: [PATCH 03/12] Change formatting --- velodyne_pointcloud/src/lib/rawdata.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cpp b/velodyne_pointcloud/src/lib/rawdata.cpp index 6e11e82ba..e7ff1541f 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cpp +++ b/velodyne_pointcloud/src/lib/rawdata.cpp @@ -328,8 +328,7 @@ void RawData::unpack( float time = 0; if (timing_offsets_.size()) { time = timing_offsets_[i][j] + time_diff_start_to_this_packet; - } - else + } else time = time_diff_start_to_this_packet; if (tmp.uint == 0) { // no valid laser beam return @@ -548,8 +547,7 @@ void RawData::unpack_vls128( if (timing_offsets_.size()) { time = timing_offsets_[block / 4][firing_order + laser_number / 64] + time_diff_start_to_this_packet; - } - else + } else time = time_diff_start_to_this_packet; velodyne_pointcloud::LaserCorrection & corrections = @@ -815,8 +813,7 @@ void RawData::unpack_vlp16( float time = 0; if (timing_offsets_.size()) { time = timing_offsets_[block][firing * 16 + dsr] + time_diff_start_to_this_packet; - } - else + } else time = time_diff_start_to_this_packet; data.addPoint( From 9ed4f0da01a5917a17ad09fa4006fdd5349463b5 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 12:19:46 +0200 Subject: [PATCH 04/12] Change formatting --- velodyne_pointcloud/src/lib/rawdata.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cpp b/velodyne_pointcloud/src/lib/rawdata.cpp index e7ff1541f..b13bba047 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cpp +++ b/velodyne_pointcloud/src/lib/rawdata.cpp @@ -328,9 +328,10 @@ void RawData::unpack( float time = 0; if (timing_offsets_.size()) { time = timing_offsets_[i][j] + time_diff_start_to_this_packet; - } else + } else { time = time_diff_start_to_this_packet; - + } + if (tmp.uint == 0) { // no valid laser beam return // call to addPoint is still required since output could be organized data.addPoint( @@ -547,9 +548,10 @@ void RawData::unpack_vls128( if (timing_offsets_.size()) { time = timing_offsets_[block / 4][firing_order + laser_number / 64] + time_diff_start_to_this_packet; - } else + } else { time = time_diff_start_to_this_packet; - + } + velodyne_pointcloud::LaserCorrection & corrections = calibration_->laser_corrections[laser_number]; @@ -813,9 +815,10 @@ void RawData::unpack_vlp16( float time = 0; if (timing_offsets_.size()) { time = timing_offsets_[block][firing * 16 + dsr] + time_diff_start_to_this_packet; - } else + } else { time = time_diff_start_to_this_packet; - + } + data.addPoint( x_coord, y_coord, z_coord, corrections.laser_ring, distance, intensity, time); From 0d3b0db98096f5a3c126bdce567c7d0c01679bd8 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 13:23:31 +0200 Subject: [PATCH 05/12] Change formatting --- velodyne_pointcloud/src/lib/rawdata.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cpp b/velodyne_pointcloud/src/lib/rawdata.cpp index b13bba047..c60332f88 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cpp +++ b/velodyne_pointcloud/src/lib/rawdata.cpp @@ -331,7 +331,7 @@ void RawData::unpack( } else { time = time_diff_start_to_this_packet; } - + if (tmp.uint == 0) { // no valid laser beam return // call to addPoint is still required since output could be organized data.addPoint( @@ -551,7 +551,7 @@ void RawData::unpack_vls128( } else { time = time_diff_start_to_this_packet; } - + velodyne_pointcloud::LaserCorrection & corrections = calibration_->laser_corrections[laser_number]; @@ -818,7 +818,7 @@ void RawData::unpack_vlp16( } else { time = time_diff_start_to_this_packet; } - + data.addPoint( x_coord, y_coord, z_coord, corrections.laser_ring, distance, intensity, time); From fd410945410a291dfda58d8289c378cf45de0f10 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 13:35:29 +0200 Subject: [PATCH 06/12] Fix include for rolling --- velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp index 1f7ef7b04..530193a5c 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp @@ -33,7 +33,7 @@ #ifndef VELODYNE_POINTCLOUD__TRANSFORM_HPP_ #define VELODYNE_POINTCLOUD__TRANSFORM_HPP_ -#include +#include #include #include #include From e533efed303f2e6729d66bea6c893a4c7d738964 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 13:44:29 +0200 Subject: [PATCH 07/12] Fix include ordering --- velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp index 530193a5c..012386bd5 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp @@ -33,7 +33,6 @@ #ifndef VELODYNE_POINTCLOUD__TRANSFORM_HPP_ #define VELODYNE_POINTCLOUD__TRANSFORM_HPP_ -#include #include #include #include @@ -43,6 +42,7 @@ #include #include +#include #include #include #include From 3ddc4f2e981369043a36a703fbf5041e37eaf4b4 Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 14:12:06 +0200 Subject: [PATCH 08/12] Add check for Iron --- velodyne_pointcloud/CMakeLists.txt | 5 ++++- .../include/velodyne_pointcloud/transform.hpp | 6 +++++- velodyne_pointcloud/package.xml | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 2a6757f74..df356c4a0 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(message_filters REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rosidl_cmake REQUIRED) find_package(sensor_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -73,9 +74,11 @@ target_link_libraries(velodyne_cloud_types PUBLIC tf2_ros::tf2_ros ${velodyne_msgs_TARGETS} ) - add_library(transform SHARED src/conversions/transform.cpp) +if(${rosidl_cmake_VERSION} VERSION_LESS 4.0.1) + target_compile_definitions(transform PRIVATE ROS2_IRON) +endif() target_include_directories(transform PUBLIC "$" "$") diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp index 012386bd5..4ce55b804 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.hpp @@ -32,7 +32,9 @@ #ifndef VELODYNE_POINTCLOUD__TRANSFORM_HPP_ #define VELODYNE_POINTCLOUD__TRANSFORM_HPP_ - +#ifdef ROS2_IRON +#include +#endif #include #include #include @@ -42,7 +44,9 @@ #include #include +#ifndef ROS2_IRON #include +#endif #include #include #include diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 6c705b7db..49a0d539d 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -16,6 +16,7 @@ Andreas Klintberg ament_cmake + rosidl_cmake angles diagnostic_updater From 2319fb3cb557e9a3de9f3048f4933a8fac5503cb Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 14:19:51 +0200 Subject: [PATCH 09/12] Fix check for Iron --- velodyne_pointcloud/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index df356c4a0..f61c8816c 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -76,7 +76,7 @@ target_link_libraries(velodyne_cloud_types PUBLIC ) add_library(transform SHARED src/conversions/transform.cpp) -if(${rosidl_cmake_VERSION} VERSION_LESS 4.0.1) +if(${rosidl_cmake_VERSION} VERSION_EQUAL 4.0.1) target_compile_definitions(transform PRIVATE ROS2_IRON) endif() target_include_directories(transform PUBLIC From f2e9fef21c30501800b595d2e2a8dcfc45a9ff4d Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 15:04:25 +0200 Subject: [PATCH 10/12] Add version output --- velodyne_pointcloud/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index f61c8816c..5f7bd8ee1 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -76,6 +76,7 @@ target_link_libraries(velodyne_cloud_types PUBLIC ) add_library(transform SHARED src/conversions/transform.cpp) +message("rosidl_cmake_VERSION: ${rosidl_cmake_VERSION}") if(${rosidl_cmake_VERSION} VERSION_EQUAL 4.0.1) target_compile_definitions(transform PRIVATE ROS2_IRON) endif() From 1c5c3c336be4ce99f393cb7989a37abee3c8012c Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 15:12:04 +0200 Subject: [PATCH 11/12] Fix check for Iron --- velodyne_pointcloud/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 5f7bd8ee1..a0d5fee73 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -78,7 +78,7 @@ add_library(transform SHARED src/conversions/transform.cpp) message("rosidl_cmake_VERSION: ${rosidl_cmake_VERSION}") if(${rosidl_cmake_VERSION} VERSION_EQUAL 4.0.1) - target_compile_definitions(transform PRIVATE ROS2_IRON) + target_compile_definitions(transform PUBLIC ROS2_IRON) endif() target_include_directories(transform PUBLIC "$" From c4bb01915a6eaf2e2b0a87b53251af164f77649a Mon Sep 17 00:00:00 2001 From: Thomas Emter Date: Mon, 23 Jun 2025 15:19:30 +0200 Subject: [PATCH 12/12] Remove version output --- velodyne_pointcloud/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index a0d5fee73..6d38d4894 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -76,7 +76,6 @@ target_link_libraries(velodyne_cloud_types PUBLIC ) add_library(transform SHARED src/conversions/transform.cpp) -message("rosidl_cmake_VERSION: ${rosidl_cmake_VERSION}") if(${rosidl_cmake_VERSION} VERSION_EQUAL 4.0.1) target_compile_definitions(transform PUBLIC ROS2_IRON) endif()