Skip to content

Commit

Permalink
feat(avoidance): output feasible avoidance path (#4145)
Browse files Browse the repository at this point in the history
feat(avoidance): output feasible shift path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
satoshi-ota authored Jul 12, 2023
1 parent 0752672 commit d6ba61a
Show file tree
Hide file tree
Showing 3 changed files with 119 additions and 69 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,9 @@ class PathShifter
static double calcFeasibleVelocityFromJerk(
const double lateral, const double jerk, const double distance);

static double calcLateralDistFromJerk(
const double longitudinal, const double jerk, const double velocity);

static double calcLongitudinalDistFromJerk(
const double lateral, const double jerk, const double velocity);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -836,15 +836,96 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future)
AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
{
const auto prepare_distance = helper_.getNominalPrepareDistance();

// To be consistent with changes in the ego position, the current shift length is considered.
const auto current_ego_shift = helper_.getEgoShift();
const auto & base_link2front = planner_data_->parameters.base_link2front;
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> {
// use each object param
const auto object_type = utils::getHighestProbLabel(object.object.classification);
const auto object_parameter = parameters_->object_parameters.at(object_type);
const auto is_object_on_right = utils::avoidance::isOnRight(object);

// use absolute dist for return-to-center, relative dist from current for avoiding.
const auto avoiding_shift = desire_shift_length - current_ego_shift;
const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(avoiding_shift);

// 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;
}

// 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;
}

// calculate remaining distance.
const auto prepare_distance = helper_.getNominalPrepareDistance();
const auto constant =
object_parameter.safety_buffer_longitudinal + base_link2front + 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;
}

// 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;
}

// calculate lateral jerk.
const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed());

// relax lateral jerk limit. avoidable.
if (required_jerk < parameters_->max_lateral_jerk) {
return desire_shift_length;
}

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

// output avoidance path under lateral jerk constraints.
const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk(
remaining_distance, parameters_->max_lateral_jerk, helper_.getSharpAvoidanceEgoSpeed());

RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 1000,
"original shift length is not feasible. generate avoidance path under the constraints. "
"[original: (%.2f) actual: (%.2f)]",
std::abs(avoiding_shift), feasible_shift_length);

return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift
: -1.0 * feasible_shift_length + current_ego_shift;
};

const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };

const auto is_valid_shift_line = [](const auto & s) {
return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
};

AvoidLineArray avoid_lines;
for (auto & o : data.target_objects) {
if (!o.avoid_margin) {
Expand All @@ -857,8 +938,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
}

const auto is_object_on_right = utils::avoidance::isOnRight(o);
const auto shift_length = helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get());
if (utils::avoidance::isSameDirectionShift(is_object_on_right, shift_length)) {
const auto desire_shift_length =
helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get());
if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) {
o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT;
if (o.avoid_required && is_forward_object(o)) {
break;
Expand All @@ -867,78 +949,39 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
}
}

const auto avoiding_shift = shift_length - current_ego_shift;
const auto return_shift = shift_length;

// use absolute dist for return-to-center, relative dist from current for avoiding.
const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(avoiding_shift);
const auto nominal_return_distance = helper_.getNominalAvoidanceDistance(return_shift);

// 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);

/**
* Is there enough distance from ego to object for avoidance?
* - Yes -> use the nominal distance.
* - No -> check if it is possible to avoid within maximum jerk limit.
* - Yes -> use the stronger jerk.
* - No -> ignore this object. Expected behavior is that the vehicle will stop in front
* of the obstacle, then start avoidance.
*/
const auto constant =
object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance;
const auto has_enough_distance = o.longitudinal > constant + nominal_avoid_distance;
const auto remaining_distance = o.longitudinal - constant;
if (!has_enough_distance) {
if (remaining_distance <= 0.0) {
// TODO(Horibe) Even if there is no enough distance for avoidance shift, the
// return-to-center shift must be considered for each object if the current_shift
// is not zero.
if (!data.avoiding_now) {
o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
if (o.avoid_required && is_forward_object(o)) {
break;
} else {
continue;
}
}
}

// This is the case of exceeding the jerk limit. Use the sharp avoidance ego speed.
const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance(
avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed());
if (required_jerk > parameters_->max_lateral_jerk) {
if (!data.avoiding_now) {
o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
if (o.avoid_required && is_forward_object(o)) {
break;
} else {
continue;
}
}
if (!feasible_shift_length) {
if (o.avoid_required && is_forward_object(o)) {
break;
} else {
continue;
}
}
const auto avoiding_distance =
has_enough_distance ? nominal_avoid_distance : remaining_distance;

const auto is_valid_shift_line = [](const auto & s) {
return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
};
// use absolute dist for return-to-center, relative dist from current for avoiding.
const auto feasible_avoid_distance =
helper_.getNominalAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
const auto feasible_return_distance =
helper_.getNominalAvoidanceDistance(feasible_shift_length.get());

AvoidLine al_avoid;
{
const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front;
const auto path_front_to_ego =
avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index);

al_avoid.start_longitudinal = o.longitudinal - offset - avoiding_distance;
al_avoid.start_longitudinal =
std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3);
al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength(
avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose;
al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);

al_avoid.end_shift_length = shift_length;
al_avoid.end_shift_length = feasible_shift_length.get();
al_avoid.end_longitudinal = o.longitudinal - offset;
al_avoid.id = getOriginalShiftLineUniqueId();
al_avoid.object = o;
Expand All @@ -957,11 +1000,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
const auto return_remaining_distance =
std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0);

al_return.start_shift_length = shift_length;
al_return.start_shift_length = feasible_shift_length.get();
al_return.end_shift_length = 0.0;
al_return.start_longitudinal = o.longitudinal + offset;
al_return.end_longitudinal =
o.longitudinal + offset + std::min(nominal_return_distance, return_remaining_distance);
o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance);
al_return.id = getOriginalShiftLineUniqueId();
al_return.object = o;

Expand All @@ -970,14 +1013,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
}
}

DEBUG_PRINT(
"object is set: avoid_shift = %f, return_shift = %f, dist = (avoidStart: %3.3f, avoidEnd: "
"%3.3f, returnEnd: %3.3f), avoiding_dist = (nom:%f, res:%f), avoid_margin = %f, return_dist "
"= %f",
avoiding_shift, return_shift, al_avoid.start_longitudinal, al_avoid.end_longitudinal,
al_return.end_longitudinal, nominal_avoid_distance, avoiding_distance, o.avoid_margin.get(),
nominal_return_distance);

o.is_avoidable = true;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -555,17 +555,29 @@ double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jer
}

double PathShifter::calcFeasibleVelocityFromJerk(
const double lateral, const double jerk, const double distance)
const double lateral, const double jerk, const double longitudinal)
{
const double j = std::abs(jerk);
const double l = std::abs(lateral);
const double d = std::abs(distance);
const double d = std::abs(longitudinal);
if (j < 1.0e-8) {
return 1.0e10; // TODO(Horibe) maybe invalid arg?
}
return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0));
}

double PathShifter::calcLateralDistFromJerk(
const double longitudinal, const double jerk, const double velocity)
{
const double j = std::abs(jerk);
const double d = std::abs(longitudinal);
const double v = std::abs(velocity);
if (j < 1.0e-8) {
return 1.0e10; // TODO(Horibe) maybe invalid arg?
}
return 2.0 * std::pow(d / (4.0 * v), 3.0) * j;
}

double PathShifter::calcLongitudinalDistFromJerk(
const double lateral, const double jerk, const double velocity)
{
Expand Down

0 comments on commit d6ba61a

Please sign in to comment.