From 70b2693a78b43e2803a3ec2f124aeaadd6a7ebda Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 6 Nov 2024 13:53:56 +0900 Subject: [PATCH] fix(lanelet_filter): add crosswalk lanelet for filtering Signed-off-by: badai-nguyen --- .../detected_object_filter/object_lanelet_filter.hpp | 2 ++ .../src/object_lanelet_filter.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 8c70f020dabd9..2dd897e110bfc 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -56,6 +56,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; lanelet::ConstLanelets shoulder_lanelets_; + lanelet::ConstLanelets crosswalk_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; @@ -75,6 +76,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & input_object, const lanelet::ConstLanelets & intersected_road_lanelets, const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const lanelet::ConstLanelets & intersected_crosswalk_lanelets, autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 46af50c311013..ca94f750867ea 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -75,6 +75,7 @@ void ObjectLaneletFilterNode::mapCallback( const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + crosswalk_lanelets_ = lanelet::utils::query::crosswalkLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -104,6 +105,9 @@ void ObjectLaneletFilterNode::objectCallback( getIntersectedLanelets(convex_hull, road_lanelets_); lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); + lanelet::ConstLanelets intersected_crosswalk_lanelets = + getIntersectedLanelets(convex_hull, crosswalk_lanelets_); + // filtering process for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { @@ -111,7 +115,7 @@ void ObjectLaneletFilterNode::objectCallback( const auto & input_object = input_msg->objects.at(index); filterObject( transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, - output_object_msg); + intersected_crosswalk_lanelets, output_object_msg); } object_pub_->publish(output_object_msg); @@ -130,6 +134,7 @@ bool ObjectLaneletFilterNode::filterObject( const autoware_auto_perception_msgs::msg::DetectedObject & input_object, const lanelet::ConstLanelets & intersected_road_lanelets, const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const lanelet::ConstLanelets & intersected_crosswalk_lanelets, autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) { const auto & label = transformed_object.classification.front().label; @@ -139,7 +144,8 @@ bool ObjectLaneletFilterNode::filterObject( if (filter_settings_.polygon_overlap_filter) { const bool is_polygon_overlap = isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || - isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_crosswalk_lanelets); filter_pass = filter_pass && is_polygon_overlap; }