diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8406affd5529f..bd96a095eb767 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -334,15 +334,341 @@ void StartPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] const std::string ns = name_ + "."; - - std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { - if (!observer.expired()) { - const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); - if (start_planner_ptr) { - start_planner_ptr->updateModuleParams(p); - } + const std::string ns = "start_planner."; + + { + updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); + updateParam(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); + updateParam( + parameters, ns + "th_turn_signal_on_lateral_offset", p->th_turn_signal_on_lateral_offset); + updateParam( + parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); + updateParam( + parameters, ns + "intersection_search_length", p->intersection_search_length); + updateParam( + parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", + p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam>( + parameters, ns + "collision_check_margins", p->collision_check_margins); + updateParam( + 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); + 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( + parameters, ns + "shift_collision_check_distance_from_end", + p->shift_collision_check_distance_from_end); + updateParam( + parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance); + updateParam( + parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num); + updateParam(parameters, ns + "lateral_jerk", p->lateral_jerk); + updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); + updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); + updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + updateParam( + parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval); + updateParam( + parameters, ns + "lane_departure_margin", + p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "pull_out_max_steer_angle", + p->parallel_parking_parameters.pull_out_max_steer_angle); + updateParam(parameters, ns + "enable_back", p->enable_back); + updateParam(parameters, ns + "backward_velocity", p->backward_velocity); + updateParam( + parameters, ns + "geometric_pull_out_velocity", + p->parallel_parking_parameters.pull_out_velocity); + updateParam( + parameters, ns + "geometric_collision_check_distance_from_end", + p->geometric_collision_check_distance_from_end); + updateParam( + parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam(parameters, ns + "search_priority", p->search_priority); + updateParam(parameters, ns + "max_back_distance", p->max_back_distance); + updateParam( + parameters, ns + "backward_search_resolution", p->backward_search_resolution); + updateParam( + parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); + updateParam( + parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + } + { + const std::string ns = "start_planner.freespace_planner."; + + updateParam(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); + updateParam( + parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm); + updateParam( + parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance); + updateParam( + parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance); + updateParam(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); + updateParam(parameters, ns + "velocity", p->freespace_planner_velocity); + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_planner_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_planner_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_planner_common_parameters.turning_radius_size); + p->freespace_planner_common_parameters.maximum_turning_radius = std::max( + p->freespace_planner_common_parameters.maximum_turning_radius, + p->freespace_planner_common_parameters.minimum_turning_radius); + p->freespace_planner_common_parameters.turning_radius_size = + std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + } + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + + updateParam( + parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_planner_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_planner_common_parameters.longitudinal_goal_range); + } + + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_planner_common_parameters.obstacle_threshold); + } + + { + const std::string ns = "start_planner.freespace_planner.astar."; + + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + { + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string base_ns = "start_planner.path_safety_check."; + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + const std::string obj_filter_ns = base_ns + "target_filtering."; + + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + updateParam( + parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); + updateParam( + parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity", + p->th_moving_obstacle_velocity); + + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->surround_moving_obstacles_type_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->surround_moving_obstacles_type_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->surround_moving_obstacles_type_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->surround_moving_obstacles_type_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->surround_moving_obstacles_type_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->surround_moving_obstacles_type_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->surround_moving_obstacles_type_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->surround_moving_obstacles_type_to_check.check_pedestrian); } + } + + std::string debug_ns = ns + "debug."; + { + updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); + } + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); }