From 4479e9433faf767ddc1607421f0f309b40d4f3dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20Z=C4=99derowski?= Date: Mon, 2 Dec 2024 01:37:06 +0100 Subject: [PATCH] =?UTF-8?q?feat(object=5Flanelet=5Ffilter):=20add=20config?= =?UTF-8?q?urable=20margin=20for=20object=20lanel=E2=80=A6=20(#9240)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(object_lanelet_filter): add configurable margin for object lanelet filter Signed-off-by: Sebastian Zęderowski modified: perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp Signed-off-by: Sebastian Zęderowski * feat(object_lanelet_filter): add condition to check wheter polygon is empty in debug mode Signed-off-by: Sebastian Zęderowski * feat(object_lanelet_filter): fix cppcheck Signed-off-by: Sebastian Zęderowski * fix: brig back missing type definition Signed-off-by: Taekjin LEE Signed-off-by: Sebastian Zęderowski * feat: add stop watch for processing time in object lanelet filter Signed-off-by: Taekjin LEE Signed-off-by: Sebastian Zęderowski * feat(object_lanelet_filter): remove extra comment line Signed-off-by: Sebastian Zęderowski * feat(_object_lanelet_filter): udpate readme Signed-off-by: Sebastian Zęderowski * style(pre-commit): autofix Signed-off-by: Sebastian Zęderowski * Update perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Sebastian Zęderowski * style(pre-commit): autofix Signed-off-by: Sebastian Zęderowski --------- Signed-off-by: Sebastian Zęderowski Signed-off-by: Taekjin LEE Co-authored-by: Sebastian Zęderowski Co-authored-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- .../CMakeLists.txt | 1 + .../config/object_lanelet_filter.param.yaml | 2 + .../object-lanelet-filter.md | 5 + .../src/lanelet_filter/debug.cpp | 128 ++++++++++++++++++ .../src/lanelet_filter/lanelet_filter.cpp | 87 ++++++++++-- .../src/lanelet_filter/lanelet_filter.hpp | 19 ++- 6 files changed, 231 insertions(+), 11 deletions(-) create mode 100644 perception/autoware_detected_object_validation/src/lanelet_filter/debug.cpp diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt index 434c258918493..bf205fb32bb1d 100644 --- 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 index 99050d9738ae6..d15b2c81cf6db 100644 --- 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 index a14e28a42666d..b748885c188b4 100644 --- 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 new file mode 100644 index 0000000000000..b4116982f5b94 --- /dev/null +++ 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 +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace autoware::detected_object_validation +{ +namespace lanelet_filter +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +template +std::optional 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 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 & 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 index e54ffd5452e7c..5e199a9ea9fad 100644 --- 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("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"); + filter_settings_.debug = declare_parameter("filter_settings.debug"); + filter_settings_.lanelet_extra_margin = + declare_parameter("filter_settings.lanelet_extra_margin"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -73,6 +76,41 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod std::make_unique(this, "object_lanelet_filter"); published_time_publisher_ = std::make_unique(this); + stop_watch_ptr_ = + std::make_unique>(); + if (filter_settings_.debug) { + viz_pub_ = this->create_publisher( + "~/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 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( "debug/pipeline_latency_ms", pipeline_latency); + debug_publisher_->publish( + "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,12 +313,12 @@ std::vector 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})); } } } @@ -278,6 +326,25 @@ std::vector ObjectLaneletFilterNode::getIntersectedLanelets( 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 & 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 index 05020336bc759..724e60df27e49 100644 --- 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 #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include @@ -50,7 +52,13 @@ namespace bg = boost::geometry; namespace bgi = boost::geometry::index; using Point2d = bg::model::point; using Box = boost::geometry::model::box; -using BoxAndLanelet = std::pair; + +struct PolygonAndLanelet +{ + lanelet::BasicPolygon2d polygon; + lanelet::ConstLanelet lanelet; +}; +using BoxAndLanelet = std::pair; 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 & lanelets); + rclcpp::Publisher::SharedPtr object_pub_; + rclcpp::Publisher::SharedPtr viz_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; + std::unique_ptr> 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 & local_rtree); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); + lanelet::BasicPolygon2d getPolygon(const lanelet::ConstLanelet & lanelet); std::unique_ptr published_time_publisher_; };