Skip to content

Commit

Permalink
fix(lane_change): cap ego's predicted path velocity (RT1-8505) (autow…
Browse files Browse the repository at this point in the history 
…arefoundation#9341)

* fix(lane_change): cap ego's predicted path velocity (RT1-8505)

Signed-off-by: Zulfaqar Azmi <[email protected]>

* properly cap based on 0.0 instead of min lc vel

Signed-off-by: Zulfaqar Azmi <[email protected]>

* fix build error

Signed-off-by: Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Dec 19, 2024
1 parent a676acd commit b9d569e
Show file tree
Hide file tree
Showing 3 changed files with 32 additions and 37 deletions.
7 changes: 3 additions & 4 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,10 +136,9 @@ std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const LaneChangeModuleType type, const Direction & direction);

std::vector<PoseWithVelocityStamped> convertToPredictedPath(
const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose,
const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters, const double resolution);
std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
const double lane_changing_acceleration);

bool isParkedObject(
const PathWithLaneId & path, const RouteHandler & route_handler,
Expand Down
11 changes: 2 additions & 9 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2171,8 +2171,6 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
return false;
}

const auto current_pose = common_data_ptr_->get_ego_pose();
const auto current_twist = common_data_ptr_->get_ego_twist();
const auto bpp_param = *common_data_ptr_->bpp_param_ptr;
const auto global_min_acc = bpp_param.min_acc;
const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
Expand All @@ -2189,15 +2187,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(),
[&](double n) { return lane_changing_acc + n * acc_resolution; });

const auto time_resolution = lane_change_parameters_->prediction_time_resolution;

const auto all_collided = std::all_of(
acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) {
const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
lane_change_path, current_twist, current_pose, acceleration, bpp_param,
*lane_change_parameters_, time_resolution);
const auto debug_predicted_path =
utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path(
common_data_ptr_, lane_change_path, acceleration);

return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) {
const auto selected_rss_param =
Expand Down
51 changes: 27 additions & 24 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -661,52 +661,55 @@ std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction);
}

std::vector<PoseWithVelocityStamped> convertToPredictedPath(
const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose,
const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters, const double resolution)
std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
const double lane_changing_acceleration)
{
if (lane_change_path.path.points.empty()) {
return {};
}

const auto & path = lane_change_path.path;
const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare;
const auto duration = lane_change_path.info.duration.sum();
const auto prepare_time = lane_change_path.info.duration.prepare;
const auto & minimum_lane_changing_velocity =
lane_change_parameters.minimum_lane_changing_velocity;

const auto & vehicle_pose = common_data_ptr->get_ego_pose();
const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr;
const auto nearest_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold,
common_parameters.ego_nearest_yaw_threshold);
path.points, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold,
bpp_param_ptr->ego_nearest_yaw_threshold);

std::vector<PoseWithVelocityStamped> predicted_path;
const auto vehicle_pose_frenet =
convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx);
const double initial_velocity = std::abs(vehicle_twist.linear.x);

const auto initial_velocity = common_data_ptr->get_ego_speed();
const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare;
const auto duration = lane_change_path.info.duration.sum();
const auto prepare_time = lane_change_path.info.duration.prepare;
const auto & lc_param_ptr = common_data_ptr->lc_param_ptr;
const auto resolution = lc_param_ptr->prediction_time_resolution;
std::vector<PoseWithVelocityStamped> predicted_path;

// prepare segment
for (double t = 0.0; t < prepare_time; t += resolution) {
const double velocity =
std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity);
const double length = initial_velocity * t + 0.5 * prepare_acc * t * t;
const auto velocity =
std::clamp(initial_velocity + prepare_acc * t, 0.0, lane_change_path.info.velocity.prepare);
const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t;
const auto pose = autoware::motion_utils::calcInterpolatedPose(
path.points, vehicle_pose_frenet.length + length);
predicted_path.emplace_back(t, pose, velocity);
}

// lane changing segment
const double lane_changing_velocity =
std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity);
const double offset =
const auto lane_changing_velocity = std::clamp(
initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare);
const auto offset =
initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time;
for (double t = prepare_time; t < duration; t += resolution) {
const double delta_t = t - prepare_time;
const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t;
const double length = lane_changing_velocity * delta_t +
0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
const auto delta_t = t - prepare_time;
const auto velocity = std::clamp(
lane_changing_velocity + lane_changing_acceleration * delta_t, 0.0,
lane_change_path.info.velocity.lane_changing);
const auto length = lane_changing_velocity * delta_t +
0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
const auto pose = autoware::motion_utils::calcInterpolatedPose(
path.points, vehicle_pose_frenet.length + length);
predicted_path.emplace_back(t, pose, velocity);
Expand Down

0 comments on commit b9d569e

Please sign in to comment.