feat: cherry pick point types update

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

@@ -123,9 +123,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/livox/livox_tag_filter/src/livox_tag_filter_node/livox_tag_filter_node.cpp

@@ -47,9 +47,13 @@ LivoxTagFilterNode::LivoxTagFilterNode(const rclcpp::NodeOptions & node_options)
   sub_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "input", rclcpp::SensorDataQoS(), std::bind(&LivoxTagFilterNode::onPointCloud, this, _1));
 
-  // Publisher
-  pub_pointcloud_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("output", rclcpp::SensorDataQoS());
+  {
+    // Publisher
+    rclcpp::PublisherOptions pub_options;
+    pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+    pub_pointcloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "output", rclcpp::SensorDataQoS(), pub_options);
+  }
 }
 
 void LivoxTagFilterNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -9,6 +9,7 @@ set(CGAL_DATA_DIR ".")
 
 find_package(OpenCV REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED)
 find_package(PCL REQUIRED)
 find_package(CGAL REQUIRED COMPONENTS Core)
@@ -19,16 +20,19 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
 )
 
 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
@@ -39,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
@@ -57,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
@@ -76,16 +80,19 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp
   src/vector_map_filter/lanelet2_map_filter_nodelet.cpp
   src/distortion_corrector/distortion_corrector.cpp
+  src/distortion_corrector/distortion_corrector_node.cpp
   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
   pointcloud_preprocessor_filter_base
   faster_voxel_grid_downsample_filter
   ${Boost_LIBRARIES}
   ${OpenCV_LIBRARIES}
+  ${Sophus_LIBRARIES}
   ${PCL_LIBRARIES}
 )
 
@@ -223,18 +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
   )
-  target_link_libraries(test_utilities pointcloud_preprocessor_filter)
 
-  add_ros_test(
-    test/test_distortion_corrector.py
-    TIMEOUT "30"
-  )
-  add_ros_test(
-    test/test_distortion_corrector_use_imu_false.py
-    TIMEOUT "30"
+  ament_add_gtest(test_distortion_corrector_node
+    test/test_distortion_corrector_node.cpp
   )
+
+  target_link_libraries(test_utilities pointcloud_preprocessor_filter)
+  target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter)
+
+
 endif()

sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    base_frame: base_link
+    use_imu: true
+    use_3d_distortion_correction: false