From a800698c4abc571ac8d104d8700e17da9fa4d7dd Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 6 Apr 2023 09:57:41 +0900 Subject: [PATCH 1/5] perf(ring_outlier_filter): a cache friendly impl Signed-off-by: Vincent Richard style(pre-commit): autofix fix(ring_outlier_filter): cleanup code with ranges Signed-off-by: Vincent Richard [breaking] fix autoware point type padding for faster memory access can memcpy between input data buffer and PointXYZI* make assumption on memory layout for faster data fetch misc optimization (reordering, constexpr, etc) Signed-off-by: Vincent Richard style(pre-commit): autofix comment limitations Signed-off-by: Vincent Richard feat(ring_outlier_filter): add accurate isCluster impl Signed-off-by: Vincent Richard style(pre-commit): autofix fix #3218 Signed-off-by: Vincent Richard cleaning Signed-off-by: Vincent Richard style(pre-commit): autofix --- .../docs/ring-outlier-filter.md | 13 +- .../ring_outlier_filter_nodelet.hpp | 16 - .../utility/utilities.hpp | 28 ++ sensing/pointcloud_preprocessor/package.xml | 1 + .../ring_outlier_filter_nodelet.cpp | 352 +++++++++++++----- .../src/utility/utilities.cpp | 102 +++++ 6 files changed, 390 insertions(+), 122 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..eedc8e6bcb573 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +22,12 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index a906eb913d863..36e5c9d85aafd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -45,7 +45,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; - size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -53,21 +52,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - bool isCluster( - const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) - { - if (walk_size > num_points_threshold_) return true; - - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); - - const auto x = first_point->x - last_point->x; - const auto y = first_point->y - last_point->y; - const auto z = first_point->z - last_point->z; - - return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; - } - public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 5de644fd8ffd1..9d3307a4d7087 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using K = CGAL::Exact_predicates_inexact_constructions_kernel; @@ -86,6 +87,33 @@ bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); +/** \brief Return offset of X field (if any). The field should match PointXYZIRADRT one */ +std::optional getXOffset(const std::vector & fields); + +/** \brief Return offset of Y field (if any). The field should match PointXYZIRADRT one */ +std::optional getYOffset(const std::vector & fields); + +/** \brief Return offset of Z field (if any). The field should match PointXYZIRADRT one */ +std::optional getZOffset(const std::vector & fields); + +/** \brief Return offset of intensity field (if any). The field should match PointXYZIRADRT one */ +std::optional getIntensityOffset(const std::vector & fields); + +/** \brief Return offset of ring field (if any). The field should match PointXYZIRADRT one */ +std::optional getRingOffset(const std::vector & fields); + +/** \brief Return offset of azimuth field (if any). The field should match PointXYZIRADRT one */ +std::optional getAzimuthOffset(const std::vector & fields); + +/** \brief Return offset of distance field (if any). The field should match PointXYZIRADRT one */ +std::optional getDistanceOffset(const std::vector & fields); + +/** \brief Return offset of return type field (if any). The field should match PointXYZIRADRT one */ +std::optional getReturnTypeOffset(const std::vector & fields); + +/** \brief Return offset of time stamp field (if any). The field should match PointXYZIRADRT one */ +std::optional getTimeStampOffset(const std::vector & fields); + } // namespace pointcloud_preprocessor::utils #endif // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 1a4b2d9925563..4e55a1fb611ee 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -31,6 +31,7 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper + range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index d2570b9c4d786..04f3d803d1530 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -13,11 +13,18 @@ // limitations under the License. #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "pointcloud_preprocessor/utility/utilities.hpp" #include +#include +#include +#include + #include +#include #include + namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -40,8 +47,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); - max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -61,130 +66,273 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width); - size_t output_size = 0; + // point field extraction helpers - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + // FIXME(VRichardJP) Everything would be simpler if Autoware enforced strict PointCloud2 format - std::vector> ring2indices; - ring2indices.reserve(max_rings_num_); - - for (uint16_t i = 0; i < max_rings_num_; i++) { - ring2indices.push_back(std::vector()); - ring2indices.back().reserve(max_points_num_per_ring_); + auto maybe_intensity_offset = utils::getIntensityOffset(input->fields); + if (!maybe_intensity_offset) { + RCLCPP_ERROR(get_logger(), "input cloud does not contain 'intensity' data field"); + return; } + const auto intensity_offset = *maybe_intensity_offset; - for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); - ring2indices[ring].push_back(data_idx); + auto maybe_ring_offset = utils::getRingOffset(input->fields); + if (!maybe_ring_offset) { + RCLCPP_ERROR(get_logger(), "input cloud does not contain 'ring' data field"); + return; } + const auto ring_offset = *maybe_ring_offset; - // walk range: [walk_first_idx, walk_last_idx] - int walk_first_idx = 0; - int walk_last_idx = -1; + auto maybe_azimuth_offset = utils::getAzimuthOffset(input->fields); + if (!maybe_azimuth_offset) { + RCLCPP_ERROR(get_logger(), "input cloud does not contain 'azimuth' data field"); + return; + } + const auto azimuth_offset = *maybe_azimuth_offset; - for (const auto & indices : ring2indices) { - if (indices.size() < 2) continue; + auto maybe_distance_offset = utils::getDistanceOffset(input->fields); + if (!maybe_distance_offset) { + RCLCPP_ERROR(get_logger(), "input cloud does not contain 'distance' data field"); + return; + } + const auto distance_offset = *maybe_distance_offset; + + // extract intensity from point raw data + auto getPointIntensity = [=](const unsigned char * raw_p) { + float intensity{}; + std::memcpy(&intensity, &raw_p[intensity_offset], sizeof(intensity)); + return intensity; + }; + + // extract ring from point raw data + auto getPointRing = [=](const unsigned char * raw_p) { + uint16_t ring{}; + std::memcpy(&ring, &raw_p[ring_offset], sizeof(ring)); + return ring; + }; + + // extract azimuth from point raw data + auto getPointAzimuth = [=](const unsigned char * raw_p) { + float azimuth{}; + std::memcpy(&azimuth, &raw_p[azimuth_offset], sizeof(azimuth)); + return azimuth; + }; + + // extract distance from point raw data + auto getPointDistance = [=](const unsigned char * raw_p) { + float distance{}; + std::memcpy(&distance, &raw_p[distance_offset], sizeof(distance)); + return distance; + }; + + // The initial implementation of ring outlier filter looked like this: + // 1. Iterate over the input cloud and group point indices by ring + // 2. For each ring: + // 2.1. iterate over the ring points, and group points belonging to the same "walk" + // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long enough. + // + // Because LIDAR data is naturally "azimut-major order" and not "ring-major order", such + // implementation is not cache friendly at all, and has negative impact on all the other nodes. + // + // To tackle this issue, the algorithm has been rewritten so that points would be accessed in + // order. To do so, all rings are being processing simultaneously instead of separately. The + // overall logic is still the same. + + // ad-hoc struct to store finished walks information (for isCluster()) + struct WalkInfo + { + int num_points; + float first_point_distance; + float first_point_azimuth; + float last_point_distance; + float last_point_azimuth; + }; + + // ad-hoc struct to keep track of each ring current walk + struct RingWalkInfo + { + size_t walk_idx; // index of associated walk in walks vector + float prev_azimuth; // azimuth of previous point on the ring + float prev_distance; // distance of previous point on the ring + }; + + // helper functions + + // check if walk is a valid cluster + const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; + auto isCluster = [=](const WalkInfo & walk_info) { + // A cluster is a walk which has many points or is long enough + if (walk_info.num_points > num_points_threshold_) return true; + + // Using the law of cosines, the distance between 2 points can be written as: + // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) + // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between + // the 2 points. + const float dist2 = + walk_info.first_point_distance * walk_info.first_point_distance + + walk_info.last_point_distance * walk_info.last_point_distance - + 2 * walk_info.first_point_distance * walk_info.last_point_distance * + std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); + return dist2 > object_length_threshold2; + }; + + // check if 2 points belong to the same walk + auto isSameWalk = + [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { + float azimuth_diff = curr_azimuth - prev_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + return std::max(curr_distance, prev_distance) < + std::min(curr_distance, prev_distance) * distance_ratio_ && + azimuth_diff < 100.f; + }; + + // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) + std::vector walks; // all walks + std::vector rings_curr_walk; // each ring curr walk info + std::vector rings_first_walk_idx; // each ring very first walk idx + std::vector points_walk_idx; // each point walk idx + + walks.reserve(max_rings_num_); + // initialize each ring with empty walk + rings_curr_walk.resize(max_rings_num_, RingWalkInfo{-1U, 0., 0.}); + rings_first_walk_idx.resize(max_rings_num_, -1U); + // points are initially associated to no walk + points_walk_idx.resize(input->width * input->height, -1U); + + int invalid_ring_count = 0; + + // Build walks and classify points + for (const auto & [raw_p, point_walk_idx] : + ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + const uint16_t ring_idx = getPointRing(raw_p.data()); + const float curr_azimuth = getPointAzimuth(raw_p.data()); + const float curr_distance = getPointDistance(raw_p.data()); + + if (ring_idx >= max_rings_num_) { + // Either the data is corrupted or max_rings_num_ is not set correctly + // Note: point_walk_idx == -1 so the point will be filtered out + invalid_ring_count += 1; + continue; + } - walk_first_idx = 0; - walk_last_idx = -1; + auto & ring = rings_curr_walk[ring_idx]; + if (ring.walk_idx == -1U) { + // first walk ever for this ring + walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}); + const auto walk_idx = walks.size() - 1; + point_walk_idx = walk_idx; + rings_first_walk_idx[ring_idx] = walk_idx; + ring.walk_idx = walk_idx; + ring.prev_azimuth = curr_azimuth; + ring.prev_distance = curr_distance; + continue; + } - for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { - const size_t & current_data_idx = indices[idx]; - const size_t & next_data_idx = indices[idx + 1]; - walk_last_idx = idx; + auto & walk = walks[ring.walk_idx]; + if (isSameWalk(curr_distance, curr_azimuth, ring.prev_distance, ring.prev_azimuth)) { + // current point is part of previous walk + walk.num_points += 1; + walk.last_point_distance = curr_distance; + walk.last_point_azimuth = curr_azimuth; + point_walk_idx = ring.walk_idx; + } else { + // previous walk is finished, start a new one + walks.push_back(WalkInfo{1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}); + point_walk_idx = walks.size() - 1; + ring.walk_idx = walks.size() - 1; + } - // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) + ring.prev_azimuth = curr_azimuth; + ring.prev_distance = curr_distance; + } - const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); - const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); - float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + // So far, we have processed ring points as if rings were not circular. Of course, the last and + // first points of a ring could totally be part of the same walk. When such thing happens, we need + // to merge the two walks + for (const auto & [first_walk_idx, last_walk_idx] : ranges::views::zip( + rings_first_walk_idx, rings_curr_walk | ranges::views::transform([](const auto & ring) { + return ring.walk_idx; + }))) { + if (first_walk_idx == -1U || last_walk_idx == -1U) { + continue; + } + auto & first_walk = walks[first_walk_idx]; + auto & last_walk = walks[last_walk_idx]; + // check if the two walks are connected + if (isSameWalk( + first_walk.first_point_distance, first_walk.first_point_azimuth, + last_walk.last_point_distance, last_walk.last_point_azimuth)) { + // merge + first_walk.first_point_distance = last_walk.first_point_distance; + first_walk.first_point_azimuth = last_walk.first_point_azimuth; + last_walk.last_point_distance = first_walk.last_point_distance; + last_walk.last_point_azimuth = first_walk.last_point_azimuth; + } + } - const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); - const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + // segregate walks by cluster (for fast lookup) + std::vector walk_is_cluster; + walk_is_cluster.reserve(walks.size()); + for (const auto & walk_info : walks) { + walk_is_cluster.push_back(isCluster(walk_info)); + } - if ( - std::max(current_distance, next_distance) < - std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + // finally copy points + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width * input->height); + size_t output_size = 0; + if (transform_info.need_transform) { + for (const auto & [raw_p, point_walk_idx] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + if (point_walk_idx == -1U || !walk_is_cluster[point_walk_idx]) { + continue; } - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; - } - } + // assume binary representation of input point is compatible with PointXYZ* + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - walk_first_idx = idx + 1; - } + Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); + p = transform_info.eigen_transform * p; + out_point.x = p[0]; + out_point.y = p[1]; + out_point.z = p[2]; + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + out_point.intensity = getPointIntensity(raw_p.data()); - if (walk_first_idx > walk_last_idx) continue; - - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + output_size += sizeof(PointXYZI); + } + } else { + for (const auto & [raw_p, point_walk_idx] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_idx)) { + if (point_walk_idx == -1U || !walk_is_cluster[point_walk_idx]) { + continue; } + +#if 0 + // assume binary representation of input point is compatible with PointXYZI + std::memcpy(&output.data[output_size], raw_p.data(), sizeof(PointXYZI)); +#else + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); + out_point.intensity = getPointIntensity(raw_p.data()); + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); +#endif + + output_size += sizeof(PointXYZI); } } - output.data.resize(output_size); - // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform // == true` output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; output.height = 1; output.width = static_cast(output.data.size() / output.height / output.point_step); + output.row_step = static_cast(output.data.size() / output.height); output.is_bigendian = input->is_bigendian; output.is_dense = input->is_dense; @@ -196,6 +344,12 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + if (invalid_ring_count > 0) { + RCLCPP_WARN( + get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", + invalid_ring_count, max_rings_num_); + } + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe9fde7301fd7..59252f92dffad 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -16,6 +16,9 @@ #include +#include +#include + namespace pointcloud_preprocessor::utils { void to_cgal_polygon(const geometry_msgs::msg::Polygon & polygon_in, PolygonCgal & polygon_out) @@ -236,4 +239,103 @@ bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::Point return same_layout; } +std::optional getXOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "x") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getYOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "y") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getZOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "z") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getIntensityOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "intensity") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getRingOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "ring") == 0 + && field.datatype == sensor_msgs::msg::PointField::UINT16 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getAzimuthOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "azimuth") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getDistanceOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "distance") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT32 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getReturnTypeOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "return_type") == 0 + && field.datatype == sensor_msgs::msg::PointField::UINT8 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + +std::optional getTimeStampOffset(const std::vector & fields) +{ + auto it = std::find_if(fields.begin(), fields.end(), + [&](const auto& field) { + return std::strcmp(field.name.c_str(), "time_stamp") == 0 + && field.datatype == sensor_msgs::msg::PointField::FLOAT64 + && field.count == 1; + }); + return (it != fields.end())? std::optional{it->offset} : std::nullopt; +} + } // namespace pointcloud_preprocessor::utils From 4f743a5c717cdcf80de13011488957c11f533ea0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 1 Jun 2023 12:55:17 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- .../utility/utilities.hpp | 15 ++- .../ring_outlier_filter_nodelet.cpp | 14 +- .../src/utility/utilities.cpp | 123 ++++++++---------- 3 files changed, 73 insertions(+), 79 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 9d3307a4d7087..3ce94528c5064 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -97,22 +97,27 @@ std::optional getYOffset(const std::vector getZOffset(const std::vector & fields); /** \brief Return offset of intensity field (if any). The field should match PointXYZIRADRT one */ -std::optional getIntensityOffset(const std::vector & fields); +std::optional getIntensityOffset( + const std::vector & fields); /** \brief Return offset of ring field (if any). The field should match PointXYZIRADRT one */ std::optional getRingOffset(const std::vector & fields); /** \brief Return offset of azimuth field (if any). The field should match PointXYZIRADRT one */ -std::optional getAzimuthOffset(const std::vector & fields); +std::optional getAzimuthOffset( + const std::vector & fields); /** \brief Return offset of distance field (if any). The field should match PointXYZIRADRT one */ -std::optional getDistanceOffset(const std::vector & fields); +std::optional getDistanceOffset( + const std::vector & fields); /** \brief Return offset of return type field (if any). The field should match PointXYZIRADRT one */ -std::optional getReturnTypeOffset(const std::vector & fields); +std::optional getReturnTypeOffset( + const std::vector & fields); /** \brief Return offset of time stamp field (if any). The field should match PointXYZIRADRT one */ -std::optional getTimeStampOffset(const std::vector & fields); +std::optional getTimeStampOffset( + const std::vector & fields); } // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 04f3d803d1530..cd9e964a25f35 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -13,14 +13,15 @@ // limitations under the License. #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" -#include +#include "pointcloud_preprocessor/utility/utilities.hpp" #include #include #include +#include + #include #include #include @@ -130,13 +131,14 @@ void RingOutlierFilterComponent::faster_filter( // 1. Iterate over the input cloud and group point indices by ring // 2. For each ring: // 2.1. iterate over the ring points, and group points belonging to the same "walk" - // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long enough. + // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long + // enough. // - // Because LIDAR data is naturally "azimut-major order" and not "ring-major order", such + // Because LIDAR data is naturally "azimut-major order" and not "ring-major order", such // implementation is not cache friendly at all, and has negative impact on all the other nodes. // - // To tackle this issue, the algorithm has been rewritten so that points would be accessed in - // order. To do so, all rings are being processing simultaneously instead of separately. The + // To tackle this issue, the algorithm has been rewritten so that points would be accessed in + // order. To do so, all rings are being processing simultaneously instead of separately. The // overall logic is still the same. // ad-hoc struct to store finished walks information (for isCluster()) diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index 59252f92dffad..707ce224ead94 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -241,101 +241,88 @@ bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::Point std::optional getXOffset(const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "x") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "x") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } std::optional getYOffset(const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "y") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "y") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } std::optional getZOffset(const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "z") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "z") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } -std::optional getIntensityOffset(const std::vector & fields) +std::optional getIntensityOffset( + const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "intensity") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "intensity") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } std::optional getRingOffset(const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "ring") == 0 - && field.datatype == sensor_msgs::msg::PointField::UINT16 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "ring") == 0 && + field.datatype == sensor_msgs::msg::PointField::UINT16 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } -std::optional getAzimuthOffset(const std::vector & fields) +std::optional getAzimuthOffset( + const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "azimuth") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "azimuth") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } -std::optional getDistanceOffset(const std::vector & fields) +std::optional getDistanceOffset( + const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "distance") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT32 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "distance") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } -std::optional getReturnTypeOffset(const std::vector & fields) +std::optional getReturnTypeOffset( + const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "return_type") == 0 - && field.datatype == sensor_msgs::msg::PointField::UINT8 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "return_type") == 0 && + field.datatype == sensor_msgs::msg::PointField::UINT8 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } -std::optional getTimeStampOffset(const std::vector & fields) +std::optional getTimeStampOffset( + const std::vector & fields) { - auto it = std::find_if(fields.begin(), fields.end(), - [&](const auto& field) { - return std::strcmp(field.name.c_str(), "time_stamp") == 0 - && field.datatype == sensor_msgs::msg::PointField::FLOAT64 - && field.count == 1; - }); - return (it != fields.end())? std::optional{it->offset} : std::nullopt; + auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { + return std::strcmp(field.name.c_str(), "time_stamp") == 0 && + field.datatype == sensor_msgs::msg::PointField::FLOAT64 && field.count == 1; + }); + return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; } } // namespace pointcloud_preprocessor::utils From 6858b4e18e5539b5c9374a5dec4ff0fa6364f2e4 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 1 Jun 2023 22:08:46 +0900 Subject: [PATCH 3/5] resize vector to data size Signed-off-by: Vincent Richard --- .../src/outlier_filter/ring_outlier_filter_nodelet.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index cd9e964a25f35..f2f36de726a06 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -327,6 +327,7 @@ void RingOutlierFilterComponent::faster_filter( output_size += sizeof(PointXYZI); } } + output.data.resize(output_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform // == true` From b7e54f6f243189220420dcf4049432d9aac49806 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 1 Jun 2023 22:09:28 +0900 Subject: [PATCH 4/5] cleaning Signed-off-by: Vincent Richard --- .../src/outlier_filter/ring_outlier_filter_nodelet.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index f2f36de726a06..739e5ac8a2656 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -335,7 +335,6 @@ void RingOutlierFilterComponent::faster_filter( output.height = 1; output.width = static_cast(output.data.size() / output.height / output.point_step); - output.row_step = static_cast(output.data.size() / output.height); output.is_bigendian = input->is_bigendian; output.is_dense = input->is_dense; From 8d8117fa7c3a36b2bd2eedd44dd1ebb29f91d0d8 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Mon, 5 Jun 2023 10:13:21 +0900 Subject: [PATCH 5/5] cleaner utilities impl Signed-off-by: Vincent Richard --- .../utility/utilities.hpp | 23 ++-- .../ring_outlier_filter_nodelet.cpp | 8 +- .../src/utility/utilities.cpp | 113 +++++++++--------- 3 files changed, 67 insertions(+), 77 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 3ce94528c5064..1265f958de9e1 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -88,36 +88,31 @@ bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); /** \brief Return offset of X field (if any). The field should match PointXYZIRADRT one */ -std::optional getXOffset(const std::vector & fields); +std::optional getXOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of Y field (if any). The field should match PointXYZIRADRT one */ -std::optional getYOffset(const std::vector & fields); +std::optional getYOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of Z field (if any). The field should match PointXYZIRADRT one */ -std::optional getZOffset(const std::vector & fields); +std::optional getZOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of intensity field (if any). The field should match PointXYZIRADRT one */ -std::optional getIntensityOffset( - const std::vector & fields); +std::optional getIntensityOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of ring field (if any). The field should match PointXYZIRADRT one */ -std::optional getRingOffset(const std::vector & fields); +std::optional getRingOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of azimuth field (if any). The field should match PointXYZIRADRT one */ -std::optional getAzimuthOffset( - const std::vector & fields); +std::optional getAzimuthOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of distance field (if any). The field should match PointXYZIRADRT one */ -std::optional getDistanceOffset( - const std::vector & fields); +std::optional getDistanceOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of return type field (if any). The field should match PointXYZIRADRT one */ -std::optional getReturnTypeOffset( - const std::vector & fields); +std::optional getReturnTypeOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); /** \brief Return offset of time stamp field (if any). The field should match PointXYZIRADRT one */ -std::optional getTimeStampOffset( - const std::vector & fields); +std::optional getTimeStampOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg); } // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 739e5ac8a2656..d951d42196eaf 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -71,28 +71,28 @@ void RingOutlierFilterComponent::faster_filter( // FIXME(VRichardJP) Everything would be simpler if Autoware enforced strict PointCloud2 format - auto maybe_intensity_offset = utils::getIntensityOffset(input->fields); + auto maybe_intensity_offset = utils::getIntensityOffset(*input); if (!maybe_intensity_offset) { RCLCPP_ERROR(get_logger(), "input cloud does not contain 'intensity' data field"); return; } const auto intensity_offset = *maybe_intensity_offset; - auto maybe_ring_offset = utils::getRingOffset(input->fields); + auto maybe_ring_offset = utils::getRingOffset(*input); if (!maybe_ring_offset) { RCLCPP_ERROR(get_logger(), "input cloud does not contain 'ring' data field"); return; } const auto ring_offset = *maybe_ring_offset; - auto maybe_azimuth_offset = utils::getAzimuthOffset(input->fields); + auto maybe_azimuth_offset = utils::getAzimuthOffset(*input); if (!maybe_azimuth_offset) { RCLCPP_ERROR(get_logger(), "input cloud does not contain 'azimuth' data field"); return; } const auto azimuth_offset = *maybe_azimuth_offset; - auto maybe_distance_offset = utils::getDistanceOffset(input->fields); + auto maybe_distance_offset = utils::getDistanceOffset(*input); if (!maybe_distance_offset) { RCLCPP_ERROR(get_logger(), "input cloud does not contain 'distance' data field"); return; diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index 707ce224ead94..544aeecb11d47 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -239,90 +239,85 @@ bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::Point return same_layout; } -std::optional getXOffset(const std::vector & fields) +std::optional getXOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "x") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "x"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getYOffset(const std::vector & fields) +std::optional getYOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "y") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "y"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getZOffset(const std::vector & fields) +std::optional getZOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "z") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "z"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getIntensityOffset( - const std::vector & fields) +std::optional getIntensityOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "intensity") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "intensity"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getRingOffset(const std::vector & fields) +std::optional getRingOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "ring") == 0 && - field.datatype == sensor_msgs::msg::PointField::UINT16 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "ring"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::UINT16) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getAzimuthOffset( - const std::vector & fields) +std::optional getAzimuthOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "azimuth") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "azimuth"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getDistanceOffset( - const std::vector & fields) +std::optional getDistanceOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "distance") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT32 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "distance"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getReturnTypeOffset( - const std::vector & fields) +std::optional getReturnTypeOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "return_type") == 0 && - field.datatype == sensor_msgs::msg::PointField::UINT8 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "return_type"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::UINT8) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } -std::optional getTimeStampOffset( - const std::vector & fields) +std::optional getTimeStampOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg) { - auto it = std::find_if(fields.begin(), fields.end(), [&](const auto & field) { - return std::strcmp(field.name.c_str(), "time_stamp") == 0 && - field.datatype == sensor_msgs::msg::PointField::FLOAT64 && field.count == 1; - }); - return (it != fields.end()) ? std::optional{it->offset} : std::nullopt; + int idx = pcl::getFieldIndex(cloud_msg, "time_stamp"); + if (idx < 0 || cloud_msg.fields[idx].datatype != sensor_msgs::msg::PointField::FLOAT64) { + return std::nullopt; + } + return std::optional{static_cast(idx)}; } } // namespace pointcloud_preprocessor::utils