Skip to content

Commit

Permalink
refactor(static obstacle avoidance): remove redundant calculation (au…
Browse files Browse the repository at this point in the history 
…towarefoundation#9326)

* refactor bases on clang tidy

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

* refactor extend backward length

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

* mover redundant calculation in getRoadShoulderDistance

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

---------

Signed-off-by: Go Sakayori <[email protected]>
Signed-off-by: Go Sakayori <[email protected]>
  • Loading branch information
@go-sakayori @satoshi-ota
go-sakayori authored and satoshi-ota committed Dec 19, 2024
1 parent 313feac commit 20cbf7a
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 31 deletions.
20 changes: 11 additions & 9 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/system/time_keeper.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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{};
Expand Down Expand Up @@ -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) {
Expand Down
36 changes: 14 additions & 22 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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<std::tuple<double, Point, Point>> 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<Pose>().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 =
Expand All @@ -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)
Expand Down Expand Up @@ -1265,7 +1257,7 @@ std::vector<UUID> 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;
Expand All @@ -1279,10 +1271,11 @@ std::vector<UUID> 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;
Expand All @@ -1303,7 +1296,6 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
}
obj.longitudinal = min_distance;
obj.length = max_distance - min_distance;
return;
}

std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
Expand Down Expand Up @@ -2173,9 +2165,9 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
});
};

const auto to_predicted_objects = [&p, &parameters](const auto & objects) {
const auto to_predicted_objects = [&parameters](const auto & objects) {
PredictedObjects ret{};
std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
std::for_each(objects.begin(), objects.end(), [&ret, &parameters](const auto & object) {
if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
// check only moving objects
if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) {
Expand Down

0 comments on commit 20cbf7a

Please sign in to comment.