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

fix(avoidance): cherry pick PRs for avoidance issue #923

Merged
merged 2 commits into from
Oct 10, 2023
+36 −9
Merged

fix(avoidance): cherry pick PRs for avoidance issue #923

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
68346ca
fix(avoidance): don't output unfeasible path (#4899)
satoshi-ota Sep 6, 2023
aaa7281
fix(avoidance): exit if there is no avoidable object (#5051)
satoshi-ota Sep 21, 2023
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
45 changes: 36 additions & 9 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,10 @@ bool AvoidanceModule::canTransitSuccessState()
}
}

const bool has_avoidance_target = !data.target_objects.empty();
const bool has_avoidance_target =
std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) {
return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
});
const bool has_shift_point = !path_shifter_.getShiftLines().empty();
const bool has_base_offset =
std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold;
Expand Down Expand Up @@ -880,17 +883,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
}

// output avoidance path under lateral jerk constraints.
const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk(
const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed());

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

const auto feasible_shift_length =
desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
: -1.0 * feasible_relative_shift_length + current_ego_shift;

const auto feasible =
std::abs(feasible_shift_length - object.overhang_dist) <
0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
if (feasible) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
return boost::none;
}

return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift
: -1.0 * feasible_shift_length + current_ego_shift;
{
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]",
std::abs(avoiding_shift), feasible_relative_shift_length);
}

return feasible_shift_length;
};

const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
Expand Down Expand Up @@ -3012,6 +3033,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const

const auto object = data.target_objects.front();

const auto enough_space =
object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
if (!enough_space) {
return;
}

// calculate shift length for front object.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto object_type = utils::getHighestProbLabel(object.object.classification);
Expand Down
Loading