From 727d5696f6f42d4c2ea796fa19eec7290870a301 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 9 Feb 2024 19:31:08 +0900 Subject: [PATCH 1/5] add object_types_to_check_for_path_generation Signed-off-by: kyoichi-sugahara --- .../config/start_planner.param.yaml | 9 +++++++++ .../data_structs.hpp | 2 ++ .../src/manager.cpp | 16 ++++++++++++++++ .../src/start_planner_module.cpp | 7 ++++++- 4 files changed, 33 insertions(+), 1 deletion(-) 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..895e1382f730d 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -49,6 +49,22 @@ 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"); + 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"); 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..91fa41fbc8287 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,11 @@ 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] = + 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 +1038,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; } From 65e1bce9b2aa9222508a5d255588f5713502309f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 04:48:28 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- .../src/start_planner_module.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 91fa41fbc8287..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,8 @@ 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); - 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); From 7df08514499065aa247d607070333da44a75ef9e Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 29 Feb 2024 03:25:17 +0900 Subject: [PATCH 3/5] fix namespace for parameter Signed-off-by: kyoichi-sugahara --- .../src/manager.cpp | 35 ++++++++++--------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 895e1382f730d..52bf4f2085db6 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -49,22 +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"); - 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"); + { + 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"); From 306594a1f15d118682381e7921ed064f3a8e6ae4 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 1 Mar 2024 19:04:33 +0900 Subject: [PATCH 4/5] Add readme Signed-off-by: kyoichi-sugahara --- planning/behavior_path_start_planner_module/README.md | 7 +++++++ 1 file changed, 7 insertions(+) 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** From 2ec4bfbaa9947c2092c4897159a6ed9e20610d44 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 2 Mar 2024 20:39:47 +0900 Subject: [PATCH 5/5] define updating feature for object_types_to_check_for_path_generation Signed-off-by: kyoichi-sugahara --- .../src/manager.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 52bf4f2085db6..beeb675efd3ae 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -377,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(