Skip to content

Commit 3253f79

Browse files
fix some conflicts
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 08faa0b commit 3253f79

File tree

1 file changed

+6
-1
lines changed
  • planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src

1 file changed

+6
-1
lines changed

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -1249,7 +1249,7 @@ bool NormalLaneChange::get_path_using_path_shifter(
12491249
PathWithLaneId prepare_segment;
12501250
try {
12511251
if (!utils::lane_change::get_prepare_segment(
1252-
common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) {
1252+
common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) {
12531253
debug_print("Reject: failed to get valid prepare segment!");
12541254
continue;
12551255
}
@@ -1268,6 +1268,11 @@ bool NormalLaneChange::get_path_using_path_shifter(
12681268
const auto lane_changing_metrics = get_lane_changing_metrics(
12691269
prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element);
12701270

1271+
// set_prepare_velocity must only be called after computing lane change metrics, as lane change
1272+
// metrics rely on the prepare segment's original velocity as max_path_velocity.
1273+
utils::lane_change::set_prepare_velocity(
1274+
prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity);
1275+
12711276
for (const auto & lc_metric : lane_changing_metrics) {
12721277
const auto debug_print_lat = [&](const std::string & s) {
12731278
RCLCPP_DEBUG(

0 commit comments

Comments
 (0)