From 54f18afef91aa2733feeb8cfbff73b405f57afc3 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 24 May 2024 02:10:05 +0900 Subject: [PATCH] feat: add lanelet direction filter Signed-off-by: yoshiri --- .../config/object_lanelet_filter.param.yaml | 12 ++- .../object_lanelet_filter.hpp | 11 +++ .../src/object_lanelet_filter.cpp | 79 ++++++++++++++++--- 3 files changed, 90 insertions(+), 12 deletions(-) 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..28d24aa13a20b 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -2,10 +2,20 @@ ros__parameters: filter_target_label: UNKNOWN : true - CAR : false + CAR : true TRUCK : false BUS : false TRAILER : false 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: true + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] 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 1bb69e14f884b..00826de68157f 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,6 +17,7 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include #include @@ -62,6 +63,13 @@ 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, @@ -73,6 +81,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); 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/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 18bc0daec7be7..3693167e7427d 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( @@ -127,21 +136,33 @@ bool ObjectLaneletFilterNode::filterObject( { 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 - Polygon2d polygon; - const auto footprint = setFootprint(transformed_object); - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( - point, transformed_object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + if (filter_settings_.polygon_overlap_filter) { + Polygon2d polygon; + const auto footprint = setFootprint(transformed_object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( + point, transformed_object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + const bool is_polygon_overlap = + isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || + isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + if (filter_settings_.lanelet_direction_filter) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; } - polygon.outer().push_back(polygon.outer().front()); - const bool is_polygon_overlap = - isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || - isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); // push back to output object - if (is_polygon_overlap) { + if (filter_pass) { output_object_msg.objects.emplace_back(input_object); return true; } @@ -221,6 +242,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