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>

Author: kyoichi-sugahara

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -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);
   }
 
   {
@@ -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>(
@@ -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>(
@@ -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);
@@ -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;
@@ -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);
   });