From 76eae066e5312f30b2424767ce049d6344a20055 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Mon, 21 Nov 2022 16:48:19 +0900 Subject: [PATCH] revert: "feat(pointcloud_preprocessor): use point_cloud_msg_wrapper" (#2317) Revert "feat(pointcloud_preprocessor): use point_cloud_msg_wrapper (#1276)" This reverts commit ef7dcda087beffaa0acf35e9647be1cb439007ba. Signed-off-by: kminoda --- .../include/common/types.hpp | 40 ----------------- .../concatenate_data_nodelet.hpp | 1 + .../distortion_corrector.hpp | 1 + .../polygon_remover/polygon_remover.hpp | 1 + .../utility/utilities.hpp | 4 +- sensing/pointcloud_preprocessor/package.xml | 1 - .../concatenate_data_nodelet.cpp | 41 ++++++++++++------ .../crop_box_filter_nodelet.cpp | 2 + .../distortion_corrector.cpp | 43 ++++++++++--------- .../src/utility/utilities.cpp | 20 +++++---- 10 files changed, 68 insertions(+), 86 deletions(-) diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/common/types.hpp index bc99dd5a922a4..3f3e444c1aa8c 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/common/types.hpp @@ -109,46 +109,6 @@ struct COMMON_PUBLIC PointXYZI } }; -struct COMMON_PUBLIC PointXYZ -{ - float32_t x{0.0F}; - float32_t y{0.0F}; - float32_t z{0.0F}; - friend bool operator==(const PointXYZ & p1, const PointXYZ & p2) noexcept - { - return helper_functions::comparisons::rel_eq( - p1.x, p2.x, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.y, p2.y, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.z, p2.z, std::numeric_limits::epsilon()); - } -}; - -struct COMMON_PUBLIC PointXYZTimestamp -{ - float32_t x{0.0F}; - float32_t y{0.0F}; - float32_t z{0.0F}; - float32_t time_stamp{0.0F}; - friend bool operator==(const PointXYZTimestamp & p1, const PointXYZTimestamp & p2) noexcept - { - return helper_functions::comparisons::rel_eq( - p1.x, p2.x, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.y, p2.y, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.z, p2.z, std::numeric_limits::epsilon()) && - - helper_functions::comparisons::rel_eq( - p1.time_stamp, p2.time_stamp, std::numeric_limits::epsilon()); - } -}; - using PointBlock = std::vector; using PointPtrBlock = std::vector; /// \brief Stores basic configuration information, does some simple validity checking diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index bf02bbd2eec75..77cfbe4610bf1 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 33df1d347860b..618957a2ac783 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp index 3c0e0c557d8cc..49baed4ed7ed5 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp @@ -19,6 +19,7 @@ #include "pointcloud_preprocessor/utility/utilities.hpp" #include +#include #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 0557c33c42c25..982b6210eca7a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -15,11 +15,9 @@ #ifndef POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ #define POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ -#include -#include - #include #include +#include #include #include diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index f1ed9917abcd7..856f7d2bc6f90 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -18,7 +18,6 @@ autoware_cmake - autoware_auto_common autoware_auto_vehicle_msgs autoware_point_types cgal diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 2a7a5b822ccb5..fbcce3b304d7e 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -51,9 +51,7 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp" -#include #include -#include #include @@ -328,19 +326,36 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { - point_cloud_msg_wrapper::PointCloud2Modifier output_modifier{ - *output_ptr, input_ptr->header.frame_id}; - - if (point_cloud_msg_wrapper::PointCloud2View< - autoware::common::types::PointXYZI>::can_be_created_from(*input_ptr)) { - point_cloud_msg_wrapper::PointCloud2View view{*input_ptr}; - for (const auto & point : view) { - output_modifier.push_back({point.x, point.y, point.z, point.intensity}); + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); } } else { - point_cloud_msg_wrapper::PointCloud2View view{*input_ptr}; - for (const auto & point : view) { - output_modifier.push_back({point.x, point.y, point.z, 0.0f}); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); } } } diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index b61d52fa08934..5cb7bc19c802e 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp" +#include + #include namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index e59baa4149ad9..40a5f59134b06 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,9 +14,6 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include -#include - #include #include #include @@ -178,22 +175,28 @@ bool DistortionCorrectorComponent::undistortPointCloud( return false; } - if (!point_cloud_msg_wrapper::PointCloud2View< - autoware::common::types::PointXYZTimestamp>::can_be_created_from(points)) { + auto time_stamp_field_it = std::find_if( + std::cbegin(points.fields), std::cend(points.fields), + [this](const sensor_msgs::msg::PointField & field) { + return field.name == time_stamp_field_name_; + }); + if (time_stamp_field_it == points.fields.cend()) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 10000 /* ms */, "Required field time stamp doesn't exist in the point cloud."); return false; } - point_cloud_msg_wrapper::PointCloud2Modifier modifier{ - points}; + sensor_msgs::PointCloud2Iterator it_x(points, "x"); + sensor_msgs::PointCloud2Iterator it_y(points, "y"); + sensor_msgs::PointCloud2Iterator it_z(points, "z"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(points, time_stamp_field_name_); float theta{0.0f}; float x{0.0f}; float y{0.0f}; - double prev_time_stamp_sec{modifier.begin()->time_stamp}; - const double first_point_time_stamp_sec{modifier.begin()->time_stamp}; + double prev_time_stamp_sec{*it_time_stamp}; + const double first_point_time_stamp_sec{*it_time_stamp}; auto twist_it = std::lower_bound( std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec, @@ -214,17 +217,17 @@ bool DistortionCorrectorComponent::undistortPointCloud( } const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()}; - for (auto & point : modifier) { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { for (; (twist_it != std::end(twist_queue_) - 1 && - point.time_stamp > rclcpp::Time(twist_it->header.stamp).seconds()); + *it_time_stamp > rclcpp::Time(twist_it->header.stamp).seconds()); ++twist_it) { } float v{static_cast(twist_it->twist.linear.x)}; float w{static_cast(twist_it->twist.angular.z)}; - if (std::abs(point.time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) { + if (std::abs(*it_time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 10000 /* ms */, "twist time_stamp is too late. Could not interpolate."); @@ -235,10 +238,10 @@ bool DistortionCorrectorComponent::undistortPointCloud( if (use_imu_ && !angular_velocity_queue_.empty()) { for (; (imu_it != std::end(angular_velocity_queue_) - 1 && - point.time_stamp > rclcpp::Time(imu_it->header.stamp).seconds()); + *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds()); ++imu_it) { } - if (std::abs(point.time_stamp - rclcpp::Time(imu_it->header.stamp).seconds()) > 0.1) { + if (std::abs(*it_time_stamp - rclcpp::Time(imu_it->header.stamp).seconds()) > 0.1) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 10000 /* ms */, "imu time_stamp is too late. Could not interpolate."); @@ -247,9 +250,9 @@ bool DistortionCorrectorComponent::undistortPointCloud( } } - const float time_offset = static_cast(point.time_stamp - prev_time_stamp_sec); + const float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); - const tf2::Vector3 sensorTF_point{point.x, point.y, point.z}; + const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z}; const tf2::Vector3 base_linkTF_point{tf2_base_link_to_sensor_inv * sensorTF_point}; @@ -268,11 +271,11 @@ bool DistortionCorrectorComponent::undistortPointCloud( const tf2::Vector3 sensorTF_trans_point{tf2_base_link_to_sensor * base_linkTF_trans_point}; - point.x = sensorTF_trans_point.getX(); - point.y = sensorTF_trans_point.getY(); - point.z = sensorTF_trans_point.getZ(); + *it_x = sensorTF_trans_point.getX(); + *it_y = sensorTF_trans_point.getY(); + *it_z = sensorTF_trans_point.getZ(); - prev_time_stamp_sec = point.time_stamp; + prev_time_stamp_sec = *it_time_stamp; } return true; } diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index bb6bc9971d230..fe39ccbcccddc 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -48,17 +48,18 @@ void remove_polygon_cgal_from_cloud( { pcl::PointCloud pcl_output; - point_cloud_msg_wrapper::PointCloud2View view{cloud_in}; - for (const auto & point : view) { + for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), + iter_z(cloud_in, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // check if the point is inside the polygon if ( CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(point.x, point.y), K()) == + polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == CGAL::ON_UNBOUNDED_SIDE) { pcl::PointXYZ p; - p.x = point.x; - p.y = point.y; - p.z = point.z; + p.x = *iter_x; + p.y = *iter_y; + p.z = *iter_z; pcl_output.emplace_back(p); } } @@ -95,10 +96,11 @@ void remove_polygon_cgal_from_cloud( } pcl::PointCloud filtered_cloud; - point_cloud_msg_wrapper::PointCloud2View view{cloud_in}; - for (const auto & point : view) { + for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), + iter_z(cloud_in, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // if the point is inside the polygon, skip inserting and check the next point - pcl::PointXYZ p(point.x, point.y, point.z); + pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); if (point_within_cgal_polys(p, polyline_polygons)) { continue; }