revert: "feat(pointcloud_preprocessor): use point_cloud_msg_wrapper" (a…

…utowarefoundation#2317)

Revert "feat(pointcloud_preprocessor): use point_cloud_msg_wrapper (autowarefoundation#1276)"

This reverts commit ef7dcda.

Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

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<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,6 +72,7 @@
 #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,6 +21,7 @@
 #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,6 +19,7 @@
 #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,11 +15,9 @@
 #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,7 +18,6 @@
 
   <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,9 +51,7 @@
 
 #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>
 
@@ -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<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});
+  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));
     }
   } else {
-    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});
+    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));
     }
   }
 }

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 <sensor_msgs/point_cloud2_iterator.hpp>
+
 #include <vector>
 
 namespace pointcloud_preprocessor

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -14,9 +14,6 @@
 
 #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>
@@ -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<autoware::common::types::PointXYZTimestamp> modifier{
-    points};
+  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_);
 
   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<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(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<float>(point.time_stamp - prev_time_stamp_sec);
+    const float time_offset = static_cast<float>(*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;
 }

sensing/pointcloud_preprocessor/src/utility/utilities.cpp

@@ -48,17 +48,18 @@ void remove_polygon_cgal_from_cloud(
 {
   pcl::PointCloud<pcl::PointXYZ> pcl_output;
 
-  point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{cloud_in};
-  for (const auto & point : view) {
+  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) {
     // 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<pcl::PointXYZ> filtered_cloud;
-  point_cloud_msg_wrapper::PointCloud2View<autoware::common::types::PointXYZ> view{cloud_in};
-  for (const auto & point : view) {
+  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) {
     // 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;
     }