Skip to content

Commit e091937

Browse files
mkqudamkquda
authored
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>
1 parent 347a2e4 commit e091937

File tree

5 files changed

+52
-21
lines changed

5 files changed

+52
-21
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -1039,6 +1039,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
10391039

10401040
| Name | Unit | Type | Description | Default value |
10411041
| :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ |
1042+
| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 |
10421043
| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 |
10431044
| `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 |
10441045
| `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
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
/**:
22
ros__parameters:
33
lane_change:
4+
time_limit: 50.0 # [ms]
45
backward_lane_length: 200.0
56
backward_length_buffer_for_end_of_lane: 3.0 # [m]
67
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

+1
Original file line numberDiff line numberDiff line change
@@ -162,6 +162,7 @@ struct Parameters
162162
FrenetPlannerParameters frenet{};
163163

164164
// lane change parameters
165+
double time_limit{50.0};
165166
double backward_lane_length{200.0};
166167
double backward_length_buffer_for_end_of_lane{0.0};
167168
double backward_length_buffer_for_blocking_object{0.0};

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

+35-17
Original file line numberDiff line numberDiff line change
@@ -190,6 +190,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
190190
}
191191

192192
// lane change parameters
193+
p.time_limit = getOrDeclareParameter<double>(*node, parameter("time_limit"));
193194
p.backward_lane_length = getOrDeclareParameter<double>(*node, parameter("backward_lane_length"));
194195
p.backward_length_buffer_for_end_of_lane =
195196
getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
@@ -313,6 +314,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
313314

314315
{
315316
const std::string ns = "lane_change.";
317+
auto time_limit = p->time_limit;
318+
updateParam<double>(parameters, ns + "time_limit", time_limit);
319+
if (time_limit >= 10.0) {
320+
p->time_limit = time_limit;
321+
} else {
322+
RCLCPP_WARN_THROTTLE(
323+
node_->get_logger(), *node_->get_clock(), 1000,
324+
"WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, "
325+
"keep current value (%.3f ms)",
326+
time_limit, p->time_limit);
327+
}
316328
updateParam<double>(parameters, ns + "backward_lane_length", p->backward_lane_length);
317329
updateParam<double>(
318330
parameters, ns + "backward_length_buffer_for_end_of_lane",
@@ -349,25 +361,27 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
349361
parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor);
350362
updateParam<double>(
351363
parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature);
352-
int longitudinal_acc_sampling_num = 0;
364+
int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num;
353365
updateParam<int>(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num);
354366
if (longitudinal_acc_sampling_num > 0) {
355367
p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num;
356368
} 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",
369+
RCLCPP_WARN_THROTTLE(
370+
node_->get_logger(), *node_->get_clock(), 1000,
371+
"WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not "
372+
"positive",
360373
longitudinal_acc_sampling_num);
361374
}
362375

363-
int lateral_acc_sampling_num = 0;
376+
int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num;
364377
updateParam<int>(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num);
365378
if (lateral_acc_sampling_num > 0) {
366379
p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num;
367380
} 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",
381+
RCLCPP_WARN_THROTTLE(
382+
node_->get_logger(), *node_->get_clock(), 1000,
383+
"WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not "
384+
"positive",
371385
lateral_acc_sampling_num);
372386
}
373387

@@ -409,8 +423,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
409423
}
410424
p->trajectory.lat_acc_map = lat_acc_map;
411425
} else {
412-
RCLCPP_WARN_ONCE(
413-
node_->get_logger(),
426+
RCLCPP_WARN_THROTTLE(
427+
node_->get_logger(), *node_->get_clock(), 1000,
414428
"Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, "
415429
"min_values: %lu, max_values: %lu",
416430
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
515529

516530
{
517531
const std::string ns = "lane_change.cancel.";
518-
bool enable_on_prepare_phase = true;
532+
bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase;
519533
updateParam<bool>(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase);
520534
if (!enable_on_prepare_phase) {
521-
RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled.");
535+
RCLCPP_WARN_THROTTLE(
536+
node_->get_logger(), *node_->get_clock(), 1000,
537+
"WARNING! Lane Change cancel function is disabled.");
522538
p->cancel.enable_on_prepare_phase = enable_on_prepare_phase;
523539
}
524540

525-
bool enable_on_lane_changing_phase = true;
541+
bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase;
526542
updateParam<bool>(
527543
parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase);
528544
if (!enable_on_lane_changing_phase) {
529-
RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled.");
545+
RCLCPP_WARN_THROTTLE(
546+
node_->get_logger(), *node_->get_clock(), 1000,
547+
"WARNING! Lane Change abort function is disabled.");
530548
p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase;
531549
}
532550

533-
int deceleration_sampling_num = 0;
551+
int deceleration_sampling_num = p->cancel.deceleration_sampling_num;
534552
updateParam<int>(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num);
535553
if (deceleration_sampling_num > 0) {
536554
p->cancel.deceleration_sampling_num = deceleration_sampling_num;
537555
} else {
538-
RCLCPP_WARN_ONCE(
539-
node_->get_logger(),
556+
RCLCPP_WARN_THROTTLE(
557+
node_->get_logger(), *node_->get_clock(), 1000,
540558
"Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not "
541559
"positive",
542560
deceleration_sampling_num);

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

+14-4
Original file line numberDiff line numberDiff line change
@@ -1154,18 +1154,22 @@ bool NormalLaneChange::get_path_using_frenet(
11541154
const std::vector<std::vector<int64_t>> & sorted_lane_ids,
11551155
LaneChangePaths & candidate_paths) const
11561156
{
1157-
stop_watch_.tic("frenet_candidates");
1157+
stop_watch_.tic(__func__);
11581158
constexpr auto found_safe_path = true;
11591159
const auto frenet_candidates = utils::lane_change::generate_frenet_candidates(
11601160
common_data_ptr_, prev_module_output_.path, prepare_metrics);
11611161
RCLCPP_DEBUG(
11621162
logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(),
1163-
stop_watch_.toc("frenet_candidates"));
1163+
stop_watch_.toc(__func__));
11641164

11651165
candidate_paths.reserve(frenet_candidates.size());
11661166
lane_change_debug_.frenet_states.clear();
11671167
lane_change_debug_.frenet_states.reserve(frenet_candidates.size());
11681168
for (const auto & frenet_candidate : frenet_candidates) {
1169+
if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) {
1170+
break;
1171+
}
1172+
11691173
lane_change_debug_.frenet_states.emplace_back(
11701174
frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter,
11711175
frenet_candidate.max_lane_changing_length);
@@ -1186,7 +1190,7 @@ bool NormalLaneChange::get_path_using_frenet(
11861190
if (check_candidate_path_safety(*candidate_path_opt, target_objects)) {
11871191
RCLCPP_DEBUG(
11881192
logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]",
1189-
frenet_candidates.size(), stop_watch_.toc("frenet_candidates"));
1193+
frenet_candidates.size(), stop_watch_.toc("__func__"));
11901194
utils::lane_change::append_target_ref_to_candidate(
11911195
*candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing);
11921196
candidate_paths.push_back(*candidate_path_opt);
@@ -1204,7 +1208,7 @@ bool NormalLaneChange::get_path_using_frenet(
12041208

12051209
RCLCPP_DEBUG(
12061210
logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(),
1207-
stop_watch_.toc("frenet_candidates"));
1211+
stop_watch_.toc(__func__));
12081212
return !found_safe_path;
12091213
}
12101214

@@ -1214,6 +1218,7 @@ bool NormalLaneChange::get_path_using_path_shifter(
12141218
const std::vector<std::vector<int64_t>> & sorted_lane_ids,
12151219
LaneChangePaths & candidate_paths) const
12161220
{
1221+
stop_watch_.tic(__func__);
12171222
const auto & target_lanes = get_target_lanes();
12181223
candidate_paths.reserve(
12191224
prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num);
@@ -1279,6 +1284,11 @@ bool NormalLaneChange::get_path_using_path_shifter(
12791284
prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity);
12801285

12811286
for (const auto & lc_metric : lane_changing_metrics) {
1287+
if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) {
1288+
RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found.");
1289+
return false;
1290+
}
1291+
12821292
debug_metrics.lc_metrics.emplace_back(lc_metric, -1);
12831293

12841294
const auto debug_print_lat = [&](const std::string & s) {

0 commit comments

Comments
 (0)