Skip to content

Commit 9d3e6a9

Browse files
committed
fix(avoidance): check intersection location
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 16a1e85 commit 9d3e6a9

File tree

1 file changed

+9
-3
lines changed
  • planning/behavior_path_avoidance_module/src

1 file changed

+9
-3
lines changed

planning/behavior_path_avoidance_module/src/utils.cpp

+9-3
Original file line numberDiff line numberDiff line change
@@ -318,14 +318,20 @@ bool isWithinCrosswalk(
318318
bool isWithinIntersection(
319319
const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
320320
{
321-
const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else");
322-
if (id == "else") {
321+
const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else");
322+
if (area_id == "else") {
323+
return false;
324+
}
325+
326+
const std::string location = object.overhang_lanelet.attributeOr("location", "else");
327+
if (location == "private") {
323328
return false;
324329
}
325330

326331
const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
327332

328-
const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
333+
const auto polygon =
334+
route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str()));
329335

330336
return boost::geometry::within(
331337
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));

0 commit comments

Comments
 (0)