Skip to content

Commit

Permalink
feat(lane_change_module): add update paramter function for non define…
Browse files Browse the repository at this point in the history 
…d paramters (#9887)

* feat(lane_change_module): add new parameters for collision check and delay lane change functionality

Signed-off-by: Kyoichi Sugahara <[email protected]>

* feat(lane_change_module): add validation for longitudinal and lateral acceleration sampling parameters

Signed-off-by: Kyoichi Sugahara <[email protected]>

* feat(lane_change): update parameter handling and add lateral acceleration mapping

Signed-off-by: Kyoichi Sugahara <[email protected]>

---------

Signed-off-by: Kyoichi Sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara authored Jan 14, 2025
1 parent 36b74c2 commit 9c0e183
Showing 1 changed file with 122 additions and 3 deletions.
125 changes: 122 additions & 3 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -324,10 +324,12 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer);
updateParam<bool>(
parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer);

updateParam<double>(
parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff);
updateParam<bool>(parameters, ns + "publish_debug_marker", p->publish_debug_marker);
updateParam<double>(
parameters, ns + "min_length_for_turn_signal_activation",
p->min_length_for_turn_signal_activation);
}

{
Expand All @@ -338,8 +340,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration);
updateParam<double>(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk);
updateParam<double>(
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<double>(
parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc);
updateParam<double>(
Expand All @@ -352,12 +353,22 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
updateParam<int>(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<int>(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<double>(
Expand All @@ -380,6 +391,57 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
updateParam<double>(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset);
}

{
const std::string ns = "lane_change.lateral_acceleration.";
std::vector<double> velocity = p->trajectory.lat_acc_map.base_vel;
std::vector<double> min_values = p->trajectory.lat_acc_map.base_min_acc;
std::vector<double> max_values = p->trajectory.lat_acc_map.base_max_acc;

updateParam<std::vector<double>>(parameters, ns + "velocity", velocity);
updateParam<std::vector<double>>(parameters, ns + "min_values", min_values);
updateParam<std::vector<double>>(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<bool>(
parameters, ns + "enable_for_prepare_phase.general_lanes",
p->safety.collision_check.enable_for_prepare_phase_in_general_lanes);
updateParam<bool>(
parameters, ns + "enable_for_prepare_phase.intersection",
p->safety.collision_check.enable_for_prepare_phase_in_intersection);
updateParam<bool>(
parameters, ns + "enable_for_prepare_phase.turns",
p->safety.collision_check.enable_for_prepare_phase_in_turns);
updateParam<bool>(
parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane);
updateParam<bool>(
parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes);
updateParam<bool>(
parameters, ns + "use_all_predicted_paths",
p->safety.collision_check.use_all_predicted_paths);
updateParam<double>(
parameters, ns + "prediction_time_resolution",
p->safety.collision_check.prediction_time_resolution);
updateParam<double>(
parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff);
}

{
const std::string ns = "lane_change.target_object.";
updateParam<bool>(parameters, ns + "car", p->safety.target_object_types.check_car);
Expand Down Expand Up @@ -407,6 +469,50 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
updateParam<double>(parameters, ns + "stop_time", p->th_stop_time);
}

auto update_rss_params = [&parameters](const std::string & prefix, auto & params) {
using autoware::universe_utils::updateParam;
updateParam<double>(
parameters, prefix + "longitudinal_distance_min_threshold",
params.longitudinal_distance_min_threshold);
updateParam<double>(
parameters, prefix + "longitudinal_velocity_delta_time",
params.longitudinal_velocity_delta_time);
updateParam<double>(
parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration);
updateParam<double>(
parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration);
updateParam<double>(
parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time);
updateParam<double>(
parameters, prefix + "rear_vehicle_safety_time_margin",
params.rear_vehicle_safety_time_margin);
updateParam<double>(
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<bool>(parameters, ns + "enable", p->delay.enable);
updateParam<bool>(
parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle);
updateParam<double>(
parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width);
updateParam<double>(
parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio);
}

{
const std::string ns = "lane_change.terminal_path.";
updateParam<bool>(parameters, ns + "enable", p->terminal_path.enable);
updateParam<bool>(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal);
updateParam<bool>(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary);
}

{
const std::string ns = "lane_change.cancel.";
bool enable_on_prepare_phase = true;
Expand All @@ -424,13 +530,26 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase;
}

int deceleration_sampling_num = 0;
updateParam<int>(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<double>(parameters, ns + "delta_time", p->cancel.delta_time);
updateParam<double>(parameters, ns + "duration", p->cancel.duration);
updateParam<double>(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk);
updateParam<double>(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance);
updateParam<int>(
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);
});
Expand Down

0 comments on commit 9c0e183

Please sign in to comment.