diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 41df033693baa..e890503ae3ffb 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,19 +65,27 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | -| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| 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 | ## Safety check with static obstacles 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 bbbf182d1c87c..8c8069b4b3c55 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/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index dd67c75c766e9..6af422dffbf20 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -45,6 +45,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 3326a0b403f5d..212dd411c232f 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -48,6 +48,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"); @@ -356,6 +375,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 6f092d0a351e4..743f0eade3c6d 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 @@ -625,9 +625,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 = @@ -993,6 +994,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; }