Skip to content

Commit 9c0e183

Browse files
feat(lane_change_module): add update paramter function for non defined paramters (autowarefoundation#9887)
* feat(lane_change_module): add new parameters for collision check and delay lane change functionality Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * feat(lane_change_module): add validation for longitudinal and lateral acceleration sampling parameters Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * feat(lane_change): update parameter handling and add lateral acceleration mapping Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
1 parent 36b74c2 commit 9c0e183

File tree

1 file changed

+122
-3
lines changed
  • planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src

1 file changed

+122
-3
lines changed

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

+122-3
Original file line numberDiff line numberDiff line change
@@ -324,10 +324,12 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
324324
parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer);
325325
updateParam<bool>(
326326
parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer);
327-
328327
updateParam<double>(
329328
parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff);
330329
updateParam<bool>(parameters, ns + "publish_debug_marker", p->publish_debug_marker);
330+
updateParam<double>(
331+
parameters, ns + "min_length_for_turn_signal_activation",
332+
p->min_length_for_turn_signal_activation);
331333
}
332334

333335
{
@@ -338,8 +340,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
338340
parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration);
339341
updateParam<double>(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk);
340342
updateParam<double>(
341-
parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity);
342-
// longitudinal acceleration
343+
parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity);
343344
updateParam<double>(
344345
parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc);
345346
updateParam<double>(
@@ -352,12 +353,22 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
352353
updateParam<int>(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num);
353354
if (longitudinal_acc_sampling_num > 0) {
354355
p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num;
356+
} else {
357+
RCLCPP_WARN_ONCE(
358+
node_->get_logger(),
359+
"Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not positive",
360+
longitudinal_acc_sampling_num);
355361
}
356362

357363
int lateral_acc_sampling_num = 0;
358364
updateParam<int>(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num);
359365
if (lateral_acc_sampling_num > 0) {
360366
p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num;
367+
} else {
368+
RCLCPP_WARN_ONCE(
369+
node_->get_logger(),
370+
"Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not positive",
371+
lateral_acc_sampling_num);
361372
}
362373

363374
updateParam<double>(
@@ -380,6 +391,57 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
380391
updateParam<double>(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset);
381392
}
382393

394+
{
395+
const std::string ns = "lane_change.lateral_acceleration.";
396+
std::vector<double> velocity = p->trajectory.lat_acc_map.base_vel;
397+
std::vector<double> min_values = p->trajectory.lat_acc_map.base_min_acc;
398+
std::vector<double> max_values = p->trajectory.lat_acc_map.base_max_acc;
399+
400+
updateParam<std::vector<double>>(parameters, ns + "velocity", velocity);
401+
updateParam<std::vector<double>>(parameters, ns + "min_values", min_values);
402+
updateParam<std::vector<double>>(parameters, ns + "max_values", max_values);
403+
if (
404+
velocity.size() >= 2 && velocity.size() == min_values.size() &&
405+
velocity.size() == max_values.size()) {
406+
LateralAccelerationMap lat_acc_map;
407+
for (size_t i = 0; i < velocity.size(); ++i) {
408+
lat_acc_map.add(velocity.at(i), min_values.at(i), max_values.at(i));
409+
}
410+
p->trajectory.lat_acc_map = lat_acc_map;
411+
} else {
412+
RCLCPP_WARN_ONCE(
413+
node_->get_logger(),
414+
"Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, "
415+
"min_values: %lu, max_values: %lu",
416+
std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size());
417+
}
418+
}
419+
420+
{
421+
const std::string ns = "lane_change.collision_check.";
422+
updateParam<bool>(
423+
parameters, ns + "enable_for_prepare_phase.general_lanes",
424+
p->safety.collision_check.enable_for_prepare_phase_in_general_lanes);
425+
updateParam<bool>(
426+
parameters, ns + "enable_for_prepare_phase.intersection",
427+
p->safety.collision_check.enable_for_prepare_phase_in_intersection);
428+
updateParam<bool>(
429+
parameters, ns + "enable_for_prepare_phase.turns",
430+
p->safety.collision_check.enable_for_prepare_phase_in_turns);
431+
updateParam<bool>(
432+
parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane);
433+
updateParam<bool>(
434+
parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes);
435+
updateParam<bool>(
436+
parameters, ns + "use_all_predicted_paths",
437+
p->safety.collision_check.use_all_predicted_paths);
438+
updateParam<double>(
439+
parameters, ns + "prediction_time_resolution",
440+
p->safety.collision_check.prediction_time_resolution);
441+
updateParam<double>(
442+
parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff);
443+
}
444+
383445
{
384446
const std::string ns = "lane_change.target_object.";
385447
updateParam<bool>(parameters, ns + "car", p->safety.target_object_types.check_car);
@@ -407,6 +469,50 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
407469
updateParam<double>(parameters, ns + "stop_time", p->th_stop_time);
408470
}
409471

472+
auto update_rss_params = [&parameters](const std::string & prefix, auto & params) {
473+
using autoware::universe_utils::updateParam;
474+
updateParam<double>(
475+
parameters, prefix + "longitudinal_distance_min_threshold",
476+
params.longitudinal_distance_min_threshold);
477+
updateParam<double>(
478+
parameters, prefix + "longitudinal_velocity_delta_time",
479+
params.longitudinal_velocity_delta_time);
480+
updateParam<double>(
481+
parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration);
482+
updateParam<double>(
483+
parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration);
484+
updateParam<double>(
485+
parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time);
486+
updateParam<double>(
487+
parameters, prefix + "rear_vehicle_safety_time_margin",
488+
params.rear_vehicle_safety_time_margin);
489+
updateParam<double>(
490+
parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold);
491+
};
492+
493+
update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params);
494+
update_rss_params("lane_change.safety_check.parked.", p->safety.rss_params_for_parked);
495+
update_rss_params("lane_change.safety_check.cancel.", p->safety.rss_params_for_abort);
496+
update_rss_params("lane_change.safety_check.stuck.", p->safety.rss_params_for_stuck);
497+
498+
{
499+
const std::string ns = "lane_change.delay_lane_change.";
500+
updateParam<bool>(parameters, ns + "enable", p->delay.enable);
501+
updateParam<bool>(
502+
parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle);
503+
updateParam<double>(
504+
parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width);
505+
updateParam<double>(
506+
parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio);
507+
}
508+
509+
{
510+
const std::string ns = "lane_change.terminal_path.";
511+
updateParam<bool>(parameters, ns + "enable", p->terminal_path.enable);
512+
updateParam<bool>(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal);
513+
updateParam<bool>(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary);
514+
}
515+
410516
{
411517
const std::string ns = "lane_change.cancel.";
412518
bool enable_on_prepare_phase = true;
@@ -424,13 +530,26 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
424530
p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase;
425531
}
426532

533+
int deceleration_sampling_num = 0;
534+
updateParam<int>(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num);
535+
if (deceleration_sampling_num > 0) {
536+
p->cancel.deceleration_sampling_num = deceleration_sampling_num;
537+
} else {
538+
RCLCPP_WARN_ONCE(
539+
node_->get_logger(),
540+
"Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not "
541+
"positive",
542+
deceleration_sampling_num);
543+
}
544+
427545
updateParam<double>(parameters, ns + "delta_time", p->cancel.delta_time);
428546
updateParam<double>(parameters, ns + "duration", p->cancel.duration);
429547
updateParam<double>(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk);
430548
updateParam<double>(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance);
431549
updateParam<int>(
432550
parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis);
433551
}
552+
434553
std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {
435554
if (!observer.expired()) observer.lock()->updateModuleParams(p);
436555
});

0 commit comments

Comments
 (0)