From e517f09cb16ba96513219cd7f389cc671a78c6dc Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 9 May 2024 08:07:01 +0900 Subject: [PATCH] feat: add low_intensity_cluster_filter (#6850) * feat: add low_intensity_cluster_filter Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: build test error Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../detector/camera_lidar_detector.launch.xml | 13 +- .../launch/perception.launch.xml | 1 + .../raindrop_cluster_filter/CMakeLists.txt | 31 ++++ .../low_intensity_cluster_filter.param.yaml | 17 +++ .../low_intensity_cluster_filter.hpp | 78 ++++++++++ .../low_intensity_cluster_filter.launch.xml | 14 ++ .../raindrop_cluster_filter/package.xml | 36 +++++ .../raindrop_cluster_filter.md | 54 +++++++ .../src/low_intensity_cluster_filter.cpp | 141 ++++++++++++++++++ 9 files changed, 384 insertions(+), 1 deletion(-) create mode 100644 perception/raindrop_cluster_filter/CMakeLists.txt create mode 100644 perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml create mode 100644 perception/raindrop_cluster_filter/include/raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp create mode 100644 perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml create mode 100644 perception/raindrop_cluster_filter/package.xml create mode 100644 perception/raindrop_cluster_filter/raindrop_cluster_filter.md create mode 100644 perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 758918e32f1d5..6294df64279a5 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -8,6 +8,7 @@ + @@ -208,9 +209,19 @@ + - + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 822376b365fa8..71fb38f883144 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -77,6 +77,7 @@ + +#include +#include + +#include + +#include +#include +// #include +#include + +#include +#include + +namespace low_intensity_cluster_filter +{ + +class LowIntensityClusterFilter : public rclcpp::Node +{ +public: + explicit LowIntensityClusterFilter(const rclcpp::NodeOptions & node_options); + +private: + void objectCallback( + const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg); + bool isValidatedCluster(const sensor_msgs::msg::PointCloud2 & cluster); + + rclcpp::Publisher::SharedPtr object_pub_; + rclcpp::Subscription::SharedPtr + object_sub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + double intensity_threshold_; + double existence_probability_threshold_; + double max_x_; + double min_x_; + double max_y_; + double min_y_; + + double max_x_transformed_; + double min_x_transformed_; + double max_y_transformed_; + double min_y_transformed_; + // Eigen::Vector4f min_boundary_transformed_; + // Eigen::Vector4f max_boundary_transformed_; + bool is_validation_range_transformed_ = false; + const std::string base_link_frame_id_ = "base_link"; + utils::FilterTargetLabel filter_target_; + + // debugger + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; +}; + +} // namespace low_intensity_cluster_filter + +#endif // RAINDROP_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER__LOW_INTENSITY_CLUSTER_FILTER_HPP_ diff --git a/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml b/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml new file mode 100644 index 0000000000000..9af4ad47055f9 --- /dev/null +++ b/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/raindrop_cluster_filter/package.xml b/perception/raindrop_cluster_filter/package.xml new file mode 100644 index 0000000000000..492b789239d54 --- /dev/null +++ b/perception/raindrop_cluster_filter/package.xml @@ -0,0 +1,36 @@ + + + + raindrop_cluster_filter + 0.1.0 + The ROS 2 filter cluster package + Yukihiro Saito + Dai Nguyen + Yoshi Ri + Apache License 2.0 + Dai Nguyen + + ament_cmake + autoware_cmake + detected_object_validation + lanelet2_extension + message_filters + nav_msgs + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/perception/raindrop_cluster_filter/raindrop_cluster_filter.md b/perception/raindrop_cluster_filter/raindrop_cluster_filter.md new file mode 100644 index 0000000000000..175bbd00a93ea --- /dev/null +++ b/perception/raindrop_cluster_filter/raindrop_cluster_filter.md @@ -0,0 +1,54 @@ +# low_intensity_cluster_filter + +## Purpose + +The `low_intensity_cluster_filter` is a node that filters clusters based on the intensity of their pointcloud. + +Mainly this focuses on filtering out unknown objects with very low intensity pointcloud, such as fail detection of unknown objects caused by raindrop or water splash from ego or other fast moving vehicles. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------- | -------------------------------------------------------- | ---------------------- | +| `input/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | input detected objects | + +### Output + +| Name | Type | Description | +| --------------- | -------------------------------------------------------- | ------------------------- | +| `output/object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | filtered detected objects | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| --------------------------------- | ----- | ------------------------------------------------------- | --------------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| `max_x` | float | 60.00 | Maximum of x of the filter effective range | +| `min_x` | float | -20.00 | Minimum of x of the filter effective range | +| `max_y` | float | 20.00 | Maximum of y of the filter effective range | +| `min_y` | float | -20.00 | Minium of y of the filter effective range | +| `intensity_threshold` | float | 1.0 | The threshold of average intensity for filter | +| `existence_probability_threshold` | float | The existence probability threshold to apply the filter | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp new file mode 100644 index 0000000000000..5c47b61eaa0a3 --- /dev/null +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -0,0 +1,141 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "raindrop_cluster_filter/low_intensity_cluster_filter/low_intensity_cluster_filter.hpp" + +#include +#include + +#include + +#include +namespace low_intensity_cluster_filter +{ +LowIntensityClusterFilter::LowIntensityClusterFilter(const rclcpp::NodeOptions & node_options) +: Node("low_intensity_cluster_filter_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + intensity_threshold_ = declare_parameter("intensity_threshold"); + existence_probability_threshold_ = declare_parameter("existence_probability_threshold"); + max_x_ = declare_parameter("max_x"); + min_x_ = declare_parameter("min_x"); + max_y_ = declare_parameter("max_y"); + min_y_ = declare_parameter("min_y"); + + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN"); + filter_target_.CAR = declare_parameter("filter_target_label.CAR"); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK"); + filter_target_.BUS = declare_parameter("filter_target_label.BUS"); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER"); + filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE"); + filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE"); + filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN"); + + using std::placeholders::_1; + // Set publisher/subscriber + object_sub_ = this->create_subscription( + "input/objects", rclcpp::QoS{1}, + std::bind(&LowIntensityClusterFilter::objectCallback, this, _1)); + object_pub_ = this->create_publisher( + "output/objects", rclcpp::QoS{1}); + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = + std::make_unique(this, "low_intensity_cluster_filter_node"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } +} + +void LowIntensityClusterFilter::objectCallback( + const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) +{ + // Guard + stop_watch_ptr_->toc("processing_time", true); + if (object_pub_->get_subscription_count() < 1) return; + + tier4_perception_msgs::msg::DetectedObjectsWithFeature output_object_msg; + output_object_msg.header = input_msg->header; + geometry_msgs::msg::TransformStamped transform_stamp; + try { + transform_stamp = tf_buffer_.lookupTransform( + input_msg->header.frame_id, base_link_frame_id_, tf2_ros::fromMsg(input_msg->header.stamp)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "Failed to lookup transform: %s", ex.what()); + return; + } + geometry_msgs::msg::Pose min_range; + min_range.position.x = min_x_; + min_range.position.y = min_y_; + geometry_msgs::msg::Pose max_pose; + max_pose.position.x = max_x_; + max_pose.position.y = max_y_; + auto min_ranged_transformed = tier4_autoware_utils::transformPose(min_range, transform_stamp); + auto max_range_transformed = tier4_autoware_utils::transformPose(max_pose, transform_stamp); + for (const auto & feature_object : input_msg->feature_objects) { + const auto & object = feature_object.object; + const auto & label = object.classification.front().label; + const auto & feature = feature_object.feature; + const auto & cluster = feature.cluster; + const auto existence_probability = object.existence_probability; + const auto & position = object.kinematics.pose_with_covariance.pose.position; + bool is_inside_validation_range = min_ranged_transformed.position.x < position.x && + position.x < max_range_transformed.position.x && + min_ranged_transformed.position.y < position.x && + position.y < max_range_transformed.position.y; + int intensity_index = pcl::getFieldIndex(cluster, "intensity"); + if ( + intensity_index != -1 && filter_target_.isTarget(label) && is_inside_validation_range && + !isValidatedCluster(cluster) && existence_probability < existence_probability_threshold_) { + continue; + } + output_object_msg.feature_objects.emplace_back(feature_object); + } + object_pub_->publish(output_object_msg); + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} +bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::PointCloud2 & cluster) +{ + double mean_intensity = 0.0; + if (cluster.point_step < 16) { + RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16."); + return true; + } + for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); iter != iter.end(); + ++iter) { + mean_intensity += *iter; + } + const size_t num_points = cluster.width * cluster.height; + mean_intensity /= static_cast(num_points); + if (mean_intensity > intensity_threshold_) { + return true; + } + return false; +} + +} // namespace low_intensity_cluster_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(low_intensity_cluster_filter::LowIntensityClusterFilter)