From 313feaccf47f15bd05b88ce1939cb5b749a4be2c Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 26 Sep 2024 20:27:40 +0900 Subject: [PATCH] refactor(static_obstacle_avoidance): move route handler based calculation outside loop (#8968) * refactor filterTargetObjects Signed-off-by: Go Sakayori * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../src/utils.cpp | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index a27a16f1d8102..ad7191c44f055 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -807,8 +807,8 @@ bool isObviousAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, - const std::shared_ptr & 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 & parameters) { // Step1. filtered by target object type. @@ -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; @@ -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::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) { @@ -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::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; }