From fea4ed982b6b36b0ad37d803262af6c22520e8c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 31 May 2024 16:05:34 +0900 Subject: [PATCH] fix(avoidance): fix bugs in parked vehicle filtering logic (#7072) * fix(avoidance): check prev/next lane Signed-off-by: satoshi-ota * fix(avoidance): fix parked vehicle filtering Signed-off-by: satoshi-ota * fix(avoidance): check intersection location Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 95 ++++++++++++++++--- 1 file changed, 80 insertions(+), 15 deletions(-) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 1533a7dd49edf..660b8d228b6f2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -318,25 +318,55 @@ 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()))); } -bool isOnEgoLane(const ObjectData & object) +bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - return boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), - object.overhang_lanelet.polygon2d().basicPolygon()); + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon())) { + return true; + } + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + prev_lanelet.front().polygon2d().basicPolygon())) { + return true; + } + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + next_lanelet.polygon2d().basicPolygon())) { + return true; + } + } + + return false; } bool isParallelToEgoLane(const ObjectData & object, const double threshold) @@ -604,14 +634,49 @@ bool isNeverAvoidanceTarget( if (object.is_on_ego_lane) { const auto right_lane = - planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + const lanelet::Attribute & right_lane_sub_type = + right_lane.value().attribute(lanelet::AttributeName::Subtype); + if (right_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_right_lane = + boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_right_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + } + const auto left_lane = - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); - if (right_lane.has_value() && left_lane.has_value()) { - object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; - RCLCPP_DEBUG( - rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); - return true; + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + const lanelet::Attribute & left_lane_sub_type = + left_lane.value().attribute(lanelet::AttributeName::Subtype); + if (left_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_left_lane = + boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_left_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } } } @@ -754,7 +819,7 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & parameters) { object.behavior = getObjectBehavior(object, parameters); - object.is_on_ego_lane = isOnEgoLane(object); + object.is_on_ego_lane = isOnEgoLane(object, planner_data->route_handler); if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { return false;