Skip to content

Commit

Permalink
feat(avoidance): consider objects on shift side lane (autowarefoundat…
Browse files Browse the repository at this point in the history
…ion#6252)

* fix(avoidance): check safety for only moving objects

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

* feat(avoidance): consider ignore object in avoid margin calculation

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

* fix(avoidance): use nearest overhang point

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

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota authored and TomohitoAndo committed Apr 1, 2024
1 parent 67f75d2 commit 2f2407c
Show file tree
Hide file tree
Showing 8 changed files with 138 additions and 119 deletions.
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 @@ -169,10 +159,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 @@ -272,8 +262,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 @@ class AvoidanceHelper
{
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);
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 @@ -113,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
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 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
"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 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
"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;
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 @@ MarkerArray createDrivableBounds(
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 @@ MarkerArray createDrivableBounds(
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
Expand Up @@ -234,14 +234,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat
// 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 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
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 @@ void AvoidanceModule::fillAvoidanceTargetObjects(

// 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 @@ -929,10 +931,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
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 All @@ -941,6 +939,21 @@ BehaviorModuleOutput AvoidanceModule::plan()
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(
clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
} else {
current_drivable_area_info.obstacles.clear();
}

output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
Expand Down Expand Up @@ -1150,8 +1163,8 @@ bool AvoidanceModule::isValidShiftLine(
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

0 comments on commit 2f2407c

Please sign in to comment.