fix: make it works (see below)

  * Add missing parts:
    - Some codes are copied from following `autoware_pointcloud_preprocessor` package
      - https://github.com/autowarefoundation/autoware_universe/tree/main/sensing/autoware_pointcloud_preprocessor
  * But the code has not been tidy up yet at this time:
    - We'll provide the refactoring plan for this

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Author: sasakisasaki

pointcloud_filter/CMakeLists.txt

@@ -24,6 +24,7 @@ include_directories(
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/scan_ground_filter/node.cpp
   src/scan_ground_filter/grid_ground_filter.cpp
+  src/scan_ground_filter/sanity_check.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

pointcloud_filter/package.xml

@@ -22,12 +22,14 @@
   <depend>ament_index_cpp</depend>
   <depend>autoware_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
+  <depend>autoware_point_types</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>message_filters</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>

pointcloud_filter/src/scan_ground_filter/node.cpp

@@ -16,6 +16,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "grid_ground_filter.hpp"
+#include "sanity_check.hpp"
 
 #include <autoware_utils/geometry/geometry.hpp>
 #include <autoware_utils/math/normalization.hpp>
@@ -26,19 +27,6 @@
 #include <string>
 #include <vector>
 
-namespace autoware::pointcloud_filter
-{
-// For PointCloud2
-using PointCloud2 = sensor_msgs::msg::PointCloud2;
-using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
-
-using autoware::vehicle_info_utils::VehicleInfoUtils;
-using autoware_utils::calc_distance3d;
-using autoware_utils::deg2rad;
-using autoware_utils::normalize_degree;
-using autoware_utils::normalize_radian;
-using autoware_utils::ScopedTimeTrack;
-
 namespace {
 /** \brief For parameter service callback */
 template <typename T>
@@ -55,6 +43,19 @@ bool get_param(const std::vector<rclcpp::Parameter> & p, const std::string & nam
 }
 }
 
+namespace autoware::pointcloud_filter
+{
+// For PointCloud2
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
+
+using autoware::vehicle_info_utils::VehicleInfoUtils;
+using autoware_utils::calc_distance3d;
+using autoware_utils::deg2rad;
+using autoware_utils::normalize_degree;
+using autoware_utils::normalize_radian;
+using autoware_utils::ScopedTimeTrack;
+
 ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options)
 : rclcpp::Node("ScanGroundFilter", options)
 {
@@ -147,6 +148,229 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
       grid_ground_filter_ptr_->setTimeKeeper(time_keeper_);
     }
   }
+
+  // pointcloud parameters
+  tf_input_frame_ = static_cast<std::string>(declare_parameter("input_frame", ""));
+  tf_output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
+  has_static_tf_only_ = static_cast<bool>(declare_parameter("has_static_tf_only", false));
+  max_queue_size_ = static_cast<std::size_t>(declare_parameter("max_queue_size", 5));
+  use_indices_ = static_cast<bool>(declare_parameter("use_indices", false));
+  latched_indices_ = static_cast<bool>(declare_parameter("latched_indices", false));
+  approximate_sync_ = static_cast<bool>(declare_parameter("approximate_sync", false));
+
+  RCLCPP_DEBUG_STREAM(
+    this->get_logger(),
+    "Filter (as Component) successfully created with the following parameters:"
+      << std::endl
+      << " - approximate_sync : " << (approximate_sync_ ? "true" : "false") << std::endl
+      << " - use_indices      : " << (use_indices_ ? "true" : "false") << std::endl
+      << " - latched_indices  : " << (latched_indices_ ? "true" : "false") << std::endl
+      << " - max_queue_size   : " << max_queue_size_);
+
+  // Set publisher
+  {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+    pub_output_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "output", rclcpp::SensorDataQoS().keep_last(max_queue_size_), pub_options);
+  }
+
+  subscribe();
+
+  // Set tf_listener, tf_buffer.
+  setupTF();
+
+  published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
+  RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created.");
+
+}
+
+void ScanGroundFilterComponent::setupTF()
+{
+  // Always consider static TF if in & out frames are same
+  if (tf_input_frame_ == tf_output_frame_) {
+    if (!has_static_tf_only_) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "Input and output frames are the same. Overriding has_static_tf_only to true.");
+    }
+    has_static_tf_only_ = true;
+  }
+  managed_tf_buffer_ =
+    std::make_unique<autoware_utils::ManagedTransformBuffer>(this, has_static_tf_only_);
+}
+
+void ScanGroundFilterComponent::subscribe()
+{
+  if (use_indices_) {
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe(
+      this, "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile());
+    sub_indices_filter_.subscribe(
+      this, "indices", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile());
+
+    if (approximate_sync_) {
+      sync_input_indices_a_ = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, pcl_msgs::msg::PointIndices>>>(max_queue_size_);
+      sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_a_->registerCallback(
+        std::bind(&ScanGroundFilterComponent::faster_input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
+    } else {
+      sync_input_indices_e_ = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, pcl_msgs::msg::PointIndices>>>(max_queue_size_);
+      sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
+      sync_input_indices_e_->registerCallback(
+        std::bind(&ScanGroundFilterComponent::faster_input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
+    }
+  } else {
+    std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)> cb =
+      std::bind(&ScanGroundFilterComponent::faster_input_indices_callback, this, std::placeholders::_1, pcl_msgs::msg::PointIndices::ConstSharedPtr());
+    sub_input_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+      "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), cb);
+  }
+}
+
+bool ScanGroundFilterComponent::calculate_transform_matrix(
+  const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
+  TransformInfo & transform_info /*output*/)
+{
+  transform_info.need_transform = false;
+
+  if (target_frame.empty() || from.header.frame_id == target_frame) return true;
+
+  RCLCPP_DEBUG(
+    this->get_logger(), "[get_transform_matrix] Transforming input dataset from %s to %s.",
+    from.header.frame_id.c_str(), target_frame.c_str());
+
+  if (!managed_tf_buffer_->get_transform(
+        target_frame, from.header.frame_id, transform_info.eigen_transform)) {
+    return false;
+  }
+
+  transform_info.need_transform = true;
+  return true;
+}
+
+bool ScanGroundFilterComponent::convert_output_costly(
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> & output)
+{
+  // In terms of performance, we should avoid using pcl_ros library function,
+  // but this code path isn't reached in the main use case of Autoware, so it's left as is for now.
+  if (!tf_output_frame_.empty() && output->header.frame_id != tf_output_frame_) {
+    RCLCPP_DEBUG(
+      this->get_logger(), "[convert_output_costly] Transforming output dataset from %s to %s.",
+      output->header.frame_id.c_str(), tf_output_frame_.c_str());
+
+    // Convert the cloud into the different frame
+    auto cloud_transformed = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+    if (!managed_tf_buffer_->transform_pointcloud(tf_output_frame_, *output, *cloud_transformed)) {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "[convert_output_costly] Error converting output dataset from %s to %s.",
+        output->header.frame_id.c_str(), tf_output_frame_.c_str());
+      return false;
+    }
+
+    output = std::move(cloud_transformed);
+  }
+
+  // Same as the comment above
+  if (tf_output_frame_.empty() && output->header.frame_id != tf_input_orig_frame_) {
+    // No tf_output_frame given, transform the dataset to its original frame
+    RCLCPP_DEBUG(
+      this->get_logger(), "[convert_output_costly] Transforming output dataset from %s back to %s.",
+      output->header.frame_id.c_str(), tf_input_orig_frame_.c_str());
+
+    auto cloud_transformed = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+    if (!managed_tf_buffer_->transform_pointcloud(
+          tf_input_orig_frame_, *output, *cloud_transformed)) {
+      return false;
+    }
+
+    output = std::move(cloud_transformed);
+  }
+
+  return true;
+}
+
+void ScanGroundFilterComponent::faster_input_indices_callback(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const pcl_msgs::msg::PointIndices::ConstSharedPtr indices)
+{
+  if (
+    !is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
+    !is_data_layout_compatible_with_point_xyzirc(*cloud)) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
+
+    if (is_data_layout_compatible_with_point_xyziradrt(*cloud)) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy "
+        "code/data");
+    }
+
+    if (is_data_layout_compatible_with_point_xyzi(*cloud)) {
+      RCLCPP_ERROR(
+        get_logger(),
+        "The pointcloud layout is compatible with PointXYZI. You may be using legacy "
+        "code/data");
+    }
+
+    return;
+  }
+
+  if (!isValid(cloud)) {
+    RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!");
+    return;
+  }
+
+  if (!isValid(indices)) {
+    RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid indices!");
+    return;
+  }
+
+  if (indices) {
+    RCLCPP_DEBUG(
+      this->get_logger(),
+      "[input_indices_callback]\n"
+      "   - PointCloud with %d data points (%s), stamp %f, and frame %s on input topic received.\n"
+      "   - PointIndices with %zu values, stamp %f, and frame %s on indices topic received.",
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+      rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str(),
+      indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(),
+      indices->header.frame_id.c_str());
+  } else {
+    RCLCPP_DEBUG(
+      this->get_logger(),
+      "[input_indices_callback] PointCloud with %d data points and frame %s on input topic "
+      "received.",
+      cloud->width * cloud->height, cloud->header.frame_id.c_str());
+  }
+
+  tf_input_orig_frame_ = cloud->header.frame_id;
+
+  // For performance reason, defer the transform computation.
+  // Do not use pcl_ros::transformPointCloud(). It's too slow due to the unnecessary copy.
+  TransformInfo transform_info;
+  if (!calculate_transform_matrix(tf_input_frame_, *cloud, transform_info)) return;
+
+  // Need setInputCloud() here because we have to extract x/y/z
+  pcl::IndicesPtr vindices;
+  if (indices) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  auto output = std::make_unique<PointCloud2>();
+
+  // TODO(sykwer): Change to `filter()` call after when the filter nodes conform to new API.
+  faster_filter(cloud, vindices, *output, transform_info);
+
+  if (!convert_output_costly(output)) return;
+
+  output->header.stamp = cloud->header.stamp;
+  pub_output_->publish(std::move(output));
+  published_time_publisher_->publish_if_subscribed(pub_output_, cloud->header.stamp);
 }
 
 void ScanGroundFilterComponent::convertPointcloud(
@@ -356,8 +580,7 @@ void ScanGroundFilterComponent::faster_filter(
   std::unique_ptr<autoware_utils::ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<autoware_utils::ScopedTimeTrack>(__func__, *time_keeper_);
 
-  // TODO(sasakisasaki): Understand why this mutex is defined before the porting. I guess this comes from pointcloud_preprocessor.
-  //std::scoped_lock lock(mutex_);
+  std::scoped_lock lock(mutex_);
 
   if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true);
 
@@ -463,13 +686,24 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter(
       this->get_logger(),
       "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_);
   }
+
+  // For pointcloud
+  std::scoped_lock lock(mutex_);
+
+  if (get_param(param, "input_frame", tf_input_frame_)) {
+    RCLCPP_DEBUG(this->get_logger(), "Setting the input TF frame to: %s.", tf_input_frame_.c_str());
+  }
+  if (get_param(param, "output_frame", tf_output_frame_)) {
+    RCLCPP_DEBUG(this->get_logger(), "Setting the output TF frame to: %s.", tf_output_frame_.c_str());
+  }
+
+  // Finally return the result
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   result.reason = "success";
 
   return result;
 }
-
 }  // namespace autoware::pointcloud_filter
 
 #include <rclcpp_components/register_node_macro.hpp>