Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(avoidance): consider objects on shift side lane #6252

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -30,16 +30,6 @@

namespace behavior_path_planner
{
namespace
{
lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
{
lanelet::BasicLineString3d ret{};
std::for_each(
bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); });
return ret;
}
} // namespace
AvoidanceByLaneChange::AvoidanceByLaneChange(
const std::shared_ptr<LaneChangeParameters> & parameters,
std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters)
Expand Down Expand Up @@ -172,10 +162,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
// calc drivable bound
const auto shorten_lanes =
utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes);
data.left_bound = toLineString3d(utils::calcBound(
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true));
data.right_bound = toLineString3d(utils::calcBound(
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false));
data.left_bound = utils::calcBound(
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true);
data.right_bound = utils::calcBound(
data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false);

// get related objects from dynamic_objects, and then separates them as target objects and non
// target objects
Expand Down Expand Up @@ -275,8 +265,8 @@ ObjectData AvoidanceByLaneChange::createObjectData(
: Direction::RIGHT;

// Find the footprint point closest to the path, set to object_data.overhang_distance.
object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance(
object_data, data.reference_path, object_data.overhang_pose.position);
object_data.overhang_points =
utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path);

// Check whether the the ego should avoid the object.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -336,12 +336,8 @@ struct ObjectData // avoidance target
{
ObjectData() = default;

ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang)
: object(std::move(obj)),
to_centerline(lat),
longitudinal(lon),
length(len),
overhang_dist(overhang)
ObjectData(PredictedObject obj, double lat, double lon, double len)
: object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len)
{
}

Expand All @@ -365,9 +361,6 @@ struct ObjectData // avoidance target
// longitudinal length of vehicle, in Frenet coordinate
double length{0.0};

// lateral distance to the closest footprint, in Frenet coordinate
double overhang_dist{0.0};

// lateral shiftable ratio
double shiftable_ratio{0.0};

Expand All @@ -392,9 +385,6 @@ struct ObjectData // avoidance target
// the position at the detected moment
Pose init_pose;

// the position of the overhang
Pose overhang_pose;

// envelope polygon
Polygon2d envelope_poly{};

Expand Down Expand Up @@ -425,14 +415,17 @@ struct ObjectData // avoidance target
// object direction.
Direction direction{Direction::NONE};

// overhang points (sort by distance)
std::vector<std::pair<double, Point>> overhang_points{};

// unavoidable reason
std::string reason{};

// lateral avoid margin
std::optional<double> avoid_margin{std::nullopt};

// the nearest bound point (use in road shoulder distance calculation)
std::optional<Point> nearest_bound_point{std::nullopt};
std::optional<std::pair<Point, Point>> narrowest_place{std::nullopt};
};
using ObjectDataArray = std::vector<ObjectData>;

Expand Down Expand Up @@ -541,9 +534,9 @@ struct AvoidancePlanningData

std::vector<DrivableLanes> drivable_lanes{};

lanelet::BasicLineString3d right_bound{};
std::vector<Point> right_bound{};

lanelet::BasicLineString3d left_bound{};
std::vector<Point> left_bound{};

bool safe{false};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,8 @@
{
using utils::avoidance::calcShiftLength;

const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin);
const auto shift_length =
calcShiftLength(is_on_right, object.overhang_points.front().first, margin);

Check warning on line 180 in planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp#L180

Added line #L180 was not covered by tests
return is_on_right ? std::min(shift_length, getLeftShiftBound())
: std::max(shift_length, getRightShiftBound());
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);

double calcEnvelopeOverhangDistance(
const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose);
std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
const ObjectData & object_data, const PathWithLaneId & path);

void setEndData(
AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
Expand Down Expand Up @@ -127,6 +127,10 @@ void filterTargetObjects(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

void updateRoadShoulderDistance(
AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);

void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line);
Expand Down
12 changes: 6 additions & 6 deletions planning/behavior_path_avoidance_module/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
int32_t ret = 0;

for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) {
ret <<= sizeof(int8_t) * 8;

Check warning on line 50 in planning/behavior_path_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/debug.cpp#L50

Added line #L50 was not covered by tests
ret |= uuid.uuid.at(i);
}

Expand Down Expand Up @@ -113,7 +113,7 @@
MarkerArray msg;

for (const auto & object : objects) {
if (!object.nearest_bound_point.has_value()) {
if (!object.narrowest_place.has_value()) {
continue;
}

Expand All @@ -122,8 +122,8 @@
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999));

marker.points.push_back(object.overhang_pose.position);
marker.points.push_back(object.nearest_bound_point.value());
marker.points.push_back(object.narrowest_place.value().first);
marker.points.push_back(object.narrowest_place.value().second);
marker.id = uuidToInt32(object.object.object_id);
msg.markers.push_back(marker);
}
Expand All @@ -133,7 +133,7 @@
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));

marker.pose.position = object.nearest_bound_point.value();
marker.pose.position = object.narrowest_place.value().second;

Check warning on line 136 in planning/behavior_path_avoidance_module/src/debug.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/debug.cpp#L136

Added line #L136 was not covered by tests
std::ostringstream string_stream;
string_stream << object.to_road_shoulder_distance << "[m]";
marker.text = string_stream.str();
Expand Down Expand Up @@ -469,7 +469,7 @@
createMarkerColor(r, g, b, 0.999));

for (const auto & p : data.right_bound) {
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
marker.points.push_back(p);
}

msg.markers.push_back(marker);
Expand All @@ -482,7 +482,7 @@
createMarkerColor(r, g, b, 0.999));

for (const auto & p : data.left_bound) {
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
marker.points.push_back(p);
}

msg.markers.push_back(marker);
Expand Down
33 changes: 23 additions & 10 deletions planning/behavior_path_avoidance_module/src/scene.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1124 to 1135, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity

The mean cyclomatic complexity increases from 4.67 to 4.71, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -234,14 +234,14 @@
// calc drivable bound
auto tmp_path = getPreviousModuleOutput().path;
const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes);
data.left_bound = toLineString3d(utils::calcBound(
data.left_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
parameters_->use_freespace_areas, true));
data.right_bound = toLineString3d(utils::calcBound(
parameters_->use_freespace_areas, true);
data.right_bound = utils::calcBound(
getPreviousModuleOutput().path, planner_data_, shorten_lanes,
parameters_->use_hatched_road_markings, parameters_->use_intersection_areas,
parameters_->use_freespace_areas, false));
parameters_->use_freespace_areas, false);

// reference path
if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
Expand Down Expand Up @@ -294,6 +294,7 @@
using utils::avoidance::filterTargetObjects;
using utils::avoidance::getTargetLanelets;
using utils::avoidance::separateObjectsByPath;
using utils::avoidance::updateRoadShoulderDistance;

// Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
constexpr double MARGIN = 10.0;
Expand All @@ -315,6 +316,7 @@

// Filter out the objects to determine the ones to be avoided.
filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
updateRoadShoulderDistance(data, planner_data_, parameters_);

// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
Expand Down Expand Up @@ -928,19 +930,30 @@
{
DrivableAreaInfo current_drivable_area_info;
// generate drivable lanes
current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes;
// generate obstacle polygons
current_drivable_area_info.obstacles =
utils::avoidance::generateObstaclePolygonsForDrivableArea(
avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
// expand intersection areas
current_drivable_area_info.enable_expanding_intersection_areas =
parameters_->use_intersection_areas;
// expand freespace areas
current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
// generate obstacle polygons
if (parameters_->enable_bound_clipping) {
ObjectDataArray clip_objects;
// If avoidance is executed by both behavior and motion, only non-avoidable object will be
// extracted from the drivable area.
std::for_each(
data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) {
if (!object.is_avoidable) clip_objects.push_back(object);
});
current_drivable_area_info.obstacles =
utils::avoidance::generateObstaclePolygonsForDrivableArea(

Check warning on line 952 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L952

Added line #L952 was not covered by tests
clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
} else {

Check warning on line 954 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_avoidance_module/src/scene.cpp#L954

Added line #L954 was not covered by tests
current_drivable_area_info.obstacles.clear();
}

Check warning on line 956 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

AvoidanceModule::plan has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 956 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Large Method

AvoidanceModule::plan is no longer above the threshold for lines of code

output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
Expand Down Expand Up @@ -1150,8 +1163,8 @@
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);
const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound));
const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound));
for (size_t i = start_idx; i <= end_idx; ++i) {
const auto p = getPoint(path.points.at(i));
lanelet::BasicPoint2d basic_point{p.x, p.y};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -235,7 +235,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;

const auto infeasible =
std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
if (infeasible) {
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
Expand Down
Loading
Loading