Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(lane_change): improve the calculation of the target lateral velocity in Frenet frame #10078

Merged
merged 1 commit into from
Feb 14, 2025
+9 −14
Merged

feat(lane_change): improve the calculation of the target lateral velocity in Frenet frame #10078

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 9 additions & 14 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2024 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.10 to 4.05, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -195,36 +195,31 @@

std::optional<SamplingParameters> init_sampling_parameters(
const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics,
const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose)
const FrenetState & initial_state, const Spline2D & ref_spline)
{
const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory;
const auto min_lc_vel = trajectory.min_lane_changing_velocity;
const auto [min_lateral_acc, max_lateral_acc] =
trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity));
const auto duration = autoware::motion_utils::calc_shift_time_from_jerk(
std::abs(initial_state.position.d), trajectory.lateral_jerk, max_lateral_acc);
const auto final_velocity =
std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration);
const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5;
const auto target_s = initial_state.position.s + lc_length;
const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation);
const auto target_lat_vel =
(1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) *
std::tan(initial_yaw - ref_spline.yaw(target_s));

if (std::isnan(target_lat_vel)) {
return std::nullopt;
}
const auto target_s = std::min(ref_spline.lastS(), initial_state.position.s + lc_length);
// for smooth lateral motion we want a constant lateral acceleration profile
// this means starting from a 0 lateral velocity and setting a positive target lateral velocity
const auto max_lat_vel = duration * max_lateral_acc;

Check warning on line 212 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp#L212 

Added line #L212 was not covered by tests
const auto target_lat_vel = std::min(max_lat_vel, initial_state.position.d / duration);

SamplingParameters sampling_parameters;
const auto & safety = common_data_ptr->lc_param_ptr->safety;
sampling_parameters.resolution = safety.collision_check.prediction_time_resolution;
sampling_parameters.parameters.emplace_back();
sampling_parameters.parameters.back().target_duration = duration;
sampling_parameters.parameters.back().target_state.position = {target_s, 0.0};
// TODO(Maxime): not sure if we should use curvature at initial or target s
sampling_parameters.parameters.back().target_state.lateral_velocity =
sign<double>(common_data_ptr->direction) * target_lat_vel;
std::copysign(target_lat_vel, sign<double>(common_data_ptr->direction));

Check notice on line 222 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments 

init_sampling_parameters is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0;
sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity;
sampling_parameters.parameters.back().target_state.longitudinal_acceleration =
Expand Down Expand Up @@ -571,8 +566,8 @@
initial_state.lateral_velocity, initial_state.longitudinal_acceleration,
initial_state.lateral_acceleration);

const auto sampling_parameters_opt = init_sampling_parameters(
common_data_ptr, metric, initial_state, reference_spline, lc_start_pose);
const auto sampling_parameters_opt =
init_sampling_parameters(common_data_ptr, metric, initial_state, reference_spline);

if (!sampling_parameters_opt) {
continue;
Expand Down
Loading