feat(object_lanelet_filter): add configurable margin for object lanel… (

autowarefoundation#9240) * feat(object_lanelet_filter): add configurable margin for object lanelet filter Signed-off-by: Sebastian Zęderowski <[email protected]> modified: perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp Signed-off-by: Sebastian Zęderowski <[email protected]> * feat(object_lanelet_filter): add condition to check wheter polygon is empty in debug mode Signed-off-by: Sebastian Zęderowski <[email protected]> * feat(object_lanelet_filter): fix cppcheck Signed-off-by: Sebastian Zęderowski <[email protected]> * fix: brig back missing type definition Signed-off-by: Taekjin LEE <[email protected]> Signed-off-by: Sebastian Zęderowski <[email protected]> * feat: add stop watch for processing time in object lanelet filter Signed-off-by: Taekjin LEE <[email protected]> Signed-off-by: Sebastian Zęderowski <[email protected]> * feat(object_lanelet_filter): remove extra comment line Signed-off-by: Sebastian Zęderowski <[email protected]> * feat(_object_lanelet_filter): udpate readme Signed-off-by: Sebastian Zęderowski <[email protected]> * style(pre-commit): autofix Signed-off-by: Sebastian Zęderowski <[email protected]> * Update perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp Co-authored-by: Shintaro Tomie <[email protected]> Signed-off-by: Sebastian Zęderowski <[email protected]> * style(pre-commit): autofix Signed-off-by: Sebastian Zęderowski <[email protected]> --------- Signed-off-by: Sebastian Zęderowski <[email protected]> Signed-off-by: Taekjin LEE <[email protected]> Co-authored-by: Sebastian Zęderowski <[email protected]> Co-authored-by: Taekjin LEE <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shintaro Tomie <[email protected]>
tier4 · Dec 2, 2024 · 4479e94 · 4479e94
1 parent ce49bfc
commit 4479e94
Show file tree

Hide file tree

Showing 6 changed files with 231 additions and 11 deletions.
diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt
@@ -48,6 +48,7 @@ target_link_libraries(obstacle_pointcloud_based_validator
 )
 
 ament_auto_add_library(object_lanelet_filter SHARED
+  src/lanelet_filter/debug.cpp
   src/lanelet_filter/lanelet_filter.cpp
   lib/utils/utils.cpp
 )

diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml
@@ -19,3 +19,5 @@
         enabled: false
         velocity_yaw_threshold: 0.785398 # [rad] (45 deg)
         object_speed_threshold: 3.0 # [m/s]
+      debug: false
+      lanelet_extra_margin: 0.0
diff --git a/perception/autoware_detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md
@@ -24,6 +24,11 @@ The objects only inside of the vector map will be published.
 
 ## Parameters
 
+| Name                   | Type     | Description                                                                          |
+| ---------------------- | -------- | ------------------------------------------------------------------------------------ |
+| `debug`                | `bool`   | if true, publish debug markers                                                       |
+| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled |
+
 ### Core Parameters
 
 | Name                             | Type | Default Value | Description                               |

diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp
@@ -0,0 +1,128 @@
+// Copyright 2022 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 "lanelet_filter.hpp"
+
+#include <boost/geometry/algorithms/convex_hull.hpp>
+#include <boost/geometry/algorithms/disjoint.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/Polygon.h>
+
+#include <optional>
+
+namespace autoware::detected_object_validation
+{
+namespace lanelet_filter
+{
+
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
+template <typename T>
+std::optional<visualization_msgs::msg::Marker> createPolygonMarker(
+  T polygon, rclcpp::Time stamp, std::string_view ns, size_t marker_id,
+  std_msgs::msg::ColorRGBA color)
+{
+  if (polygon.empty()) {
+    return std::nullopt;
+  }
+  const constexpr std::string_view FRAME_ID = "map";
+
+  auto create_marker = [&](auto marker_type) {
+    Marker marker;
+    marker.id = marker_id;
+    marker.header.frame_id = FRAME_ID;
+    marker.header.stamp = stamp;
+    marker.ns = ns;
+    marker.type = marker_type;
+    marker.action = Marker::ADD;
+    marker.lifetime = rclcpp::Duration::from_seconds(0.1);
+    return marker;
+  };
+
+  auto marker = create_marker(Marker::LINE_LIST);
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 0.1;
+  marker.color.a = color.a;
+  marker.color.r = color.r;
+  marker.color.g = color.g;
+  marker.color.b = color.b;
+
+  std::vector<geometry_msgs::msg::Point> points;
+  points.reserve(polygon.size() * 2);
+
+  for (auto it = polygon.begin(); it != std::prev(polygon.end()); ++it) {
+    geometry_msgs::msg::Point point;
+
+    point.x = it->x();
+    point.y = it->y();
+    points.push_back(point);
+
+    const auto next = std::next(it);
+    point.x = next->x();
+    point.y = next->y();
+    points.push_back(point);
+  }
+  geometry_msgs::msg::Point point;
+  point.x = polygon.back().x();
+  point.y = polygon.back().y();
+
+  points.push_back(point);
+  point.x = polygon.front().x();
+  point.y = polygon.front().y();
+
+  points.push_back(point);
+  marker.points = points;
+  return marker;
+}
+
+void ObjectLaneletFilterNode::publishDebugMarkers(
+  rclcpp::Time stamp, const LinearRing2d & hull, const std::vector<BoxAndLanelet> & lanelets)
+{
+  using visualization_msgs::msg::Marker;
+  using visualization_msgs::msg::MarkerArray;
+
+  uint8_t marker_id = 0;
+  Marker delete_marker;
+  constexpr std::string_view lanelet_range = "lanelet_range";
+  constexpr std::string_view roi = "roi";
+
+  MarkerArray marker_array;
+
+  std_msgs::msg::ColorRGBA color;
+  color.a = 0.5f;
+  color.r = 0.3f;
+  color.g = 1.0f;
+  color.b = 0.2f;
+  if (auto marker = createPolygonMarker(hull, stamp, lanelet_range, ++marker_id, color); marker) {
+    marker_array.markers.push_back(std::move(*marker));
+  }
+  for (const auto & box_and_lanelet : lanelets) {
+    color.r = 0.2;
+    color.g = 0.5;
+    color.b = 1.0;
+    auto p = box_and_lanelet.second.polygon;
+    if (auto marker = createPolygonMarker(p, stamp, roi, ++marker_id, color); marker) {
+      marker_array.markers.push_back(std::move(*marker));
+    }
+  }
+  viz_pub_->publish(marker_array);
+}
+}  // namespace lanelet_filter
+}  // namespace autoware::detected_object_validation
diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp
@@ -59,6 +59,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
     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");
+  filter_settings_.debug = declare_parameter<bool>("filter_settings.debug");
+  filter_settings_.lanelet_extra_margin =
+    declare_parameter<double>("filter_settings.lanelet_extra_margin");
 
   // Set publisher/subscriber
   map_sub_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
@@ -73,6 +76,41 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
     std::make_unique<autoware::universe_utils::DebugPublisher>(this, "object_lanelet_filter");
   published_time_publisher_ =
     std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
+  stop_watch_ptr_ =
+    std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
+  if (filter_settings_.debug) {
+    viz_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "~/debug/marker", rclcpp::QoS{1});
+  }
+}
+
+bool isInPolygon(
+  const geometry_msgs::msg::Pose & current_pose, const lanelet::BasicPolygon2d & polygon,
+  const double radius)
+{
+  constexpr double eps = 1.0e-9;
+  const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y);
+  return boost::geometry::distance(p, polygon) < radius + eps;
+}
+
+LinearRing2d expandPolygon(const LinearRing2d & polygon, double distance)
+{
+  universe_utils::MultiPolygon2d multi_polygon;
+  bg::strategy::buffer::distance_symmetric<double> distance_strategy(distance);
+  bg::strategy::buffer::join_miter join_strategy;
+  bg::strategy::buffer::end_flat end_strategy;
+  bg::strategy::buffer::point_circle circle_strategy;
+  bg::strategy::buffer::side_straight side_strategy;
+
+  bg::buffer(
+    polygon, multi_polygon, distance_strategy, side_strategy, join_strategy, end_strategy,
+    circle_strategy);
+
+  if (multi_polygon.empty()) {
+    return polygon;
+  }
+
+  return multi_polygon.front().outer();
 }
 
 void ObjectLaneletFilterNode::mapCallback(
@@ -86,6 +124,8 @@ void ObjectLaneletFilterNode::mapCallback(
 void ObjectLaneletFilterNode::objectCallback(
   const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg)
 {
+  stop_watch_ptr_->tic("processing_time");
+
   // Guard
   if (object_pub_->get_subscription_count() < 1) return;
 
@@ -116,6 +156,9 @@ void ObjectLaneletFilterNode::objectCallback(
       local_rtree.insert(bbox_and_lanelet);
     }
 
+    if (filter_settings_.debug) {
+      publishDebugMarkers(input_msg->header.stamp, convex_hull, intersected_lanelets_with_bbox);
+    }
     // filtering process
     for (size_t index = 0; index < transformed_objects.objects.size(); ++index) {
       const auto & transformed_object = transformed_objects.objects.at(index);
@@ -135,6 +178,8 @@ void ObjectLaneletFilterNode::objectCallback(
       .count();
   debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
     "debug/pipeline_latency_ms", pipeline_latency);
+  debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
 }
 
 bool ObjectLaneletFilterNode::filterObject(
@@ -217,6 +262,9 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull(
   LinearRing2d convex_hull;
   bg::convex_hull(candidate_points, convex_hull);
 
+  if (filter_settings_.lanelet_extra_margin > 0) {
+    return expandPolygon(convex_hull, filter_settings_.lanelet_extra_margin);
+  }
   return convex_hull;
 }
 
@@ -265,19 +313,38 @@ std::vector<BoxAndLanelet> ObjectLaneletFilterNode::getIntersectedLanelets(
        lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) {
       if (bg::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) {
         // create bbox using boost for making the R-tree in later phase
-        lanelet::BoundingBox2d lanelet_bbox = lanelet::geometry::boundingBox2d(lanelet);
-        Point2d min_corner(lanelet_bbox.min().x(), lanelet_bbox.min().y());
-        Point2d max_corner(lanelet_bbox.max().x(), lanelet_bbox.max().y());
-        Box boost_bbox(min_corner, max_corner);
+        auto polygon = getPolygon(lanelet);
+        Box boost_bbox;
+        bg::envelope(polygon, boost_bbox);
 
-        intersected_lanelets_with_bbox.emplace_back(std::make_pair(boost_bbox, lanelet));
+        intersected_lanelets_with_bbox.emplace_back(
+          std::make_pair(boost_bbox, PolygonAndLanelet{polygon, lanelet}));
       }
     }
   }
 
   return intersected_lanelets_with_bbox;
 }
 
+lanelet::BasicPolygon2d ObjectLaneletFilterNode::getPolygon(const lanelet::ConstLanelet & lanelet)
+{
+  if (filter_settings_.lanelet_extra_margin <= 0) {
+    return lanelet.polygon2d().basicPolygon();
+  }
+
+  auto lanelet_polygon = lanelet.polygon2d().basicPolygon();
+  Polygon2d polygon;
+  bg::assign_points(polygon, lanelet_polygon);
+
+  bg::correct(polygon);
+  auto polygon_result = expandPolygon(polygon.outer(), filter_settings_.lanelet_extra_margin);
+  lanelet::BasicPolygon2d result;
+
+  bg::assign_points(result, polygon_result);
+
+  return result;
+}
+
 bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
   const autoware_perception_msgs::msg::DetectedObject & object,
   const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree)
@@ -314,7 +381,7 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
       point2d.position.y = point_transformed.y;
 
       for (const auto & candidate : candidates) {
-        if (lanelet::utils::isInLanelet(point2d, candidate.second, 0.0)) {
+        if (isInPolygon(point2d, candidate.second.polygon, 0.0)) {
           return true;
         }
       }
@@ -334,7 +401,7 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
   local_rtree.query(bgi::intersects(bbox_of_convex_hull), std::back_inserter(candidates));
 
   for (const auto & box_and_lanelet : candidates) {
-    if (!bg::disjoint(polygon, box_and_lanelet.second.polygon2d().basicPolygon())) {
+    if (!bg::disjoint(polygon, box_and_lanelet.second.polygon)) {
       return true;
     }
   }
@@ -374,14 +441,14 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets(
   local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates));
 
   for (const auto & box_and_lanelet : candidates) {
-    const bool is_in_lanelet = lanelet::utils::isInLanelet(
-      object.kinematics.pose_with_covariance.pose, box_and_lanelet.second, 0.0);
+    const bool is_in_lanelet =
+      isInPolygon(object.kinematics.pose_with_covariance.pose, box_and_lanelet.second.polygon, 0.0);
     if (!is_in_lanelet) {
       continue;
     }
 
     const double lane_yaw = lanelet::utils::getLaneletAngle(
-      box_and_lanelet.second, object.kinematics.pose_with_covariance.pose.position);
+      box_and_lanelet.second.lanelet, object.kinematics.pose_with_covariance.pose.position);
     const double delta_yaw = object_velocity_yaw - lane_yaw;
     const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw);
     const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);

diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp
@@ -19,12 +19,14 @@
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/ros/debug_publisher.hpp"
 #include "autoware/universe_utils/ros/published_time_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
 #include "autoware_lanelet2_extension/utility/utilities.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_map_msgs/msg/lanelet_map_bin.hpp"
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <boost/geometry/index/rtree.hpp>
 
@@ -50,7 +52,13 @@ namespace bg = boost::geometry;
 namespace bgi = boost::geometry::index;
 using Point2d = bg::model::point<double, 2, bg::cs::cartesian>;
 using Box = boost::geometry::model::box<Point2d>;
-using BoxAndLanelet = std::pair<Box, lanelet::Lanelet>;
+
+struct PolygonAndLanelet
+{
+  lanelet::BasicPolygon2d polygon;
+  lanelet::ConstLanelet lanelet;
+};
+using BoxAndLanelet = std::pair<Box, PolygonAndLanelet>;
 using RtreeAlgo = bgi::rstar<16>;
 
 class ObjectLaneletFilterNode : public rclcpp::Node
@@ -62,10 +70,16 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   void objectCallback(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
   void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr);
 
+  void publishDebugMarkers(
+    rclcpp::Time stamp, const LinearRing2d & hull, const std::vector<BoxAndLanelet> & lanelets);
+
   rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr viz_pub_;
   rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr map_sub_;
   rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
+
   std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_{nullptr};
+  std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
 
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   std::string lanelet_frame_id_;
@@ -80,6 +94,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
     bool lanelet_direction_filter;
     double lanelet_direction_filter_velocity_yaw_threshold;
     double lanelet_direction_filter_object_speed_threshold;
+    bool debug;
+    double lanelet_extra_margin;
   } filter_settings_;
 
   bool filterObject(
@@ -101,6 +117,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
     const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree);
   geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &);
 
+  lanelet::BasicPolygon2d getPolygon(const lanelet::ConstLanelet & lanelet);
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
 };