feat(lane_change): ensure path generation doesn't exceed time limit

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -1039,6 +1039,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 
 | Name                                         | Unit   | Type   | Description                                                                                                            | Default value      |
 | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ |
+| `time_limit`                                 | [ms]   | double | Time limit for lane change candidate path generation                                                                   | 50.0               |
 | `backward_lane_length`                       | [m]    | double | The backward length to check incoming objects in lane change target lane.                                              | 200.0              |
 | `backward_length_buffer_for_end_of_lane`     | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     lane_change:
+      time_limit: 50.0 # [ms]
       backward_lane_length: 200.0
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       backward_length_buffer_for_blocking_object: 3.0 # [m]

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp

@@ -162,6 +162,7 @@ struct Parameters
   FrenetPlannerParameters frenet{};
 
   // lane change parameters
+  double time_limit{50.0};
   double backward_lane_length{200.0};
   double backward_length_buffer_for_end_of_lane{0.0};
   double backward_length_buffer_for_blocking_object{0.0};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -190,6 +190,7 @@
   }
 
   // lane change parameters
+  p.time_limit = getOrDeclareParameter<double>(*node, parameter("time_limit"));
   p.backward_lane_length = getOrDeclareParameter<double>(*node, parameter("backward_lane_length"));
   p.backward_length_buffer_for_end_of_lane =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
@@ -313,6 +314,17 @@
 
   {
     const std::string ns = "lane_change.";
+    auto time_limit = p->time_limit;
+    updateParam<double>(parameters, ns + "time_limit", time_limit);
+    if (time_limit >= 10.0) {
+      p->time_limit = time_limit;
+    } else {
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, "
+        "keep current value (%.3f ms)",
+        time_limit, p->time_limit);
+    }
     updateParam<double>(parameters, ns + "backward_lane_length", p->backward_lane_length);
     updateParam<double>(
       parameters, ns + "backward_length_buffer_for_end_of_lane",
@@ -349,25 +361,27 @@
       parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor);
     updateParam<double>(
       parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature);
-    int longitudinal_acc_sampling_num = 0;
+    int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num;
     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",
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "WARNING! 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;
+    int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num;
     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",
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not "
+        "positive",
         lateral_acc_sampling_num);
     }
 
@@ -409,8 +423,8 @@
       }
       p->trajectory.lat_acc_map = lat_acc_map;
     } else {
-      RCLCPP_WARN_ONCE(
-        node_->get_logger(),
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
         "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());
@@ -515,28 +529,32 @@
 
   {
     const std::string ns = "lane_change.cancel.";
-    bool enable_on_prepare_phase = true;
+    bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase;
     updateParam<bool>(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase);
     if (!enable_on_prepare_phase) {
-      RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled.");
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "WARNING! Lane Change cancel function is disabled.");
       p->cancel.enable_on_prepare_phase = enable_on_prepare_phase;
     }
 
-    bool enable_on_lane_changing_phase = true;
+    bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase;
     updateParam<bool>(
       parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase);
     if (!enable_on_lane_changing_phase) {
-      RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled.");
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "WARNING! Lane Change abort function is disabled.");
       p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase;
     }
 
-    int deceleration_sampling_num = 0;
+    int deceleration_sampling_num = p->cancel.deceleration_sampling_num;
     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(),
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
         "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not "
         "positive",
         deceleration_sampling_num);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1154,18 +1154,22 @@
   const std::vector<std::vector<int64_t>> & sorted_lane_ids,
   LaneChangePaths & candidate_paths) const
 {
-  stop_watch_.tic("frenet_candidates");
+  stop_watch_.tic(__func__);
   constexpr auto found_safe_path = true;
   const auto frenet_candidates = utils::lane_change::generate_frenet_candidates(
     common_data_ptr_, prev_module_output_.path, prepare_metrics);
   RCLCPP_DEBUG(
     logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(),
-    stop_watch_.toc("frenet_candidates"));
+    stop_watch_.toc(__func__));
 
   candidate_paths.reserve(frenet_candidates.size());
   lane_change_debug_.frenet_states.clear();
   lane_change_debug_.frenet_states.reserve(frenet_candidates.size());
   for (const auto & frenet_candidate : frenet_candidates) {
+    if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) {
+      break;
+    }
+
     lane_change_debug_.frenet_states.emplace_back(
       frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter,
       frenet_candidate.max_lane_changing_length);
@@ -1186,7 +1190,7 @@
       if (check_candidate_path_safety(*candidate_path_opt, target_objects)) {
         RCLCPP_DEBUG(
           logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]",
-          frenet_candidates.size(), stop_watch_.toc("frenet_candidates"));
+          frenet_candidates.size(), stop_watch_.toc("__func__"));
         utils::lane_change::append_target_ref_to_candidate(
           *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing);
         candidate_paths.push_back(*candidate_path_opt);
@@ -1204,7 +1208,7 @@
 
   RCLCPP_DEBUG(
     logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(),
-    stop_watch_.toc("frenet_candidates"));
+    stop_watch_.toc(__func__));
   return !found_safe_path;
 }
 
@@ -1214,71 +1218,77 @@
   const std::vector<std::vector<int64_t>> & sorted_lane_ids,
   LaneChangePaths & candidate_paths) const
 {
+  stop_watch_.tic(__func__);
   const auto & target_lanes = get_target_lanes();
   candidate_paths.reserve(
     prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num);
 
   const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time;
   const auto dist_to_next_regulatory_element =
     utils::lane_change::get_distance_to_next_regulatory_element(common_data_ptr_, only_tl, only_tl);
 
   auto check_length_diff =
     [&](const double prep_length, const double lc_length, const bool check_lc) {
       if (candidate_paths.empty()) return true;
 
       const auto prep_diff = std::abs(candidate_paths.back().info.length.prepare - prep_length);
       if (prep_diff > lane_change_parameters_->trajectory.th_prepare_length_diff) return true;
 
       if (!check_lc) return false;
 
       const auto lc_diff = std::abs(candidate_paths.back().info.length.lane_changing - lc_length);
       return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff;
     };
 
   for (const auto & prep_metric : prepare_metrics) {
     const auto debug_print = [&](const std::string & s) {
       RCLCPP_DEBUG(
         logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(),
         prep_metric.duration, prep_metric.actual_lon_accel, prep_metric.length);
     };
 
     if (!check_length_diff(prep_metric.length, 0.0, false)) {
       RCLCPP_DEBUG(logger_, "Skip: Change in prepare length is less than threshold.");
       continue;
     }
 
     PathWithLaneId prepare_segment;
     try {
       if (!utils::lane_change::get_prepare_segment(
             common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) {
         debug_print("Reject: failed to get valid prepare segment!");
         continue;
       }
     } catch (const std::exception & e) {
       debug_print(e.what());
       break;
     }
 
     debug_print("Prepare path satisfy constraints");
 
     const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
 
     const auto shift_length =
       lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
 
     lane_change_debug_.lane_change_metrics.emplace_back();
     auto & debug_metrics = lane_change_debug_.lane_change_metrics.back();
     debug_metrics.prep_metric = prep_metric;
     debug_metrics.max_prepare_length = common_data_ptr_->transient_data.dist_to_terminal_start;
     const auto lane_changing_metrics = get_lane_changing_metrics(
       prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element, debug_metrics);
 
     // set_prepare_velocity must only be called after computing lane change metrics, as lane change
     // metrics rely on the prepare segment's original velocity as max_path_velocity.
     utils::lane_change::set_prepare_velocity(
       prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity);
 
     for (const auto & lc_metric : lane_changing_metrics) {
+      if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) {
+        RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found.");
+        return false;
+      }
+
       debug_metrics.lc_metrics.emplace_back(lc_metric, -1);
 
       const auto debug_print_lat = [&](const std::string & s) {