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(drivable area expansion): do not over expand beyond the desired width #5640

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 @@ -13,7 +13,7 @@
smoothing:
curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average
max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length
extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied
arc_length_range: 2.0 # [m] arc length range where an expansion distance is initially applied
ego:
extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
extra_front_overhang: 0.5 # [m] extra length to add to the front overhang
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -929,8 +929,8 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM,
planner_data_->drivable_area_expansion_parameters.max_bound_rate);
updateParam(
parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM,
planner_data_->drivable_area_expansion_parameters.extra_arc_length);
parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM,
planner_data_->drivable_area_expansion_parameters.arc_length_range);
updateParam(
parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM,
planner_data_->drivable_area_expansion_parameters.print_runtime);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,14 +35,14 @@ void expand_drivable_area(
PathWithLaneId & path,
const std::shared_ptr<const behavior_path_planner::PlannerData> planner_data);

/// @brief prepare path poses and try to reuse their previously calculated curvatures
/// @brief try to reuse the previous path poses and their previously calculated curvatures
/// @details poses are reused if they do not deviate too much from the current path
/// @param [in] path input path
/// @param [inout] prev_poses previous poses to reuse
/// @param [inout] prev_curvatures previous curvatures to reuse
/// @param [in] ego_point current ego point
/// @param [in] params parameters for reuse criteria and resampling interval
void update_path_poses_and_previous_curvatures(
void reuse_previous_poses(
const PathWithLaneId & path, std::vector<Pose> & prev_poses,
std::vector<double> & prev_curvatures, const Point & ego_point,
const DrivableAreaExpansionParameters & params);
Expand All @@ -57,6 +57,36 @@ void update_path_poses_and_previous_curvatures(
double calculate_minimum_lane_width(
const double curvature_radius, const DrivableAreaExpansionParameters & params);

/// @brief calculate mappings between path poses and the given drivable area bound
/// @param [inout] expansion expansion data to update with the mapping
/// @param [in] path_poses path poses
/// @param [in] bound drivable area bound
/// @param [in] Side left or right side
void calculate_bound_index_mappings(
Expansion & expansion, const std::vector<Pose> & path_poses, const std::vector<Point> & bound,
const Side side);

/// @brief apply expansion distances to all bound points within the given arc length range
/// @param [inout] expansion expansion data to update
/// @param [in] bound drivable area bound
/// @param [in] arc_length_range [m] arc length range where the expansion distances are also applied
/// @param [in] Side left or right side
void apply_arc_length_range_smoothing(
Expansion & expansion, const std::vector<Point> & bound, const double arc_length_range,
const Side side);

/// @brief calculate minimum lane widths and mappings between path and and drivable area bounds
/// @param [in] path_poses path poses
/// @param [in] left_bound left drivable area bound
/// @param [in] right_bound right drivable area bound
/// @param [in] curvatures curvatures at each path point
/// @param [in] params parameters with the vehicle dimensions used to calculate the min lane width
/// @return expansion data (path->bound mappings, min lane widths, ...)
Expansion calculate_expansion(
const std::vector<Pose> & path_poses, const std::vector<Point> & left_bound,
const std::vector<Point> & right_bound, const std::vector<double> & curvatures,
const DrivableAreaExpansionParameters & params);

/// @brief smooth the bound by applying a limit on its rate of change
/// @details rate of change is the lateral distance from the path over the arc length along the path
/// @param [inout] bound_distances bound distances (lateral distance from the path)
Expand All @@ -66,16 +96,16 @@ void apply_bound_change_rate_limit(
std::vector<double> & distances, const std::vector<Point> & bound, const double max_rate);

/// @brief calculate the maximum distance by which a bound can be expanded
/// @param [in] path_poses input path
/// @param [in] bound bound points
/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree
/// @param [in] uncrossable_polygons polygons that limit the bound expansion
/// @param [in] params parameters with the buffer distance to keep with lines,
/// and the static maximum expansion distance
/// @param [in] Side left or right side
std::vector<double> calculate_maximum_distance(
const std::vector<Pose> & path_poses, const std::vector<Point> bound,
const SegmentRtree & uncrossable_lines, const std::vector<Polygon2d> & uncrossable_polygons,
const DrivableAreaExpansionParameters & params);
const std::vector<Point> & bound, const SegmentRtree & uncrossable_lines,
const std::vector<Polygon2d> & uncrossable_polygons,
const DrivableAreaExpansionParameters & params, const Side side);

/// @brief expand a bound by the given lateral distances away from the path
/// @param [inout] bound bound points to expand
Expand All @@ -85,6 +115,14 @@ void expand_bound(
std::vector<Point> & bound, const std::vector<Pose> & path_poses,
const std::vector<double> & distances);

/// @brief calculate the expansion distances of the left and right drivable area bounds
/// @param [inout] expansion expansion data to be updated with the left/right expansion distances
/// @param [in] max_left_expansions maximum left expansion distances
/// @param [in] max_right_expansions maximum right expansion distances
void calculate_expansion_distances(
Expansion & expansion, const std::vector<double> & max_left_expansions,
const std::vector<double> & max_right_expansions);

/// @brief calculate smoothed curvatures
/// @details smoothing is done using a moving average
/// @param [in] poses input poses used to calculate the curvatures
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ struct DrivableAreaExpansionParameters
"dynamic_expansion.smoothing.curvature_average_window";
static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM =
"dynamic_expansion.smoothing.max_bound_rate";
static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM =
"dynamic_expansion.smoothing.extra_arc_length";
static constexpr auto SMOOTHING_ARC_LENGTH_RANGE_PARAM =
"dynamic_expansion.smoothing.arc_length_range";
static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime";

// static expansion
Expand All @@ -78,7 +78,7 @@ struct DrivableAreaExpansionParameters
double max_expansion_distance{};
double max_path_arc_length{};
double resample_interval{};
double extra_arc_length{};
double arc_length_range{};
double max_reuse_deviation{};
bool avoid_dynamic_objects{};
bool print_runtime{};
Expand All @@ -103,7 +103,7 @@ struct DrivableAreaExpansionParameters
extra_width = node.declare_parameter<double>(EGO_EXTRA_WIDTH);
curvature_average_window = node.declare_parameter<int>(SMOOTHING_CURVATURE_WINDOW_PARAM);
max_bound_rate = node.declare_parameter<double>(SMOOTHING_MAX_BOUND_RATE_PARAM);
extra_arc_length = node.declare_parameter<double>(SMOOTHING_EXTRA_ARC_LENGTH_PARAM);
arc_length_range = node.declare_parameter<double>(SMOOTHING_ARC_LENGTH_RANGE_PARAM);
max_path_arc_length = node.declare_parameter<double>(MAX_PATH_ARC_LENGTH_PARAM);
resample_interval = node.declare_parameter<double>(RESAMPLE_INTERVAL_PARAM);
max_reuse_deviation = node.declare_parameter<double>(MAX_REUSE_DEVIATION_PARAM);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
namespace drivable_area_expansion
{
/// @brief project a point to a segment
/// @details the distance is signed based on the side of the point: left=positive, right=negative
maxime-clem marked this conversation as resolved.
Show resolved Hide resolved
/// @param p point to project on the segment
/// @param p1 first segment point
/// @param p2 second segment point
Expand All @@ -37,22 +36,18 @@ inline PointDistance point_to_segment_projection(
const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};

const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x();
const auto dist_sign = cross < 0.0 ? -1.0 : 1.0;

const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
if (c1 <= 0) return {p1, boost::geometry::distance(p, p1) * dist_sign};
if (c1 <= 0) return {p1, boost::geometry::distance(p, p1)};

const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign};
if (c2 <= c1) return {p2, boost::geometry::distance(p, p2)};

const auto projection = p1 + (p2_vec * c1 / c2);
const auto projection_point = Point2d{projection.x(), projection.y()};
return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign};
return {projection_point, boost::geometry::distance(p, projection_point)};
}

/// @brief project a point to a line
/// @details the distance is signed based on the side of the point: left=positive, right=negative
/// @param p point to project on the line
/// @param p1 first line point
/// @param p2 second line point
Expand All @@ -63,14 +58,11 @@ inline PointDistance point_to_line_projection(
const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};

const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x();
const auto dist_sign = cross < 0.0 ? -1.0 : 1.0;

const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
const auto projection = p1 + (p2_vec * c1 / c2);
const auto projection_point = Point2d{projection.x(), projection.y()};
return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign};
return {projection_point, boost::geometry::distance(p, projection_point)};
}

/// @brief project a point to a linestring
Expand All @@ -84,7 +76,7 @@ inline Projection point_to_linestring_projection(const Point2d & p, const LineSt
auto arc_length = 0.0;
for (auto ls_it = ls.begin(); std::next(ls_it) != ls.end(); ++ls_it) {
const auto pd = point_to_segment_projection(p, *ls_it, *(ls_it + 1));
if (std::abs(pd.distance) < std::abs(closest.distance)) {
if (pd.distance < closest.distance) {
closest.projected_point = pd.point;
closest.distance = pd.distance;
closest.arc_length = arc_length + boost::geometry::distance(*ls_it, pd.point);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@

#include <boost/geometry/index/rtree.hpp>

#include <optional>
#include <vector>

namespace drivable_area_expansion
{
using autoware_auto_perception_msgs::msg::PredictedObjects;
Expand All @@ -45,16 +48,27 @@ using SegmentRtree = boost::geometry::index::rtree<Segment2d, boost::geometry::i

struct PointDistance
{
Point2d point;
double distance{0.0};
Point2d point{};
double distance{};
};
struct Projection
{
Point2d projected_point;
double distance{0.0};
double arc_length{0.0};
Point2d projected_point{};
double distance{};
double arc_length{};
};
enum Side { LEFT, RIGHT };

struct Expansion
{
// mappings from bound index
std::vector<double> left_distances;
std::vector<double> right_distances;
// mappings from path index
std::vector<size_t> left_bound_indexes;
std::vector<PointDistance> left_projections;
std::vector<size_t> right_bound_indexes;
std::vector<PointDistance> right_projections;
std::vector<double> min_lane_widths;
};
} // namespace drivable_area_expansion
#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_
Loading
Loading