Skip to content

Commit

Permalink
refactor(lane_change): refactor longitudinal acceleration sampling (#…
Browse files Browse the repository at this point in the history 
…9091)

* fix calc_all_max_lc_lengths function

Signed-off-by: mohammad alqudah <[email protected]>

* remove unused functions

Signed-off-by: mohammad alqudah <[email protected]>

* remove limit on velocity in calc_all_max_lc_lengths function

Signed-off-by: mohammad alqudah <[email protected]>

* sample longitudinal acceleration separately for each prepater duration

Signed-off-by: mohammad alqudah <[email protected]>

* refactor prepare phase metrics calculation

Signed-off-by: mohammad alqudah <[email protected]>

* check for zero value prepare duration

Signed-off-by: mohammad alqudah <[email protected]>

* refactor calc_lon_acceleration_samples function

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
  • Loading branch information
@mkquda
mkquda authored Oct 23, 2024
1 parent 41d6312 commit be12c10
Show file tree
Hide file tree
Showing 7 changed files with 179 additions and 284 deletions.
11 changes: 0 additions & 11 deletions ...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,12 +120,6 @@ class NormalLaneChange : public LaneChangeBase

TurnSignalInfo get_terminal_turn_signal_info() const final;

std::vector<double> sampleLongitudinalAccValues(
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;

std::vector<double> calc_prepare_durations() const;

lane_change::TargetObjects get_target_objects(
const FilteredByLanesExtendedObjects & filtered_objects,
const lanelet::ConstLanelets & current_lanes) const;
Expand Down Expand Up @@ -206,11 +200,6 @@ class NormalLaneChange : public LaneChangeBase

bool check_prepare_phase() const;

double calcMaximumLaneChangeLength(
const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;

std::pair<double, double> calcCurrentMinMaxAcceleration() const;

void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);

void updateStopTime();
Expand Down
19 changes: 8 additions & 11 deletions ...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ using behavior_path_planner::lane_change::LCParamPtr;
using behavior_path_planner::lane_change::MinMaxValue;
using behavior_path_planner::lane_change::PhaseMetrics;

static constexpr double eps = 0.001;

double calc_dist_from_pose_to_terminal_end(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
const Pose & src_pose);
Expand Down Expand Up @@ -103,14 +105,6 @@ double calc_ego_dist_to_lanes_start(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);

double calc_maximum_lane_change_length(
const double current_velocity, const LaneChangeParameters & lane_change_parameters,
const std::vector<double> & shift_intervals, const double max_acc);

double calc_maximum_lane_change_length(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
const double max_acc);

double calc_lane_changing_acceleration(
const double initial_lane_changing_velocity, const double max_path_velocity,
const double lane_changing_time, const double prepare_longitudinal_acc);
Expand All @@ -132,10 +126,13 @@ double calc_phase_length(
const double velocity, const double maximum_velocity, const double acceleration,
const double duration);

std::vector<double> calc_lon_acceleration_samples(
const CommonDataPtr & common_data_ptr, const double max_path_velocity,
const double prepare_duration);

std::vector<PhaseMetrics> calc_prepare_phase_metrics(
const CommonDataPtr & common_data_ptr, const std::vector<double> & prepare_durations,
const std::vector<double> & lon_accel_values, const double current_velocity,
const double min_length_threshold = 0.0,
const CommonDataPtr & common_data_ptr, const double current_velocity,
const double max_path_velocity, const double min_length_threshold = 0.0,
const double max_length_threshold = std::numeric_limits<double>::max());

std::vector<PhaseMetrics> calc_shift_phase_metrics(
Expand Down
4 changes: 3 additions & 1 deletion ...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -323,7 +323,6 @@ struct MinMaxValue

struct TransientData
{
MinMaxValue acc; // acceleration profile for accelerating lane change path
MinMaxValue lane_changing_length; // lane changing length for a single lane change
MinMaxValue
current_dist_buffer; // distance buffer computed backward from current lanes' terminal end
Expand All @@ -343,6 +342,9 @@ struct TransientData
lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes
lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes

size_t current_path_seg_idx; // index of nearest segment to ego along current path
double current_path_velocity; // velocity of the current path at the ego position along the path

bool is_ego_near_current_terminal_start{false};
bool is_ego_stuck{false};
};
Expand Down
11 changes: 0 additions & 11 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 @@ -64,20 +64,9 @@ bool is_mandatory_lane_change(const ModuleType lc_type);
double calcLaneChangeResampleInterval(
const double lane_changing_length, const double lane_changing_velocity);

double calcMinimumAcceleration(
const double current_velocity, const double min_longitudinal_acc,
const LaneChangeParameters & lane_change_parameters);

double calcMaximumAcceleration(
const double current_velocity, const double current_max_velocity,
const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters);

void setPrepareVelocity(
PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);

std::vector<double> getAccelerationValues(
const double min_acc, const double max_acc, const size_t sampling_num);

std::vector<int64_t> replaceWithSortedIds(
const std::vector<int64_t> & original_lane_ids,
const std::vector<std::vector<int64_t>> & sorted_lane_ids);
Expand Down
145 changes: 17 additions & 128 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 @@ -114,7 +114,15 @@ void NormalLaneChange::update_transient_data()
}

auto & transient_data = common_data_ptr_->transient_data;
std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration();

const auto & p = *common_data_ptr_->bpp_param_ptr;
const auto nearest_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
prev_module_output_.path.points, common_data_ptr_->get_ego_pose(),
p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold);
transient_data.current_path_velocity =
prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps;
transient_data.current_path_seg_idx = nearest_seg_idx;

std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) =
calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes());
Expand Down Expand Up @@ -145,8 +153,6 @@ void NormalLaneChange::update_transient_data()
updateStopTime();
transient_data.is_ego_stuck = is_ego_stuck();

RCLCPP_DEBUG(
logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max);
RCLCPP_DEBUG(
logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min,
transient_data.lane_changing_length.max);
Expand Down Expand Up @@ -308,11 +314,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const
const auto terminal_turn_signal_info =
get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end);

const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold;
const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
const size_t current_nearest_seg_idx =
autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
const auto nearest_dist_threshold = common_param.ego_nearest_dist_threshold;
const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx;

return getTurnSignalDecider().overwrite_turn_signal(
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
Expand Down Expand Up @@ -861,121 +865,14 @@ bool NormalLaneChange::isAbortState() const
lane_change_debug_.is_abort = true;
return true;
}

int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const
{
const auto get_opposite_direction =
(direction_ == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT;
return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction));
}

std::pair<double, double> NormalLaneChange::calcCurrentMinMaxAcceleration() const
{
const auto & p = getCommonParam();

const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc);
const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc);

const auto ego_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold,
p.ego_nearest_yaw_threshold);
const auto max_path_velocity =
prev_module_output_.path.points.at(ego_seg_idx).point.longitudinal_velocity_mps;

// calculate minimum and maximum acceleration
const auto min_acc = utils::lane_change::calcMinimumAcceleration(
getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_);
const auto max_acc = utils::lane_change::calcMaximumAcceleration(
getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_);

return {min_acc, max_acc};
}

std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
{
// TODO(Azu): sampler should work even when we're not approaching terminal
if (prev_module_output_.path.points.empty()) {
return {};
}

const auto & route_handler = *getRouteHandler();
const auto current_pose = getEgoPose();
const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num;

const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration();

// if max acc is not positive, then we do the normal sampling
if (max_acc <= 0.0) {
RCLCPP_DEBUG(
logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc);
return utils::lane_change::getAccelerationValues(
min_acc, max_acc, longitudinal_acc_sampling_num);
}

// calculate maximum lane change length
// TODO(Azu) Double check why it's failing with transient data
const auto current_max_dist_buffer =
calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc);

if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) {
RCLCPP_DEBUG(
logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc,
max_acc);
return utils::lane_change::getAccelerationValues(
min_acc, max_acc, longitudinal_acc_sampling_num);
}

// If the ego is in stuck, sampling all possible accelerations to find avoiding path.
if (common_data_ptr_->transient_data.is_ego_stuck) {
auto clock = rclcpp::Clock(RCL_ROS_TIME);
RCLCPP_INFO_THROTTLE(
logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc,
max_acc);
return utils::lane_change::getAccelerationValues(
min_acc, max_acc, longitudinal_acc_sampling_num);
}

// if maximum lane change length is less than length to goal or the end of target lanes, only
// sample max acc
if (route_handler.isInGoalRouteSection(target_lanes.back())) {
const auto goal_pose = route_handler.getGoalPose();
if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) {
RCLCPP_DEBUG(
logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc);
return {max_acc};
}
} else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) {
RCLCPP_DEBUG(
logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc);
return {max_acc};
}

RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc);
return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num);
}

std::vector<double> NormalLaneChange::calc_prepare_durations() const
{
const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr;
const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front +
lc_param_ptr->min_length_for_turn_signal_activation;
const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration;

// TODO(Azu) this check seems to cause scenario failures.
if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) {
return {max_prepare_duration};
}

std::vector<double> prepare_durations;
constexpr double step = 0.5;

for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) {
prepare_durations.push_back(duration);
}

return prepare_durations;
}

bool NormalLaneChange::get_prepare_segment(
PathWithLaneId & prepare_segment, const double prepare_length) const
{
Expand Down Expand Up @@ -1296,22 +1193,14 @@ std::vector<LaneChangePhaseMetrics> NormalLaneChange::get_prepare_metrics() cons
const auto & current_lanes = common_data_ptr_->lanes_ptr->current;
const auto & target_lanes = common_data_ptr_->lanes_ptr->target;
const auto current_velocity = getEgoVelocity();

// get sampling acceleration values
const auto longitudinal_acc_sampling_values =
sampleLongitudinalAccValues(current_lanes, target_lanes);

const auto prepare_durations = calc_prepare_durations();

RCLCPP_DEBUG(
logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu",
prepare_durations.size(), longitudinal_acc_sampling_values.size());
// set speed limit to be current path velocity;
const auto max_path_velocity = common_data_ptr_->transient_data.current_path_velocity;

const auto dist_to_target_start =
calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes);
return calculation::calc_prepare_phase_metrics(
common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity,
dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start);
common_data_ptr_, current_velocity, max_path_velocity, dist_to_target_start,
common_data_ptr_->transient_data.dist_to_terminal_start);
}

std::vector<LaneChangePhaseMetrics> NormalLaneChange::get_lane_changing_metrics(
Expand Down
Loading

0 comments on commit be12c10

Please sign in to comment.