feat(pointcloud_preprocessor): use point_cloud_msg_wrapper (autowaref…

…oundation#1276)

Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

common/autoware_auto_common/include/common/types.hpp

@@ -109,6 +109,46 @@ 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<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.z, p2.z, std::numeric_limits<float32_t>::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<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.z, p2.z, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.time_stamp, p2.time_stamp, std::numeric_limits<float32_t>::epsilon());
+  }
+};
+
 using PointBlock = std::vector<PointXYZIF>;
 using PointPtrBlock = std::vector<const PointXYZIF *>;
 /// \brief Stores basic configuration information, does some simple validity checking

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp

@@ -72,7 +72,6 @@
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <tier4_debug_msgs/msg/int32_stamped.hpp>
 #include <tier4_debug_msgs/msg/string_stamped.hpp>
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -21,7 +21,6 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp

@@ -19,7 +19,6 @@
 #include "pointcloud_preprocessor/utility/utilities.hpp"
 
 #include <geometry_msgs/msg/polygon_stamped.hpp>
-#include <sensor_msgs/point_cloud2_iterator.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp

@@ -15,9 +15,11 @@
 #ifndef POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_
 #define POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_
 
+#include <common/types.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
 #include <geometry_msgs/msg/polygon.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 #include <CGAL/Polygon_2_algorithms.h>

sensing/pointcloud_preprocessor/package.xml

@@ -18,6 +18,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_auto_common</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>autoware_point_types</depend>
   <depend>cgal</depend>

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp

@@ -51,7 +51,9 @@
 
 #include "pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp"
 
+#include <common/types.hpp>
 #include <pcl_ros/transforms.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -326,36 +328,19 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud(
   const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
   sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr)
 {
-  output_ptr->header = input_ptr->header;
-  PointCloud2Modifier<PointXYZI> 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<float> it_x(*input_ptr, "x");
-  sensor_msgs::PointCloud2Iterator<float> it_y(*input_ptr, "y");
-  sensor_msgs::PointCloud2Iterator<float> it_z(*input_ptr, "z");
-
-  if (has_intensity) {
-    sensor_msgs::PointCloud2Iterator<float> 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));
+  point_cloud_msg_wrapper::PointCloud2Modifier<autoware::common::types::PointXYZI> 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<autoware::common::types::PointXYZI> view{*input_ptr};
+    for (const auto & point : view) {
+      output_modifier.push_back({point.x, point.y, point.z, point.intensity});
     }
   } else {
-    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));
+    point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{*input_ptr};
+    for (const auto & point : view) {
+      output_modifier.push_back({point.x, point.y, point.z, 0.0f});
     }
   }
 }

sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp

@@ -51,8 +51,6 @@
 
 #include "pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp"
 
-#include <sensor_msgs/point_cloud2_iterator.hpp>
-
 #include <vector>
 
 namespace pointcloud_preprocessor

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -14,6 +14,9 @@
 
 #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
+#include <common/types.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
 #include <deque>
 #include <string>
 #include <utility>
@@ -175,28 +178,22 @@ bool DistortionCorrectorComponent::undistortPointCloud(
     return false;
   }
 
-  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()) {
+  if (!point_cloud_msg_wrapper::PointCloud2View<
+        autoware::common::types::PointXYZTimestamp>::can_be_created_from(points)) {
     RCLCPP_WARN_STREAM_THROTTLE(
       get_logger(), *get_clock(), 10000 /* ms */,
       "Required field time stamp doesn't exist in the point cloud.");
     return false;
   }
 
-  sensor_msgs::PointCloud2Iterator<float> it_x(points, "x");
-  sensor_msgs::PointCloud2Iterator<float> it_y(points, "y");
-  sensor_msgs::PointCloud2Iterator<float> it_z(points, "z");
-  sensor_msgs::PointCloud2ConstIterator<double> it_time_stamp(points, time_stamp_field_name_);
+  point_cloud_msg_wrapper::PointCloud2Modifier<autoware::common::types::PointXYZTimestamp> modifier{
+    points};
 
   float theta{0.0f};
   float x{0.0f};
   float y{0.0f};
-  double prev_time_stamp_sec{*it_time_stamp};
-  const double first_point_time_stamp_sec{*it_time_stamp};
+  double prev_time_stamp_sec{modifier.begin()->time_stamp};
+  const double first_point_time_stamp_sec{modifier.begin()->time_stamp};
 
   auto twist_it = std::lower_bound(
     std::begin(twist_queue_), std::end(twist_queue_), first_point_time_stamp_sec,
@@ -217,17 +214,17 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   }
 
   const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()};
-  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
+  for (auto & point : modifier) {
     for (;
          (twist_it != std::end(twist_queue_) - 1 &&
-          *it_time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
+          point.time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
          ++twist_it) {
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
-    if (std::abs(*it_time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) {
+    if (std::abs(point.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.");
@@ -238,10 +235,10 @@ bool DistortionCorrectorComponent::undistortPointCloud(
     if (use_imu_ && !angular_velocity_queue_.empty()) {
       for (;
            (imu_it != std::end(angular_velocity_queue_) - 1 &&
-            *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
+            point.time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
            ++imu_it) {
       }
-      if (std::abs(*it_time_stamp - rclcpp::Time(imu_it->header.stamp).seconds()) > 0.1) {
+      if (std::abs(point.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.");
@@ -250,9 +247,9 @@ bool DistortionCorrectorComponent::undistortPointCloud(
       }
     }
 
-    const float time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
+    const float time_offset = static_cast<float>(point.time_stamp - prev_time_stamp_sec);
 
-    const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z};
+    const tf2::Vector3 sensorTF_point{point.x, point.y, point.z};
 
     const tf2::Vector3 base_linkTF_point{tf2_base_link_to_sensor_inv * sensorTF_point};
 
@@ -271,11 +268,11 @@ bool DistortionCorrectorComponent::undistortPointCloud(
 
     const tf2::Vector3 sensorTF_trans_point{tf2_base_link_to_sensor * base_linkTF_trans_point};
 
-    *it_x = sensorTF_trans_point.getX();
-    *it_y = sensorTF_trans_point.getY();
-    *it_z = sensorTF_trans_point.getZ();
+    point.x = sensorTF_trans_point.getX();
+    point.y = sensorTF_trans_point.getY();
+    point.z = sensorTF_trans_point.getZ();
 
-    prev_time_stamp_sec = *it_time_stamp;
+    prev_time_stamp_sec = point.time_stamp;
   }
   return true;
 }

sensing/pointcloud_preprocessor/src/utility/utilities.cpp

@@ -48,18 +48,17 @@ void remove_polygon_cgal_from_cloud(
 {
   pcl::PointCloud<pcl::PointXYZ> pcl_output;
 
-  for (sensor_msgs::PointCloud2ConstIterator<float> 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) {
+  point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{cloud_in};
+  for (const auto & point : view) {
     // check if the point is inside the polygon
     if (
       CGAL::bounded_side_2(
-        polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) ==
+        polyline_polygon.begin(), polyline_polygon.end(), PointCgal(point.x, point.y), K()) ==
       CGAL::ON_UNBOUNDED_SIDE) {
       pcl::PointXYZ p;
-      p.x = *iter_x;
-      p.y = *iter_y;
-      p.z = *iter_z;
+      p.x = point.x;
+      p.y = point.y;
+      p.z = point.z;
       pcl_output.emplace_back(p);
     }
   }
@@ -96,11 +95,10 @@ void remove_polygon_cgal_from_cloud(
   }
 
   pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
-  for (sensor_msgs::PointCloud2ConstIterator<float> 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) {
+  point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{cloud_in};
+  for (const auto & point : view) {
     // if the point is inside the polygon, skip inserting and check the next point
-    pcl::PointXYZ p(*iter_x, *iter_y, *iter_z);
+    pcl::PointXYZ p(point.x, point.y, point.z);
     if (point_within_cgal_polys(p, polyline_polygons)) {
       continue;
     }