From a4563aa739a0a7be6de34e6d205dbfac64f95ba9 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Aug 2024 10:23:39 +0900 Subject: [PATCH] perf(static_obstacle_avoidance): improve logic to reduce computational cost (#8432) * perf(safety_check): check within first Signed-off-by: satoshi-ota * perf(static_obstacle_avoidance): remove duplicated process Signed-off-by: satoshi-ota * perf(static_obstacle_avoidance): remove heavy process Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.cpp | 28 +++++++++---------- .../src/scene.cpp | 5 ---- .../src/utils.cpp | 5 ++-- 3 files changed, 15 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 95a498b7b8475..1de55dca29347 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -61,32 +61,30 @@ bool isCentroidWithinLanelet( const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); - if ( - std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > - yaw_threshold) { + if (!boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position)) + .basicPoint(), + lanelet.polygon2d().basicPolygon())) { return false; } - return boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position)) - .basicPoint(), - lanelet.polygon2d().basicPolygon()); + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) < + yaw_threshold; } bool isPolygonOverlapLanelet( const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); - if ( - std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > - yaw_threshold) { + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + if (!isPolygonOverlapLanelet(object, lanelet_polygon)) { return false; } - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return isPolygonOverlapLanelet(object, lanelet_polygon); + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) < + yaw_threshold; } bool isPolygonOverlapLanelet( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index d125f043f477b..39a8d1b853e47 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -926,11 +926,6 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() spline_shift_path = helper_->getPreviousSplineShiftPath(); } - // post processing - { - postProcess(); // remove old shift points - } - BehaviorModuleOutput output; const auto is_ignore_signal = [this](const UUID & uuid) { 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 0830c0f10dd4e..f7bda56150b49 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 @@ -2253,10 +2253,9 @@ std::pair separateObjectsByPath( Pose p_reference_ego_front = reference_path.points.front().point.pose; Pose p_spline_ego_front = spline_path.points.front().point.pose; double next_longitudinal_distance = parameters->resample_interval_for_output; + const auto offset = arc_length_array.at(ego_idx); for (size_t i = 0; i < points_size; ++i) { - const auto distance_from_ego = - autoware::motion_utils::calcSignedArcLength(reference_path.points, ego_idx, i); - if (distance_from_ego > object_check_forward_distance) { + if (arc_length_array.at(i) > object_check_forward_distance + offset) { break; }