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

…9908) * add time limit for lane change candidate path generation Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * apply time limit for frenet method as well Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * ensure param update value is valid Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix param update initial value Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix spelling Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix param update initial values Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
autowarefoundation · Jan 16, 2025 · e091937 · e091937
1 parent 347a2e4
commit e091937
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 21 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/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                |

diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_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]

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/...ne_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};

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
@@ -190,6 +190,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
   }
 
   // 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 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
 
   {
     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 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       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 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       }
       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 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
 
   {
     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);

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -1154,18 +1154,22 @@ bool NormalLaneChange::get_path_using_frenet(
   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 @@ bool NormalLaneChange::get_path_using_frenet(
       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 @@ bool NormalLaneChange::get_path_using_frenet(
 
   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,6 +1218,7 @@ bool NormalLaneChange::get_path_using_path_shifter(
   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);
@@ -1279,6 +1284,11 @@ bool NormalLaneChange::get_path_using_path_shifter(
       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) {