diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3d0a334cedf84..b0f736916ebda 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -9,7 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: false @@ -120,11 +119,6 @@ # For target object filtering target_filtering: - # params for avoidance of not-parked objects - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] - object_ignore_section_traffic_light_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range object_check_goal_distance: 20.0 # [m] # filtering parking objects @@ -141,6 +135,17 @@ max_forward_distance: 150.0 # [m] backward_distance: 10.0 # [m] + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + force_avoidance: + enable: false # [-] + time_threshold: 1.0 # [s] + ignore_area: + traffic_light: + front_distance: 100.0 # [m] + crosswalk: + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] + # For safety check safety_check: # safety check configuration diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index e881c8a0d3b18..7976399a7ed40 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -381,6 +381,9 @@ struct ObjectData // avoidance target // is stoppable under the constraints bool is_stoppable{false}; + // is within intersection area + bool is_within_intersection{false}; + // unavoidable reason std::string reason{""}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0237fb458ea0b..b8b4efb7aed57 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -39,6 +39,9 @@ bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs); +bool isWithinIntersection( + const ObjectData & object, const std::shared_ptr & route_handler); + bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 38c2620096ca2..bfa83c43b061c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -330,6 +330,8 @@ std::optional getSignedDistanceFromBoundary( Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 7988239e68071..33c6982f99fd9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -43,8 +43,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); @@ -108,14 +106,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -128,6 +118,20 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 78e05c940a814..ee7b45de09e93 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -275,8 +275,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); } // target object @@ -320,14 +318,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -340,6 +330,20 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 725b528abad51..54a1c0c649916 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -292,6 +292,81 @@ bool isWithinCrosswalk( return false; } +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") { + 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())); + + return boost::geometry::within( + object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); +} + +bool isForceAvoidanceTarget( + ObjectData & object, const lanelet::ConstLanelets & extend_lanelets, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (!parameters->enable_force_avoidance_for_stopped_vehicle) { + return false; + } + + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + + if (!stop_time_longer_than_threshold) { + return false; + } + + if (object.is_within_intersection) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); + return false; + } + + const auto rh = planner_data->route_handler; + + if ( + !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && + !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); + return false; + } + + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + + // force avoidance for stopped vehicle + bool not_parked_object = true; + + // check traffic light + const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); + { + not_parked_object = + to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; + } + + // check crosswalk + const auto to_crosswalk = + utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - + object.longitudinal; + { + const auto stop_for_crosswalk = + to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && + to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; + not_parked_object = not_parked_object || stop_for_crosswalk; + } + + object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); + + return !not_parked_object; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -1127,42 +1202,14 @@ void filterTargetObjects( continue; } - // from here condition check for vehicle type objects. + o.is_within_intersection = isWithinIntersection(o, rh); - const auto stop_time_longer_than_threshold = - o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) { - // force avoidance for stopped vehicle - bool not_parked_object = true; - - // check traffic light - const auto to_traffic_light = - utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); - { - not_parked_object = - to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; - } - - // check crosswalk - const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - - o.longitudinal; - { - const auto stop_for_crosswalk = - to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && - to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; - } - - o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - - if (!not_parked_object) { - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); - continue; - } + // from here condition check for vehicle type objects. + if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) { + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + continue; } // Object is on center line -> ignore. diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 3ab20e7f76718..37a628f54607e 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2708,6 +2708,17 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d ret; + for (const auto & p : polygon) { + ret.outer().emplace_back(p.x(), p.y()); + } + ret.outer().push_back(ret.outer().front()); + + return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type)