From cf797055784b59cd58c6f239aaecbf10cc0c2f81 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 13 Jun 2023 12:15:50 +0900 Subject: [PATCH 1/3] feature(detected_object_validation): add BOUNDING_BOX interface to lanelet filter Signed-off-by: scepter914 --- .../object_lanelet_filter.hpp | 2 ++ .../src/object_lanelet_filter.cpp | 28 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index 48b7d89b295cd..d1d3cf9e904c5 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -63,6 +63,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + geometry_msgs::msg::Polygon setFootprint( + const autoware_auto_perception_msgs::msg::DetectedObject &); }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 8ac0c40e115cb..93d15c1ec1448 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -82,6 +82,13 @@ void ObjectLaneletFilterNode::objectCallback( return; } + // if shape is BOUNDING_BOX, make foot print + for (auto & object : transformed_objects.objects) { + if (object.shape.type == object.shape.BOUNDING_BOX) { + object.shape.footprint = setFootprint(object); + } + } + // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); // get intersected lanelets @@ -110,6 +117,27 @@ void ObjectLaneletFilterNode::objectCallback( object_pub_->publish(output_object_msg); } +geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( + const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) +{ + const auto object_size = detected_object.shape.dimensions; + const double x_front = object_size.x / 2.0; + const double x_rear = -object_size.x / 2.0; + const double y_left = object_size.y / 2.0; + const double y_right = -object_size.y / 2.0; + + geometry_msgs::msg::Polygon footprint; + footprint.points.push_back( + geometry_msgs::build().x(x_front).y(y_left).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_front).y(y_right).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_rear).y(y_right).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_rear).y(y_left).z(0.0)); + return footprint; +} + LinearRing2d ObjectLaneletFilterNode::getConvexHull( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects) { From 54176a39f938c43f32fc5c1512b4d0ea49196d8f Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 13 Jun 2023 12:22:38 +0900 Subject: [PATCH 2/3] update README Signed-off-by: scepter914 --- .../detected_object_validation/object-lanelet-filter.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/detected_object_validation/object-lanelet-filter.md index 2f0438b616401..eebd8ede79270 100644 --- a/perception/detected_object_validation/object-lanelet-filter.md +++ b/perception/detected_object_validation/object-lanelet-filter.md @@ -2,7 +2,7 @@ ## Purpose -The `object_lanelet_filter` is a node that filters detected object by using vector map. +The `object_lanelet_filter` is a node that filters detected object by using vector map. The objects only inside of the vector map will be published. ## Inner-workings / Algorithms @@ -39,7 +39,7 @@ The objects only inside of the vector map will be published. ## Assumptions / Known limits -Filtering is performed based on the shape polygon of the object. +The lanelet filter is performed based on the shape polygon and bounding box of the objects. ## (Optional) Error detection and handling From d0ef539f7d91353f680b9fb51e92135675e72937 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 20 Jun 2023 06:42:17 +0900 Subject: [PATCH 3/3] avoid reuse msg data Signed-off-by: scepter914 --- .../src/object_lanelet_filter.cpp | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 93d15c1ec1448..bd33db92ad78f 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -82,13 +82,6 @@ void ObjectLaneletFilterNode::objectCallback( return; } - // if shape is BOUNDING_BOX, make foot print - for (auto & object : transformed_objects.objects) { - if (object.shape.type == object.shape.BOUNDING_BOX) { - object.shape.footprint = setFootprint(object); - } - } - // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); // get intersected lanelets @@ -96,7 +89,7 @@ void ObjectLaneletFilterNode::objectCallback( int index = 0; for (const auto & object : transformed_objects.objects) { - const auto & footprint = object.shape.footprint; + const auto footprint = setFootprint(object); const auto & label = object.classification.front().label; if (filter_target_.isTarget(label)) { Polygon2d polygon; @@ -120,21 +113,25 @@ void ObjectLaneletFilterNode::objectCallback( geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) { - const auto object_size = detected_object.shape.dimensions; - const double x_front = object_size.x / 2.0; - const double x_rear = -object_size.x / 2.0; - const double y_left = object_size.y / 2.0; - const double y_right = -object_size.y / 2.0; - geometry_msgs::msg::Polygon footprint; - footprint.points.push_back( - geometry_msgs::build().x(x_front).y(y_left).z(0.0)); - footprint.points.push_back( - geometry_msgs::build().x(x_front).y(y_right).z(0.0)); - footprint.points.push_back( - geometry_msgs::build().x(x_rear).y(y_right).z(0.0)); - footprint.points.push_back( - geometry_msgs::build().x(x_rear).y(y_left).z(0.0)); + if (detected_object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto object_size = detected_object.shape.dimensions; + const double x_front = object_size.x / 2.0; + const double x_rear = -object_size.x / 2.0; + const double y_left = object_size.y / 2.0; + const double y_right = -object_size.y / 2.0; + + footprint.points.push_back( + geometry_msgs::build().x(x_front).y(y_left).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_front).y(y_right).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_rear).y(y_right).z(0.0)); + footprint.points.push_back( + geometry_msgs::build().x(x_rear).y(y_left).z(0.0)); + } else { + footprint = detected_object.shape.footprint; + } return footprint; } @@ -144,7 +141,8 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull( MultiPoint2d candidate_points; for (const auto & object : detected_objects.objects) { const auto & pos = object.kinematics.pose_with_covariance.pose.position; - for (const auto & p : object.shape.footprint.points) { + const auto footprint = setFootprint(object); + for (const auto & p : footprint.points) { candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); } }