From 9d3e6a92a59315467ec6c9bfce0f3056b0b3aadf Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 20 May 2024 19:15:31 +0900 Subject: [PATCH] fix(avoidance): check intersection location Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/utils.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 72f41f0cc4c5a..6b2950a5ba1ac 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -318,14 +318,20 @@ bool isWithinCrosswalk( bool isWithinIntersection( const ObjectData & object, const std::shared_ptr & route_handler) { - const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else"); + if (area_id == "else") { + return false; + } + + const std::string location = object.overhang_lanelet.attributeOr("location", "else"); + if (location == "private") { return false; } const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); - const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));