diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ef1775f314bbd..4b52aae91d032 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -332,6 +332,13 @@ PullOutPath --o PullOutPlannerBase | shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | | geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | | collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 5d82fbbfdf401..fad29b84c9c76 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -8,6 +8,15 @@ collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 + object_types_to_check_for_path_generation: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: true th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 5b41edaee0da8..07c81d2edd050 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -67,6 +67,8 @@ struct StartPlannerParameters std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; + behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; // shift pull out diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index f921df83120ae..beeb675efd3ae 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -49,6 +49,25 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + node->declare_parameter(ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + node->declare_parameter(ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + node->declare_parameter(ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + node->declare_parameter(ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + node->declare_parameter(ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + node->declare_parameter(ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + node->declare_parameter(ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + node->declare_parameter(ns + "check_pedestrian"); + } p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); @@ -358,6 +377,33 @@ void StartPlannerModuleManager::updateModuleParams( parameters, ns + "collision_check_margin_from_front_object", p->collision_check_margin_from_front_object); updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->object_types_to_check_for_path_generation.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->object_types_to_check_for_path_generation.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->object_types_to_check_for_path_generation.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->object_types_to_check_for_path_generation.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->object_types_to_check_for_path_generation.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->object_types_to_check_for_path_generation.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->object_types_to_check_for_path_generation.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->object_types_to_check_for_path_generation.check_pedestrian); + } updateParam(parameters, ns + "center_line_path_interval", p->center_line_path_interval); updateParam(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 04c10a3d18c86..3c08d62f500ae 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -668,9 +668,10 @@ bool StartPlannerModule::findPullOutPath( // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_->th_moving_object_velocity); - const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + utils::path_safety_checker::filterObjectsByClass( + pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation); // if start_pose_candidate is far from refined_start_pose, backward driving is necessary const bool backward_is_unnecessary = @@ -1036,6 +1037,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, object_check_backward_distance); + utils::path_safety_checker::filterObjectsByClass( + stop_objects_in_pull_out_lanes, parameters_->object_types_to_check_for_path_generation); + return stop_objects_in_pull_out_lanes; }