feat: add lanelet direction filter

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

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]

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 <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
@@ -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 &);
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
   filter_target_.MOTORCYCLE = declare_parameter<bool>("filter_target_label.MOTORCYCLE", false);
   filter_target_.BICYCLE = declare_parameter<bool>("filter_target_label.BICYCLE", false);
   filter_target_.PEDESTRIAN = declare_parameter<bool>("filter_target_label.PEDESTRIAN", false);
+  // Set filter settings
+  filter_settings_.polygon_overlap_filter =
+    declare_parameter<bool>("filter_settings.polygon_overlap_filter.enabled");
+  filter_settings_.lanelet_direction_filter =
+    declare_parameter<bool>("filter_settings.lanelet_direction_filter.enabled");
+  filter_settings_.lanelet_direction_filter_velocity_yaw_threshold =
+    declare_parameter<double>("filter_settings.lanelet_direction_filter.velocity_yaw_threshold");
+  filter_settings_.lanelet_direction_filter_object_speed_threshold =
+    declare_parameter<double>("filter_settings.lanelet_direction_filter.object_speed_threshold");
 
   // Set publisher/subscriber
   map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -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 <rclcpp_components/register_node_macro.hpp>