From 20cbf7a6774a82d1b4be5e6b1610e3c7d89227db Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 15 Nov 2024 11:36:18 +0900 Subject: [PATCH] refactor(static obstacle avoidance): remove redundant calculation (#9326) * refactor bases on clang tidy Signed-off-by: Go Sakayori * refactor extend backward length Signed-off-by: Go Sakayori * mover redundant calculation in getRoadShoulderDistance Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../src/scene.cpp | 20 ++++++----- .../src/utils.cpp | 36 ++++++++----------- 2 files changed, 25 insertions(+), 31 deletions(-) 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 8b2e0a6bd1077..7d218a503ed8b 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 @@ -24,6 +24,7 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include #include #include #include @@ -572,7 +573,7 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( } const auto registered_sl_force_deactivated = - [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) { + [&](const std::string & direction, const RegisteredShiftLineArray & shift_line_array) { return std::any_of( shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) { return rtc_interface_ptr_map_.at(direction)->isForceDeactivated(shift_line.uuid); @@ -609,7 +610,7 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( }; auto registered_sl_force_activated = - [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) { + [&](const std::string & direction, const RegisteredShiftLineArray & shift_line_array) { return std::any_of( shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) { return rtc_interface_ptr_map_.at(direction)->isForceActivated(shift_line.uuid); @@ -914,13 +915,14 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( } size_t clip_idx = 0; - for (size_t i = 0; i < prev_ego_idx; ++i) { - if ( - backward_length > - autoware::motion_utils::calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { + double accumulated_length = 0.0; + for (size_t i = prev_ego_idx.value(); i > 0; i--) { + accumulated_length += autoware::universe_utils::calcDistance2d( + previous_path.points.at(i - 1), previous_path.points.at(i)); + if (accumulated_length > backward_length) { + clip_idx = i; break; } - clip_idx = i; } PathWithLaneId extended_path{}; @@ -1172,8 +1174,8 @@ void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shi generator_.setRawRegisteredShiftLine(shift_lines, avoid_data_); const auto sl = helper_->getMainShiftLine(shift_lines); - const auto sl_front = shift_lines.front(); - const auto sl_back = shift_lines.back(); + const auto & sl_front = shift_lines.front(); + const auto & sl_back = shift_lines.back(); const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal; if (helper_->getRelativeShiftToPath(sl) > 0.0) { 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 ad7191c44f055..a6b1d2f4751b8 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 @@ -567,11 +567,7 @@ bool isParkedVehicle( object.to_centerline = lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; - if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - return false; - } - - return true; + return std::abs(object.to_centerline) >= parameters->threshold_distance_object_is_on_center; } bool isCloseToStopFactor( @@ -1055,16 +1051,17 @@ double getRoadShoulderDistance( return 0.0; } + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + const auto envelope_polygon_width = boost::geometry::area(object.envelope_poly) / + std::max(object.length, 1e-3); // prevent division by zero + std::vector> intersects; for (const auto & p1 : object.overhang_points) { - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); const auto p_tmp = geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - for (size_t i = 1; i < bound.size(); i++) { { const auto p2 = @@ -1078,11 +1075,6 @@ double getRoadShoulderDistance( break; } } - - const auto envelope_polygon_width = - boost::geometry::area(object.envelope_poly) / - std::max(object.length, 1e-3); // prevent division by zero - { const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) @@ -1265,7 +1257,7 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & for (const auto & al : lines1) { const auto p_s = al.start_longitudinal; const auto p_e = al.end_longitudinal; - const auto has_overlap = !(p_e < lines2.start_longitudinal || lines2.end_longitudinal < p_s); + const auto has_overlap = p_e >= lines2.start_longitudinal && lines2.end_longitudinal >= p_s; if (!has_overlap) { continue; @@ -1279,10 +1271,11 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & double lerpShiftLengthOnArc(double arc, const AvoidLine & ap) { if (ap.start_longitudinal <= arc && arc < ap.end_longitudinal) { - if (std::abs(ap.getRelativeLongitudinal()) < 1.0e-5) { + const auto relative_longitudinal = ap.getRelativeLongitudinal(); + if (std::abs(relative_longitudinal) < 1.0e-5) { return ap.end_shift_length; } - const auto start_weight = (ap.end_longitudinal - arc) / ap.getRelativeLongitudinal(); + const auto start_weight = (ap.end_longitudinal - arc) / relative_longitudinal; return start_weight * ap.start_shift_length + (1.0 - start_weight) * ap.end_shift_length; } return 0.0; @@ -1303,7 +1296,6 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } obj.longitudinal = min_distance; obj.length = max_distance - min_distance; - return; } std::vector> calcEnvelopeOverhangDistance( @@ -2173,9 +2165,9 @@ std::vector getSafetyCheckTargetObjects( }); }; - const auto to_predicted_objects = [&p, ¶meters](const auto & objects) { + const auto to_predicted_objects = [¶meters](const auto & objects) { PredictedObjects ret{}; - std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { + std::for_each(objects.begin(), objects.end(), [&ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { // check only moving objects if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) {