feat: migrating pointcloud types (autowarefoundation#6996)

* feat: changed most of sensing to the new type

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: started applying changes to the perception stack

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: confirmed operation until centerpoint

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: reverted to the original implementation of pointpainting

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: forgot to push a header

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: also implemented the changes for the subsample filters that were out of scope before

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* fix: some point type changes were missing from the latest merge from main

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: removed unused code, added comments, and brought back a removed publish

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: added memory layout checks

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: updated documentation regarding the point types

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: added hyperlinks to the point definitions. will be valid only once the PR is merged

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* fix: fixed compilation due to moving the utilities files to the base library

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: separated the utilities functions due to a dependency issue

* chore: forgot that perception also uses the filter class

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feature: adapted the undistortion tests to the new point type

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>
Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>

common/autoware_point_types/include/autoware_point_types/types.hpp

@@ -54,6 +54,23 @@ enum ReturnType : uint8_t {
   DUAL_ONLY,
 };
 
+struct PointXYZIRC
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+
+  friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel;
+  }
+};
+
 struct PointXYZIRADRT
 {
   float x{0.0F};
@@ -75,25 +92,97 @@ struct PointXYZIRADRT
   }
 };
 
-enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp };
+struct PointXYZIRCAEDT
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+  float azimuth{0.0F};
+  float elevation{0.0F};
+  float distance{0.0F};
+  std::uint32_t time_stamp{0U};
+
+  friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel &&
+           float_eq<float>(p1.azimuth, p2.azimuth) && float_eq<float>(p1.distance, p2.distance) &&
+           p1.time_stamp == p2.time_stamp;
+  }
+};
+
+enum class PointXYZIIndex { X, Y, Z, Intensity };
+enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel };
+enum class PointXYZIRADRTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  Ring,
+  Azimuth,
+  Distance,
+  ReturnType,
+  TimeStamp
+};
+enum class PointXYZIRCAEDTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  ReturnType,
+  Channel,
+  Azimuth,
+  Elevation,
+  Distance,
+  TimeStamp
+};
 
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth);
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp);
 
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel);
+
+using PointXYZIRCGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator>;
+
 using PointXYZIRADRTGenerator = std::tuple<
   point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
   point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
   point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator,
   field_return_type_generator, field_time_stamp_generator>;
 
+using PointXYZIRCAEDTGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator, field_azimuth_generator,
+  field_elevation_generator, field_distance_generator, field_time_stamp_generator>;
+
 }  // namespace autoware_point_types
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRC,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel))
+
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   autoware_point_types::PointXYZIRADRT,
   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
     float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
     double, time_stamp, time_stamp))
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRCAEDT,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type,
+    return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
+    float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))
 #endif  // AUTOWARE_POINT_TYPES__TYPES_HPP_

common/autoware_point_types/test/test_point_types.cpp

@@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT)
   EXPECT_EQ(pt0, pt1);
 }
 
+TEST(PointEquality, PointXYZIRCAEDT)
+{
+  using autoware_point_types::PointXYZIRCAEDT;
+
+  PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  EXPECT_EQ(pt0, pt1);
+}
+
 TEST(PointEquality, FloatEq)
 {
   // test template

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml

@@ -11,7 +11,7 @@
   <arg name="pedestrian_traffic_light_classifier_label_name" default="lamp_labels_ped.txt" description="classifier label filename"/>
   <arg name="car_traffic_light_classifier_model_name" default="traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model filename"/>
   <arg name="pedestrian_traffic_light_classifier_model_name" default="ped_traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model filename"/>
-  <arg name="input/cloud" default="/sensing/lidar/top/pointcloud_raw" description="point cloud for occlusion prediction"/>
+  <arg name="input/cloud" default="/sensing/lidar/top/pointcloud_raw_ex" description="point cloud for occlusion prediction"/>
   <arg name="judged/traffic_signals" default="/perception/traffic_light_recognition/judged/traffic_signals"/>
   <arg name="internal/traffic_signals" default="/perception/traffic_light_recognition/internal/traffic_signals"/>
   <arg name="external/traffic_signals" default="/perception/traffic_light_recognition/external/traffic_signals"/>

perception/ground_segmentation/src/scan_ground_filter/node.cpp

@@ -96,8 +96,9 @@ inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstP
   int intensity_index = pcl::getFieldIndex(*input, "intensity");
   if (intensity_index != -1) {
     intensity_offset_ = input->fields[intensity_index].offset;
+    intensity_type_ = input->fields[intensity_index].datatype;
   } else {
-    intensity_offset_ = z_offset_ + sizeof(float);
+    intensity_offset_ = -1;
   }
   offset_initialized_ = true;
 }
@@ -569,6 +570,7 @@ void ScanGroundFilterComponent::extractObjectPoints(
   PointCloud2 & out_object_cloud)
 {
   size_t output_data_size = 0;
+
   for (const auto & i : in_indices.indices) {
     std::memcpy(
       &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step],

perception/ground_segmentation/src/scan_ground_filter/node.hpp

@@ -161,6 +161,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   int y_offset_;
   int z_offset_;
   int intensity_offset_;
+  int intensity_type_;
   bool offset_initialized_;
 
   void set_field_offsets(const PointCloud2ConstPtr & input);

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -289,13 +289,13 @@ void PointPaintingFusionNode::fuseOnSingleImage(
   // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped);
 
   const auto x_offset =
-    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::X))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::X))
       .offset;
   const auto y_offset =
-    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Y))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::Y))
       .offset;
   const auto z_offset =
-    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Z))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointXYZIRCIndex::Z))
       .offset;
   const auto class_offset = painted_pointcloud_msg.fields.at(4).offset;
   const auto p_step = painted_pointcloud_msg.point_step;

perception/lidar_apollo_instance_segmentation/src/detector.cpp

@@ -16,6 +16,8 @@
 
 #include "lidar_apollo_instance_segmentation/feature_map.hpp"
 
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
 #include <NvCaffeParser.h>
 #include <NvInfer.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -126,7 +128,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects(
 
   // convert from ros to pcl
   pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud<pcl::PointXYZI>);
-  pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr);
+  // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr);
+
+  auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr;
+  pcl_pointcloud_raw.width = transformed_cloud.width;
+  pcl_pointcloud_raw.height = transformed_cloud.height;
+  pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1;
+  pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height);
+
+  sensor_msgs::PointCloud2ConstIterator<float> it_x(transformed_cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> it_y(transformed_cloud, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> it_z(transformed_cloud, "z");
+  sensor_msgs::PointCloud2ConstIterator<uint8_t> it_intensity(transformed_cloud, "intensity");
+
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) {
+    pcl::PointXYZI point;
+    point.x = *it_x;
+    point.y = *it_y;
+    point.z = *it_z;
+    point.intensity = static_cast<float>(*it_intensity);
+    pcl_pointcloud_raw.emplace_back(std::move(point));
+  }
 
   // generate feature map
   std::shared_ptr<FeatureMapInterface> feature_map_ptr =

perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp

@@ -18,6 +18,8 @@
 #include <lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp>
 #include <tvm_utility/pipeline.hpp>
 
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
 #include <memory>
 #include <string>
 #include <vector>
@@ -168,7 +170,29 @@ std::shared_ptr<const DetectedObjectsWithFeature> ApolloLidarSegmentation::detec
   sensor_msgs::msg::PointCloud2 transformed_cloud;
   ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_);
   // convert from ros to pcl
-  pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_ptr_);
+  // pcl::fromROSMsg(
+  //  transformed_cloud, *pcl_pointcloud_ptr_);  // Manual conversion is needed since intensity
+  //  comes as an uint8_t
+
+  auto pcl_pointcloud = *pcl_pointcloud_ptr_;
+  pcl_pointcloud.width = input.width;
+  pcl_pointcloud.height = input.height;
+  pcl_pointcloud.is_dense = input.is_dense == 1;
+  pcl_pointcloud.resize(input.width * input.height);
+
+  sensor_msgs::PointCloud2ConstIterator<float> it_x(input, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> it_y(input, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> it_z(input, "z");
+  sensor_msgs::PointCloud2ConstIterator<uint8_t> it_intensity(input, "intensity");
+
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) {
+    pcl::PointXYZI point;
+    point.x = *it_x;
+    point.y = *it_y;
+    point.z = *it_z;
+    point.intensity = static_cast<float>(*it_intensity);
+    pcl_pointcloud.emplace_back(std::move(point));
+  }
 
   // inference pipeline
   auto output = pipeline->schedule(pcl_pointcloud_ptr_);

perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp

@@ -125,9 +125,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point
     RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16.");
     return true;
   }
-  for (sensor_msgs::PointCloud2ConstIterator<float> iter(cluster, "intensity"); iter != iter.end();
-       ++iter) {
-    mean_intensity += *iter;
+  for (sensor_msgs::PointCloud2ConstIterator<uint8_t> iter(cluster, "intensity");
+       iter != iter.end(); ++iter) {
+    mean_intensity += static_cast<float>(*iter);
   }
   const size_t num_points = cluster.width * cluster.height;
   mean_intensity /= static_cast<double>(num_points);

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -26,11 +26,13 @@ include_directories(
 
 add_library(pointcloud_preprocessor_filter_base SHARED
   src/filter.cpp
+  src/utility/memory.cpp
 )
 
 target_include_directories(pointcloud_preprocessor_filter_base PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+  ${autoware_point_types_INCLUDE_DIRS}
 )
 
 ament_target_dependencies(pointcloud_preprocessor_filter_base
@@ -41,6 +43,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base
   tf2_ros
   autoware_universe_utils
   pcl_ros
+  autoware_point_types
 )
 
 add_library(faster_voxel_grid_downsample_filter SHARED
@@ -59,7 +62,6 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
 )
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
-  src/utility/utilities.cpp
   src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
   src/concatenate_data/concatenate_pointclouds.cpp
   src/time_synchronizer/time_synchronizer_nodelet.cpp
@@ -82,6 +84,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/blockage_diag/blockage_diag_nodelet.cpp
   src/polygon_remover/polygon_remover.cpp
   src/vector_map_filter/vector_map_inside_area_filter.cpp
+  src/utility/geometry.cpp
 )
 
 target_link_libraries(pointcloud_preprocessor_filter
@@ -227,16 +230,19 @@ ament_auto_package(INSTALL_TO_SHARE
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_gtest)
+
   ament_lint_auto_find_test_dependencies()
+
   ament_add_gtest(test_utilities
     test/test_utilities.cpp
   )
-  ament_add_gtest(distortion_corrector_node_tests
-  test/test_distortion_corrector_node.cpp
+
+  ament_add_gtest(test_distortion_corrector_node
+    test/test_distortion_corrector_node.cpp
   )
 
   target_link_libraries(test_utilities pointcloud_preprocessor_filter)
-  target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter)
+  target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter)
 
 
 endif()

sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md

@@ -10,7 +10,7 @@ This node can remove rain and fog by considering the light reflected from the ob
 
 ![outlier_filter-return_type](./image/outlier_filter-return_type.drawio.svg)
 
-Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADRT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L57-L76) data structure.
+Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) data structure.
 
 Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle.
 
@@ -75,7 +75,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 ## Assumptions / Known limits
 
 Not recommended for use as it is under development.
-Input data must be `PointXYZIRADRT` type data including `return_type`.
+Input data must be [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) type data including `return_type`.
 
 ## (Optional) Error detection and handling
 

sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md

@@ -75,16 +75,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 
 ## Assumptions / Known limits
 
-It is a prerequisite to input a scan point cloud in chronological order. In this repository it is defined as blow structure (please refer to [PointXYZIRADT](https://github.com/tier4/AutowareArchitectureProposal.iv/blob/5d8dff0db51634f0c42d2a3e87ca423fbee84348/sensing/preprocessor/pointcloud/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp#L53-L62)).
-
-- X: x
-- Y: y
-- z: z
-- I: intensity
-- R: ring
-- A :azimuth
-- D: distance
-- T: time_stamp
+This nodes requires that the points of the input point cloud are in chronological order and that individual points follow the memory layout specified by [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116).
 
 ## (Optional) Error detection and handling
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -87,7 +87,7 @@
 
 namespace pointcloud_preprocessor
 {
-using autoware_point_types::PointXYZI;
+using autoware_point_types::PointXYZIRC;
 using point_cloud_msg_wrapper::PointCloud2Modifier;
 
 /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data
@@ -169,7 +169,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
     sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr);
   void publish();
 
-  void convertToXYZICloud(
+  void convertToXYZIRCCloud(
     const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
     sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr);
   void setPeriod(const int64_t new_period);