diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml index dfdee95642fed..99050d9738ae6 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -9,3 +9,13 @@ MOTORCYCLE : false BICYCLE : false PEDESTRIAN : false + + filter_settings: + # polygon overlap based filter + polygon_overlap_filter: + enabled: true + # velocity direction based filter + lanelet_direction_filter: + enabled: false + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index f7a27a52bfa8a..cddee782e8af0 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 4331b5c19d3f1..8c70f020dabd9 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,8 +17,10 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include +#include #include #include @@ -27,6 +29,7 @@ #include #include +#include #include namespace object_lanelet_filter @@ -48,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -58,11 +62,30 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; - + struct FilterSettings + { + bool polygon_overlap_filter; + bool lanelet_direction_filter; + double lanelet_direction_filter_velocity_yaw_threshold; + double lanelet_direction_filter_object_speed_threshold; + } filter_settings_; + + bool filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); + bool isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + bool isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); }; diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..059f97cbad431 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace object_position_filter diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index 31bb633294748..f18f8ab922c49 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,7 +6,9 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura - badai nguyen + Dai Nguyen + Shintaro Tomie + Yoshi RI Apache License 2.0 ament_cmake_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index cd16d414425cb..46af50c311013 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + // Set filter settings + filter_settings_.polygon_overlap_filter = + declare_parameter("filter_settings.polygon_overlap_filter.enabled"); + filter_settings_.lanelet_direction_filter = + declare_parameter("filter_settings.lanelet_direction_filter.enabled"); + filter_settings_.lanelet_direction_filter_velocity_yaw_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); + filter_settings_.lanelet_direction_filter_object_speed_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -52,6 +61,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + debug_publisher_ = + std::make_unique(this, "object_lanelet_filter"); } void ObjectLaneletFilterNode::mapCallback( @@ -93,29 +105,65 @@ void ObjectLaneletFilterNode::objectCallback( lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); - int index = 0; - for (const auto & object : transformed_objects.objects) { - const auto footprint = setFootprint(object); - const auto & label = object.classification.front().label; - if (filter_target_.isTarget(label)) { - Polygon2d polygon; - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - } else { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - ++index; + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject( + transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, + output_object_msg); } object_pub_->publish(output_object_msg); + + // Publish debug info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); +} + +bool ObjectLaneletFilterNode::filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) +{ + const auto & label = transformed_object.classification.front().label; + if (filter_target_.isTarget(label)) { + bool filter_pass = true; + // 1. is polygon overlap with road lanelets or shoulder lanelets + if (filter_settings_.polygon_overlap_filter) { + const bool is_polygon_overlap = + isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + const bool orientation_not_available = + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; + } + + // push back to output object + if (filter_pass) { + output_object_msg.objects.emplace_back(input_object); + return true; + } + } else { + output_object_msg.objects.emplace_back(input_object); + return true; + } + return false; } geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( @@ -176,6 +224,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets; } +bool ObjectLaneletFilterNode::isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets) +{ + // if has bounding box, use polygon overlap + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::POLYGON) { + Polygon2d polygon; + const auto footprint = setFootprint(object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + return isPolygonOverlapLanelets(polygon, intersected_lanelets); + } else { + // if object do not have bounding box, check each footprint is inside polygon + for (const auto & point : object.shape.footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + geometry_msgs::msg::Pose point2d; + point2d.position.x = point_transformed.x; + point2d.position.y = point_transformed.y; + for (const auto & lanelet : intersected_lanelets) { + if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + return true; + } + } + } + return false; + } +} + bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) { @@ -187,6 +268,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( return false; } +bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_velocity_norm = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double object_velocity_yaw = std::atan2( + object.kinematics.twist_with_covariance.twist.linear.y, + object.kinematics.twist_with_covariance.twist.linear.x) + + object_yaw; + + if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { + return true; + } + for (const auto & lanelet : lanelets) { + const bool is_in_lanelet = + lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + if (!is_in_lanelet) { + continue; + } + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_velocity_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { + return true; + } + } + + return false; +} + } // namespace object_lanelet_filter #include diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp new file mode 100644 index 0000000000000..ace4dd10a191f --- /dev/null +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -0,0 +1,67 @@ +// 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 "detected_object_validation/utils/utils.hpp" + +#include + +#include + +using AutowareLabel = autoware_auto_perception_msgs::msg::ObjectClassification; + +utils::FilterTargetLabel createFilterTargetAll() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = true; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = true; + filter.BICYCLE = true; + filter.PEDESTRIAN = true; + return filter; +} + +utils::FilterTargetLabel createFilterTargetVehicle() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = false; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = false; + filter.BICYCLE = false; + filter.PEDESTRIAN = false; + return filter; +} + +TEST(IsTargetTest, AllTarget) +{ + auto filter = createFilterTargetAll(); + auto label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(label)); +} + +TEST(IsTargetTest, VehicleTarget) +{ + auto filter = createFilterTargetVehicle(); + + auto car_label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(car_label)); + + auto unknown_label = AutowareLabel::UNKNOWN; + EXPECT_FALSE(filter.isTarget(unknown_label)); +}