Skip to content

Commit

Permalink
feat(avoidance): improve object detection area in order not to preven…
Browse files Browse the repository at this point in the history
…t endless loop (autowarefoundation#6084)

* perf(avoidance): reduce heavy process

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

* fix(avoidance): filter objects by precise lon distance

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

* refactor(avoidance): remove unused function

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

* feat(avoidance): improve detection area

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

* fix(avoidance): return shift line

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

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored Jan 16, 2024
1 parent 3acbdd7 commit 3928295
Show file tree
Hide file tree
Showing 5 changed files with 143 additions and 125 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
}

void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
AvoidancePlanningData & data, DebugData & debug) const
AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
{
const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);

Expand Down Expand Up @@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
[&](const auto & object) { return createObjectData(data, object); });
}

utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p);
utils::avoidance::filterTargetObjects(
target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance,
planner_data_, p);
}

ObjectData AvoidanceByLaneChange::createObjectData(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,11 +77,6 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
const double vehicle_width);

double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);

bool isCentroidWithinLanelets(
const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets);

lanelet::ConstLanelets getAdjacentLane(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
Expand Down Expand Up @@ -128,12 +123,7 @@ void compensateDetectionLost(
ObjectDataArray & other_objects);

void filterTargetObjects(
ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

double getRoadShoulderDistance(
ObjectData & object, const AvoidancePlanningData & data,
ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

Expand All @@ -157,9 +147,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
DebugData & debug);

std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, const bool is_running, DebugData & debug);
const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,
const std::shared_ptr<const PlannerData> & planner_data, const AvoidancePlanningData & data,
const std::shared_ptr<AvoidanceParameters> & parameters,
const double object_check_forward_distance, DebugData & debug);

DrivableLanes generateExpandDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
Expand Down
32 changes: 10 additions & 22 deletions planning/behavior_path_avoidance_module/src/scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -287,21 +287,18 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
void AvoidanceModule::fillAvoidanceTargetObjects(
AvoidancePlanningData & data, DebugData & debug) const
{
using utils::avoidance::fillAvoidanceNecessity;
using utils::avoidance::fillObjectStoppableJudge;
using utils::avoidance::filterTargetObjects;
using utils::avoidance::getTargetLanelets;

// Add margin in order to prevent avoidance request chattering only when the module is running.
const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING ||
getCurrentStatus() == ModuleStatus::WAITING_APPROVAL;
using utils::avoidance::separateObjectsByPath;

// Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
const auto sparse_resample_path = utils::resamplePathWithSpline(
helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output);
const auto [object_within_target_lane, object_outside_target_lane] =
utils::avoidance::separateObjectsByPath(
sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(),
is_running, debug);
constexpr double MARGIN = 10.0;
const auto forward_detection_range = helper_->getForwardDetectionRange();
const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath(
helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_,
data, parameters_, forward_detection_range + MARGIN, debug);

for (const auto & object : object_outside_target_lane.objects) {
ObjectData other_object;
Expand All @@ -316,11 +313,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
}

// Filter out the objects to determine the ones to be avoided.
filterTargetObjects(objects, data, debug, planner_data_, parameters_);
filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);

// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) {
fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
o.to_stop_line = calcDistanceToStopLine(o);
fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
});
Expand Down Expand Up @@ -380,21 +379,10 @@ ObjectData AvoidanceModule::createObjectData(
utils::avoidance::fillInitialPose(object_data, detected_objects_);

// Calc lateral deviation from path to target object.
object_data.to_centerline =
lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0
? Direction::LEFT
: Direction::RIGHT;

// Find the footprint point closest to the path, set to object_data.overhang_distance.
object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
object_data, data.reference_path, object_data.overhang_pose.position);

// Check whether the the ego should avoid the object.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
utils::avoidance::fillAvoidanceNecessity(
object_data, registered_objects_, vehicle_width, parameters_);

return object_data;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1101,8 +1101,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
const double variable_prepare_distance =
std::max(nominal_prepare_distance - last_sl_distance, 0.0);

double prepare_distance_scaled =
std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance);
double prepare_distance_scaled = std::max(
helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
double avoid_distance_scaled = nominal_avoid_distance;
if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) {
const auto scale = (remaining_distance - last_sl_distance) /
Expand All @@ -1122,7 +1122,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
al.end_idx =
utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
al.end = data.reference_path.points.at(al.end_idx).point.pose;
al.end_longitudinal = arclength_from_ego.at(al.end_idx);
al.end_longitudinal = prepare_distance_scaled;
al.end_shift_length = last_sl.end_shift_length;
al.start_shift_length = last_sl.end_shift_length;
ret.push_back(al);
Expand All @@ -1136,7 +1136,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
al.start_idx =
utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
al.start = data.reference_path.points.at(al.start_idx).point.pose;
al.start_longitudinal = arclength_from_ego.at(al.start_idx);
al.start_longitudinal = prepare_distance_scaled;
al.end_idx = utils::avoidance::findPathIndexFromArclength(
arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled);
al.end = data.reference_path.points.at(al.end_idx).point.pose;
Expand Down
Loading

0 comments on commit 3928295

Please sign in to comment.