From 1ba9c670e4842816b02d4cf513f4fe068c1cbdcb Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 5 Mar 2024 13:46:58 +0900 Subject: [PATCH] rename param Signed-off-by: Daniel Sanchez --- .../config/start_planner.param.yaml | 1 + .../behavior_path_start_planner_module/data_structs.hpp | 1 + planning/behavior_path_start_planner_module/src/manager.cpp | 6 ++++++ .../src/start_planner_module.cpp | 6 ++++-- 4 files changed, 12 insertions(+), 2 deletions(-) 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 fad29b84c9c76..b17db7f907471 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 @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 + extra_width_margin_for_rear_obstacle: 0.5 th_moving_object_velocity: 1.0 object_types_to_check_for_path_generation: check_car: true 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 07c81d2edd050..1e32237ffd870 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 @@ -64,6 +64,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double extra_width_margin_for_rear_obstacle{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index beeb675efd3ae..050d591128a15 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); 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 dc202fea6c066..c8b90d8b2eec2 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 @@ -401,7 +401,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); double closest_object_width = -1.0; - std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -420,8 +419,11 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const std::cerr << "Dims \n"; std::cerr << "target_object_itr->shape.dimensions.y " << closest_object_width.value() << "\n"; std::cerr << "gap_between_ego_and_lane_border " << gap_between_ego_and_lane_border << "\n"; + std::cerr << "parameters_->extra_width_margin_for_rear_obstacle " + << parameters_->extra_width_margin_for_rear_obstacle << "\n"; // Decide if the closest object does not fit in the gap left by the ego vehicle. - return closest_object_width.value() > gap_between_ego_and_lane_border; + return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > + gap_between_ego_and_lane_border; } bool StartPlannerModule::isOverlapWithCenterLane() const