Skip to content

Commit

Permalink
perf(bpp): reduce computational cost (#6054)
Browse files Browse the repository at this point in the history
* pref(avoidance): don't use boost union_

Signed-off-by: satoshi-ota <[email protected]>

* perf(avoidance): reduce frequency to call calcSignedArcLength

Signed-off-by: satoshi-ota <[email protected]>

* perf(turn_signal): reduce frequency to call calcSignedArcLength

Signed-off-by: satoshi-ota <[email protected]>

* perf(static_drivable_area_expansion): don't use calcDistance2d

Signed-off-by: satoshi-ota <[email protected]>

* fix(static_drivable_area_expansion): rename and fix return value consistency

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored Jan 15, 2024
1 parent b60975f commit eeed846
Show file tree
Hide file tree
Showing 7 changed files with 84 additions and 80 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -588,7 +588,7 @@ struct ShiftLineData
*/
struct DebugData
{
geometry_msgs::msg::Polygon detection_area;
std::vector<geometry_msgs::msg::Polygon> detection_areas;

lanelet::ConstLineStrings3d bounds;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface
*/
void updateRegisteredRTCStatus(const PathWithLaneId & path)
{
const Point ego_position = planner_data_->self_odometry->pose.pose.position;
const auto ego_idx = planner_data_->findEgoIndex(path.points);

for (const auto & left_shift : left_shift_array_) {
const double start_distance =
calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position);
calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
const double finish_distance = start_distance + left_shift.relative_longitudinal;
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
left_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
Expand All @@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface

for (const auto & right_shift : right_shift_array_) {
const double start_distance =
calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position);
const double finish_distance =
calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position);
calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
const double finish_distance = start_distance + right_shift.relative_longitudinal;
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
right_shift.uuid, true, start_distance, finish_distance, clock_->now());
if (finish_distance > -1.0e-03) {
Expand Down Expand Up @@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface
UUID uuid;
Pose start_pose;
Pose finish_pose;
double relative_longitudinal{0.0};
};

using RegisteredShiftLineArray = std::vector<RegisteredShiftLine>;
Expand Down
7 changes: 6 additions & 1 deletion planning/behavior_path_avoidance_module/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray(
addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9);
}

// detection area
size_t i = 0;
for (const auto & detection_area : debug.detection_areas) {
add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
}

// misc
{
add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
add(laneletsAsTriangleMarkerArray(
"drivable_lanes", transformToLanelets(data.drivable_lanes),
Expand Down
62 changes: 37 additions & 25 deletions planning/behavior_path_avoidance_module/src/scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -703,18 +703,6 @@ bool AvoidanceModule::isSafePath(
return true; // if safety check is disabled, it always return safe.
}

const bool limit_to_max_velocity = false;
const auto ego_predicted_path_params =
std::make_shared<utils::path_safety_checker::EgoPredictedPathParams>(
parameters_->ego_predicted_path_params);
const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points);
const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath(
ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
true, limit_to_max_velocity);
const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath(
ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
false, limit_to_max_velocity);

const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
const auto is_right_shift = [&]() -> std::optional<bool> {
for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
Expand All @@ -741,6 +729,22 @@ bool AvoidanceModule::isSafePath(
const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug);

if (safety_check_target_objects.empty()) {
return true;
}

const bool limit_to_max_velocity = false;
const auto ego_predicted_path_params =
std::make_shared<utils::path_safety_checker::EgoPredictedPathParams>(
parameters_->ego_predicted_path_params);
const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points);
const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath(
ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
true, limit_to_max_velocity);
const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath(
ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx,
false, limit_to_max_velocity);

for (const auto & object : safety_check_target_objects) {
auto current_debug_data = utils::path_safety_checker::createObjectDebug(object);

Expand Down Expand Up @@ -793,15 +797,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig

const auto longest_dist_to_shift_point = [&]() {
double max_dist = 0.0;
for (const auto & pnt : path_shifter_.getShiftLines()) {
max_dist = std::max(
max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition()));
auto lines = path_shifter_.getShiftLines();
if (lines.empty()) {
return max_dist;
}
for (const auto & sp : generator_.getRawRegisteredShiftLine()) {
max_dist = std::max(
max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition()));
}
return max_dist;
std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) {
return a.start_idx < b.start_idx;
});
return std::max(
max_dist,
calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition()));
}();

const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line.
Expand Down Expand Up @@ -1029,11 +1034,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines)
const auto sl = helper_->getMainShiftLine(shift_lines);
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) {
left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end});
left_shift_array_.push_back(
{uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal});
} else if (helper_->getRelativeShiftToPath(sl) < 0.0) {
right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end});
right_shift_array_.push_back(
{uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal});
}

uuid_map_.at("left") = generateUUID();
Expand Down Expand Up @@ -1150,15 +1158,19 @@ bool AvoidanceModule::isValidShiftLine(
const size_t start_idx = shift_lines.front().start_idx;
const size_t end_idx = shift_lines.back().end_idx;

const auto path = shifter_for_validate.getReferencePath();
const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound);
const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound);
for (size_t i = start_idx; i <= end_idx; ++i) {
const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i));
const auto p = getPoint(path.points.at(i));
lanelet::BasicPoint2d basic_point{p.x, p.y};

const auto shift_length = proposed_shift_path.shift_length.at(i);
const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound;
const auto THRESHOLD = minimum_distance + std::abs(shift_length);

if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) {
if (
boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) <
THRESHOLD) {
RCLCPP_DEBUG_THROTTLE(
getLogger(), *clock_, 1000,
"following latest new shift line may cause deviation from drivable area.");
Expand Down
45 changes: 16 additions & 29 deletions planning/behavior_path_avoidance_module/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1892,11 +1892,12 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
max_offset = std::max(max_offset, offset);
}

const double MARGIN = is_running ? 1.0 : 0.0; // [m]
const auto detection_area =
createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset);
createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset + MARGIN);
const auto ego_idx = planner_data->findEgoIndex(path.points);

Polygon2d attention_area;
std::vector<Polygon2d> detection_areas;
for (size_t i = 0; i < path.points.size() - 1; ++i) {
const auto & p_ego_front = path.points.at(i).point.pose;
const auto & p_ego_back = path.points.at(i + 1).point.pose;
Expand All @@ -1906,41 +1907,27 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
break;
}

const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area);

std::vector<Polygon2d> unions;
boost::geometry::union_(attention_area, ego_one_step_polygon, unions);
if (!unions.empty()) {
attention_area = unions.front();
boost::geometry::correct(attention_area);
}
detection_areas.push_back(createOneStepPolygon(p_ego_front, p_ego_back, detection_area));
}

// expand detection area width only when the module is running.
if (is_running) {
constexpr int PER_CIRCLE = 36;
constexpr double MARGIN = 1.0; // [m]
boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(MARGIN);
boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE);
boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE);
boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE);
boost::geometry::strategy::buffer::side_straight side_strategy;
boost::geometry::model::multi_polygon<Polygon2d> result;
// Create the buffer of a multi polygon
boost::geometry::buffer(
attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy,
circle_strategy);
if (!result.empty()) {
attention_area = result.front();
std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) {
debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z));
});

const auto within_detection_area = [&](const auto & obj_polygon) {
for (const auto & detection_area : detection_areas) {
if (!boost::geometry::disjoint(obj_polygon, detection_area)) {
return true;
}
}
}

debug.detection_area = toMsg(attention_area, data.reference_pose.position.z);
return false;
};

const auto objects = planner_data->dynamic_object->objects;
std::for_each(objects.begin(), objects.end(), [&](const auto & object) {
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
if (boost::geometry::disjoint(obj_polygon, attention_area)) {
if (!within_detection_area(obj_polygon)) {
other_objects.objects.push_back(object);
} else {
target_objects.objects.push_back(object);
Expand Down
18 changes: 10 additions & 8 deletions planning/behavior_path_planner_common/src/turn_signal_decider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -415,15 +415,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(

const double dist_to_original_desired_start =
get_distance(original_desired_start_point) - base_link2front_;
const double dist_to_original_desired_end = get_distance(original_desired_end_point);
const double dist_to_original_required_start =
get_distance(original_required_start_point) - base_link2front_;
const double dist_to_original_required_end = get_distance(original_required_end_point);
const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_;
const double dist_to_new_desired_end = get_distance(new_desired_end_point);
const double dist_to_new_required_start =
get_distance(new_required_start_point) - base_link2front_;
const double dist_to_new_required_end = get_distance(new_required_end_point);

// If we still do not reach the desired front point we ignore it
if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) {
Expand All @@ -435,6 +427,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
return original_signal;
}

const double dist_to_original_desired_end = get_distance(original_desired_end_point);
const double dist_to_new_desired_end = get_distance(new_desired_end_point);

// If we already passed the desired end point, return the other signal
if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) {
TurnSignalInfo empty_signal_info;
Expand All @@ -445,6 +440,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
return original_signal;
}

const double dist_to_original_required_start =
get_distance(original_required_start_point) - base_link2front_;
const double dist_to_original_required_end = get_distance(original_required_end_point);
const double dist_to_new_required_start =
get_distance(new_required_start_point) - base_link2front_;
const double dist_to_new_required_end = get_distance(new_required_end_point);

if (dist_to_original_desired_start <= dist_to_new_desired_start) {
const auto enable_prior = use_prior_turn_signal(
dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -208,26 +208,25 @@ std::optional<std::pair<size_t, geometry_msgs::msg::Point>> intersectBound(
return std::nullopt;
}

double calcDistanceFromPointToSegment(
double calcSquaredDistanceFromPointToSegment(
const geometry_msgs::msg::Point & segment_start_point,
const geometry_msgs::msg::Point & segment_end_point,
const geometry_msgs::msg::Point & target_point)
{
using tier4_autoware_utils::calcSquaredDistance2d;

const auto & a = segment_start_point;
const auto & b = segment_end_point;
const auto & p = target_point;

const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y);
const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b);
const double squared_segment_length = calcSquaredDistance2d(a, b);
if (0 <= dot_val && dot_val <= squared_segment_length) {
const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x));
const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
return numerator / denominator;
return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length;
}

// target_point is outside the segment.
return std::min(
tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p));
return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p));
}

PolygonPoint transformBoundFrenetCoordinate(
Expand All @@ -238,8 +237,8 @@ PolygonPoint transformBoundFrenetCoordinate(
// find wrong nearest index.
std::vector<double> dist_to_bound_segment_vec;
for (size_t i = 0; i < bound_points.size() - 1; ++i) {
const double dist_to_bound_segment =
calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point);
const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment(
bound_points.at(i), bound_points.at(i + 1), target_point);
dist_to_bound_segment_vec.push_back(dist_to_bound_segment);
}

Expand Down

0 comments on commit eeed846

Please sign in to comment.