feat(object_lanelet_filter, vector_map_inside_area_filter): cherry-pick unknown removal feature from awf (#1327)

* feat: add utils to handle z height filter

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

* feat: add test to utilities and fix code

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

* feat: add node param to filter by z

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

* feat: cherry-pick lanelet filter update

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

---------

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

Author: YoshiRi

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]

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

perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    enable_debug: false

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 <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -27,6 +29,7 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <memory>
 #include <string>
 
 namespace object_lanelet_filter
@@ -48,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> 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 &);
 };

perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp

@@ -26,6 +26,7 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <memory>
 #include <string>
 
 namespace object_position_filter

perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml

@@ -9,7 +9,6 @@
     <remap from="~/input/detected_objects" to="$(var input/detected_objects)"/>
     <remap from="~/input/obstacle_pointcloud" to="$(var input/obstacle_pointcloud)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="enable_debugger" value="false"/>
     <param from="$(var obstacle_pointcloud_based_validator_param_path)"/>
   </node>
 </launch>

perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml

@@ -7,6 +7,7 @@
   <arg name="input/occupancy_grid_map" default="/perception/object_recognition/detection/validation/occupancy_grid_based/single_frame_occupancy_grid_map"/>
   <arg name="occupancy_grid_map_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/laserscan_based_occupancy_grid_map.param.yaml"/>
   <arg name="occupancy_grid_map_updater_param_path" default="$(find-pkg-share probabilistic_occupancy_grid_map)/config/binary_bayes_filter_updater.param.yaml"/>
+  <arg name="occupancy_grid_based_validator_param_path" default="$(find-pkg-share detected_object_validation)/config/occupancy_grid_based_validator_param_path.param.yaml"/>
 
   <node pkg="probabilistic_occupancy_grid_map" exec="laserscan_based_occupancy_grid_map_node" name="single_frame_laserscan_occupancy_grid_map" output="screen">
     <remap from="~/input/laserscan" to="$(var input/laserscan)"/>
@@ -22,6 +23,6 @@
     <remap from="~/input/detected_objects" to="$(var input/detected_objects)"/>
     <remap from="~/input/occupancy_grid_map" to="$(var input/occupancy_grid_map)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="enable_debug" value="false"/>
+    <param from="$(var occupancy_grid_based_validator_param_path)"/>
   </node>
 </launch>

perception/detected_object_validation/package.xml

@@ -6,7 +6,9 @@
   <description>The ROS 2 detected_object_validation package</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
-  <maintainer email="dai.nguyen@tier4.jp">badai nguyen</maintainer>
+  <maintainer email="dai.nguyen@tier4.jp">Dai Nguyen</maintainer>
+  <maintainer email="shintaro.tomie@tier4.jp">Shintaro Tomie</maintainer>
+  <maintainer email="yoshi.ri@tier4.jp">Yoshi RI</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

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>(
@@ -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<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "output/object", rclcpp::QoS{1});
+
+  debug_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(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<double, std::milli>(
+      std::chrono::nanoseconds(
+        (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds()))
+      .count();
+  debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "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 <rclcpp_components/register_node_macro.hpp>

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 <autoware_auto_perception_msgs/msg/object_classification.hpp>
+
+#include <gtest/gtest.h>
+
+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));
+}