Skip to content

Commit

Permalink
refactor(static_obstacle_avoidance): move route handler based calcula…
Browse files Browse the repository at this point in the history
…tion outside loop (autowarefoundation#8968)

* refactor filterTargetObjects

Signed-off-by: Go Sakayori <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

Co-authored-by: Satoshi OTA <[email protected]>

---------

Signed-off-by: Go Sakayori <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
  • Loading branch information
go-sakayori and satoshi-ota committed Dec 19, 2024
1 parent a89f6b7 commit 313feac
Showing 1 changed file with 17 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -807,8 +807,8 @@ bool isObviousAvoidanceTarget(
}

bool isSatisfiedWithCommonCondition(
ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range,
const std::shared_ptr<const PlannerData> & planner_data,
ObjectData & object, const PathWithLaneId & path, const double forward_detection_range,
const double to_goal_distance, const Point & ego_pos, const bool is_allowed_goal_modification,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
// Step1. filtered by target object type.
Expand All @@ -824,8 +824,7 @@ bool isSatisfiedWithCommonCondition(
}

// Step3. filtered by longitudinal distance.
const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object);
fillLongitudinalAndLengthByClosestEnvelopeFootprint(path, ego_pos, object);

if (object.longitudinal < -parameters->object_check_backward_distance) {
object.info = ObjectInfo::FURTHER_THAN_THRESHOLD;
Expand All @@ -840,20 +839,12 @@ bool isSatisfiedWithCommonCondition(
// Step4. filtered by distance between object and goal position.
// TODO(Satoshi OTA): remove following two conditions after it can execute avoidance and goal
// planner module simultaneously.
const auto & rh = planner_data->route_handler;
const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points);
const auto to_goal_distance =
rh->isInGoalRouteSection(data.current_lanelets.back())
? autoware::motion_utils::calcSignedArcLength(
data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1)
: std::numeric_limits<double>::max();

if (object.longitudinal > to_goal_distance) {
object.info = ObjectInfo::FURTHER_THAN_GOAL;
return false;
}

if (!utils::isAllowedGoalModification(planner_data->route_handler)) {
if (!is_allowed_goal_modification) {
if (
object.longitudinal + object.length / 2 + parameters->object_check_goal_distance >
to_goal_distance) {
Expand Down Expand Up @@ -1924,9 +1915,21 @@ void filterTargetObjects(
data.target_objects.push_back(object);
};

const auto & rh = planner_data->route_handler;
const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points);
const auto to_goal_distance =
rh->isInGoalRouteSection(data.current_lanelets.back())
? autoware::motion_utils::calcSignedArcLength(
data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1)
: std::numeric_limits<double>::max();
const auto & is_allowed_goal_modification =
utils::isAllowedGoalModification(planner_data->route_handler);

for (auto & o : objects) {
if (!filtering_utils::isSatisfiedWithCommonCondition(
o, data, forward_detection_range, planner_data, parameters)) {
o, data.reference_path_rough, forward_detection_range, to_goal_distance,
planner_data->self_odometry->pose.pose.position, is_allowed_goal_modification,
parameters)) {
data.other_objects.push_back(o);
continue;
}
Expand Down

0 comments on commit 313feac

Please sign in to comment.