diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 06a9d505f90ad..07b05ab0ea131 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -324,10 +324,12 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorlane_change_finish_judge_buffer); updateParam( parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); - updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam( + parameters, ns + "min_length_for_turn_signal_activation", + p->min_length_for_turn_signal_activation); } { @@ -338,8 +340,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( - parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); - // longitudinal acceleration + parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); updateParam( parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); updateParam( @@ -352,12 +353,22 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; + } else { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not positive", + longitudinal_acc_sampling_num); } int lateral_acc_sampling_num = 0; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; + } else { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not positive", + lateral_acc_sampling_num); } updateParam( @@ -380,6 +391,57 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } + { + const std::string ns = "lane_change.lateral_acceleration."; + std::vector velocity = p->trajectory.lat_acc_map.base_vel; + std::vector min_values = p->trajectory.lat_acc_map.base_min_acc; + std::vector max_values = p->trajectory.lat_acc_map.base_max_acc; + + updateParam>(parameters, ns + "velocity", velocity); + updateParam>(parameters, ns + "min_values", min_values); + updateParam>(parameters, ns + "max_values", max_values); + if ( + velocity.size() >= 2 && velocity.size() == min_values.size() && + velocity.size() == max_values.size()) { + LateralAccelerationMap lat_acc_map; + for (size_t i = 0; i < velocity.size(); ++i) { + lat_acc_map.add(velocity.at(i), min_values.at(i), max_values.at(i)); + } + p->trajectory.lat_acc_map = lat_acc_map; + } else { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " + "min_values: %lu, max_values: %lu", + std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); + } + } + + { + const std::string ns = "lane_change.collision_check."; + updateParam( + parameters, ns + "enable_for_prepare_phase.general_lanes", + p->safety.collision_check.enable_for_prepare_phase_in_general_lanes); + updateParam( + parameters, ns + "enable_for_prepare_phase.intersection", + p->safety.collision_check.enable_for_prepare_phase_in_intersection); + updateParam( + parameters, ns + "enable_for_prepare_phase.turns", + p->safety.collision_check.enable_for_prepare_phase_in_turns); + updateParam( + parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane); + updateParam( + parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes); + updateParam( + parameters, ns + "use_all_predicted_paths", + p->safety.collision_check.use_all_predicted_paths); + updateParam( + parameters, ns + "prediction_time_resolution", + p->safety.collision_check.prediction_time_resolution); + updateParam( + parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + } + { const std::string ns = "lane_change.target_object."; updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); @@ -407,6 +469,50 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } + auto update_rss_params = [¶meters](const std::string & prefix, auto & params) { + using autoware::universe_utils::updateParam; + updateParam( + parameters, prefix + "longitudinal_distance_min_threshold", + params.longitudinal_distance_min_threshold); + updateParam( + parameters, prefix + "longitudinal_velocity_delta_time", + params.longitudinal_velocity_delta_time); + updateParam( + parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration); + updateParam( + parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration); + updateParam( + parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time); + updateParam( + parameters, prefix + "rear_vehicle_safety_time_margin", + params.rear_vehicle_safety_time_margin); + updateParam( + parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + }; + + update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); + update_rss_params("lane_change.safety_check.parked.", p->safety.rss_params_for_parked); + update_rss_params("lane_change.safety_check.cancel.", p->safety.rss_params_for_abort); + update_rss_params("lane_change.safety_check.stuck.", p->safety.rss_params_for_stuck); + + { + const std::string ns = "lane_change.delay_lane_change."; + updateParam(parameters, ns + "enable", p->delay.enable); + updateParam( + parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle); + updateParam( + parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width); + updateParam( + parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio); + } + + { + const std::string ns = "lane_change.terminal_path."; + updateParam(parameters, ns + "enable", p->terminal_path.enable); + updateParam(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); + updateParam(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); + } + { const std::string ns = "lane_change.cancel."; bool enable_on_prepare_phase = true; @@ -424,6 +530,18 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorcancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } + int deceleration_sampling_num = 0; + updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); + if (deceleration_sampling_num > 0) { + p->cancel.deceleration_sampling_num = deceleration_sampling_num; + } else { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " + "positive", + deceleration_sampling_num); + } + updateParam(parameters, ns + "delta_time", p->cancel.delta_time); updateParam(parameters, ns + "duration", p->cancel.duration); updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); @@ -431,6 +549,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); });