Skip to content

Commit

Permalink
fix(avoidance): fix bug in shift lon distance calculation (autowarefo…
Browse files Browse the repository at this point in the history 
…undation#5557)

* fix(avoidance): consider avoidance prepare time

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): consider avoidance prepare distance

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): use std::optional

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota @takayuki5168
satoshi-ota authored and takayuki5168 committed Nov 22, 2023
1 parent 7b1e139 commit bce0800
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 39 deletions.
3 changes: 2 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,8 @@ class AvoidanceHelper
max_shift_length, getLateralMinJerkLimit(), getEgoSpeed());

return std::clamp(
1.5 * dynamic_distance, parameters_->object_check_min_forward_distance,
1.5 * dynamic_distance + getNominalPrepareDistance(),
parameters_->object_check_min_forward_distance,
parameters_->object_check_max_forward_distance);
}

Expand Down
78 changes: 40 additions & 38 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -920,8 +920,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
const auto & base_link2rear = planner_data_->parameters.base_link2rear;

// Calculate feasible shift length
const auto get_shift_length =
[&](auto & object, const auto & desire_shift_length) -> boost::optional<double> {
const auto get_shift_profile =
[&](
auto & object, const auto & desire_shift_length) -> std::optional<std::pair<double, double>> {
// use each object param
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
Expand All @@ -931,55 +932,57 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
const auto avoiding_shift = desire_shift_length - current_ego_shift;
const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift);

// calculate remaining distance.
const auto prepare_distance = helper_.getNominalPrepareDistance();
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant = object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal + prepare_distance;
const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
const auto remaining_distance = object.longitudinal - constant;
const auto avoidance_distance =
has_enough_distance ? nominal_avoid_distance : remaining_distance;

// nominal case. avoidable.
if (has_enough_distance) {
return std::make_pair(desire_shift_length, avoidance_distance);
}

if (!isBestEffort(parameters_->policy_lateral_margin)) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// ego already has enough positive shift.
const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3;
if (is_object_on_right && has_enough_positive_shift) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// ego already has enough negative shift.
const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3;
if (!is_object_on_right && has_enough_negative_shift) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// don't relax shift length since it can stop in front of the object.
if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// calculate remaining distance.
const auto prepare_distance = helper_.getNominalPrepareDistance();
const auto & additional_buffer_longitudinal =
object_parameter.use_conservative_buffer_longitudinal
? planner_data_->parameters.base_link2front
: 0.0;
const auto constant = object_parameter.safety_buffer_longitudinal +
additional_buffer_longitudinal + prepare_distance;
const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
const auto remaining_distance = object.longitudinal - constant;

// the avoidance path is already approved
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) ||
(helper_.getShift(object_pos) < 0.0 && !is_object_on_right);
if (is_approved) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// prepare distance is not enough. unavoidable.
if (remaining_distance < 1e-3) {
object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
return boost::none;
}

// nominal case. avoidable.
if (has_enough_distance) {
return desire_shift_length;
return std::nullopt;
}

// calculate lateral jerk.
Expand All @@ -988,13 +991,13 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(

// relax lateral jerk limit. avoidable.
if (required_jerk < helper_.getLateralMaxJerkLimit()) {
return desire_shift_length;
return std::make_pair(desire_shift_length, avoidance_distance);
}

// avoidance distance is not enough. unavoidable.
if (!isBestEffort(parameters_->policy_deceleration)) {
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
return boost::none;
return std::nullopt;
}

// output avoidance path under lateral jerk constraints.
Expand All @@ -1003,7 +1006,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(

if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
object.reason = "LessThanExecutionThreshold";
return boost::none;
return std::nullopt;
}

const auto feasible_shift_length =
Expand All @@ -1017,7 +1020,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
return boost::none;
return std::nullopt;
}

{
Expand All @@ -1026,7 +1029,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
std::abs(avoiding_shift), feasible_relative_shift_length);
}

return feasible_shift_length;
return std::make_pair(feasible_shift_length, avoidance_distance);
};

const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
Expand Down Expand Up @@ -1061,9 +1064,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
// use each object param
const auto object_type = utils::getHighestProbLabel(o.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto feasible_shift_length = get_shift_length(o, desire_shift_length);
const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);

if (!feasible_shift_length) {
if (!feasible_shift_profile.has_value()) {
if (o.avoid_required && is_forward_object(o)) {
break;
} else {
Expand All @@ -1072,10 +1075,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
}

// use absolute dist for return-to-center, relative dist from current for avoiding.
const auto feasible_avoid_distance =
helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
const auto feasible_return_distance =
helper_.getMaxAvoidanceDistance(feasible_shift_length.get());
helper_.getMaxAvoidanceDistance(feasible_shift_profile.value().first);

AvoidLine al_avoid;
{
Expand All @@ -1091,14 +1092,15 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(

// start point (use previous linear shift length as start shift length.)
al_avoid.start_longitudinal = [&]() {
const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3);
const auto nearest_avoid_distance =
std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3);

if (data.to_start_point > to_shift_end) {
return nearest_avoid_distance;
}

const auto minimum_avoid_distance =
helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift);
const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);

return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
Expand All @@ -1110,7 +1112,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);

// end point
al_avoid.end_shift_length = feasible_shift_length.get();
al_avoid.end_shift_length = feasible_shift_profile.value().first;
al_avoid.end_longitudinal = to_shift_end;

// misc
Expand All @@ -1125,7 +1127,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
const auto to_shift_start = o.longitudinal + offset;

// start point
al_return.start_shift_length = feasible_shift_length.get();
al_return.start_shift_length = feasible_shift_profile.value().first;
al_return.start_longitudinal = to_shift_start;

// end point
Expand Down

0 comments on commit bce0800

Please sign in to comment.