feat: migrating pointcloud types

common/autoware_point_types/include/autoware_point_types/types.hpp

@@ -54,6 +54,23 @@
   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 @@
   }
 };
 
-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

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -162,6 +162,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/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -96,8 +96,9 @@
   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 @@
   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/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -296,13 +296,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 @@
 
   // 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>
@@ -173,7 +175,29 @@
   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.cpp

@@ -123,9 +123,9 @@
     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/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);

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp

@@ -86,7 +86,7 @@
 
 namespace pointcloud_preprocessor
 {
-using autoware_point_types::PointXYZI;
+using autoware_point_types::PointXYZIRC;
 using point_cloud_msg_wrapper::PointCloud2Modifier;
 /** \brief @b PointCloudConcatenationComponent is a special form of data
  * synchronizer: it listens for a set of input PointCloud messages on the same topic,
@@ -160,7 +160,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node
   void combineClouds(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);

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp

@@ -34,7 +34,7 @@
 public:
   FasterVoxelGridDownsampleFilter();
   void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z);
-  void set_field_offsets(const PointCloud2ConstPtr & input);
+  void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger);
   void filter(
     const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info,
     const rclcpp::Logger & logger);
@@ -48,7 +48,7 @@
     float intensity;
     uint32_t point_count_;
 
-    Centroid() : x(0), y(0), z(0), intensity(-1.0) {}
+    Centroid() : x(0), y(0), z(0), intensity(0) {}
     Centroid(float _x, float _y, float _z, float _intensity)
     : x(_x), y(_y), z(_z), intensity(_intensity)
     {

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp

@@ -35,12 +35,15 @@
 
 namespace pointcloud_preprocessor
 {
-using autoware_point_types::PointXYZI;
 using point_cloud_msg_wrapper::PointCloud2Modifier;
 
 class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
+  using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex;
+  using InputPointType = autoware_point_types::PointXYZIRCAEDT;
+  using OutputPointType = autoware_point_types::PointXYZIRC;
+
   virtual void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
 
@@ -83,8 +86,10 @@
   {
     if (walk_size > num_points_threshold_) return true;
 
-    auto first_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.first]);
-    auto last_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.second]);
+    auto first_point =
+      reinterpret_cast<const InputPointType *>(&input->data[data_idx_both_ends.first]);
+    auto last_point =
+      reinterpret_cast<const InputPointType *>(&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;
@@ -94,8 +99,7 @@
   }
 
   void setUpPointCloudFormat(
-    const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
-    size_t num_fields);
+    const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size);
   float calculateVisibilityScore(const PointCloud2 & input);
 
 public:

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_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;
 // cspell:ignore Yoshi
 /** \brief @b PointCloudDataSynchronizerComponent is a special form of data
@@ -166,7 +166,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node
   std::map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> synchronizeClouds();
   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);

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

@@ -89,6 +89,10 @@ 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 whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That
+ * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */
+bool is_data_layout_compatible_with_PointXYZIRCAEDT(const sensor_msgs::msg::PointCloud2 & input);
+
 }  // namespace pointcloud_preprocessor::utils
 
 #endif  // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_