From 6eefb62ed6f5e7c9b343bed80f25b69c72a9b256 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 5 Oct 2023 22:34:16 +0900 Subject: [PATCH 01/17] Switch to new curvature based dynamic drivable area expansion Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.hpp | 21 +- .../drivable_area_expansion/parameters.hpp | 10 +- .../utils/drivable_area_expansion/types.hpp | 3 + .../drivable_area_expansion.cpp | 403 ++++++------------ .../behavior_path_planner/src/utils/utils.cpp | 2 +- .../test/test_drivable_area_expansion.cpp | 2 +- 6 files changed, 146 insertions(+), 295 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 8293e0a1d10a9..79c1816443c5f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -27,27 +27,12 @@ namespace drivable_area_expansion { -/// @brief Expand the drivable area based on the projected ego footprint along the path +/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions /// @param[inout] path path whose drivable area will be expanded -/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) -/// @param[in] path_lanes lanelets of the path +/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) void expandDrivableArea( PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes); - -/// @brief Create a polygon combining the drivable area of a path and some expansion polygons -/// @param[in] path path and its drivable area -/// @param[in] expansion_polygons polygons to add to the drivable area -/// @return expanded drivable area polygon -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); - -/// @brief Update the drivable area of the given path with the given polygon -/// @details this function splits the polygon into a left and right bound and sets it in the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] expanded_drivable_area polygon of the new drivable area -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area); + const std::shared_ptr planner_data); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index d93cb65d0554d..4529cb4e2962c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -90,9 +90,7 @@ struct DrivableAreaExpansionParameters std::vector avoid_linestring_types{}; bool compensate_uncrossable_lines = false; double compensate_extra_dist{}; - bool replan_enable{}; - double replan_max_deviation{}; - bool debug_print{}; + vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -133,11 +131,7 @@ struct DrivableAreaExpansionParameters replan_max_deviation = node.declare_parameter(REPLAN_MAX_DEVIATION_PARAM); debug_print = node.declare_parameter(DEBUG_PRINT_PARAM); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - ego_left_offset = vehicle_info.max_lateral_offset_m; - ego_right_offset = vehicle_info.min_lateral_offset_m; - ego_rear_offset = vehicle_info.min_longitudinal_offset_m; - ego_front_offset = vehicle_info.max_longitudinal_offset_m; + vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e300a347e47a8..ce161d327cc0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -50,5 +50,8 @@ struct Projection double distance; double arc_length; }; + +enum Side { LEFT, RIGHT }; + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 102ed5b8c5fd1..9e5a91e15b824 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -26,6 +27,15 @@ #include +// for writing the svg file +#include +#include +// for the geometry types +#include +// for the svg mapper +#include +#include + namespace drivable_area_expansion { @@ -50,16 +60,109 @@ std::vector crop_and_resample( return resampled_path.points; } +point_t convert_point(const Point & p) +{ + return point_t{p.x, p.y}; +} + +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params) +{ + const auto k = curvature_radius; + const auto a = params.vehicle_info.front_overhang_m + params.ego_extra_front_offset; + // TODO(Maxime): update param + const auto w = params.vehicle_info.vehicle_width_m + params.ego_extra_left_offset; + const auto l = params.vehicle_info.wheel_base_m; + return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w); +} + +std::vector calculate_minimum_lateral_distances( + const std::vector & points, const std::vector bound, + const std::vector curvatures, const Side side, + const DrivableAreaExpansionParameters & params) +{ + size_t lb_idx = 0; + std::vector min_lateral_distances(bound.size(), 0.0); + for (auto path_idx = 0UL; path_idx < points.size(); ++path_idx) { + const auto & path_p = points[path_idx].point.pose; + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); + const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? -1.0 : 1.0); + const auto offset_pose = + tier4_autoware_utils::calcOffsetPose(path_p, 0.0, side_distance, 0.0).position; + for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { + const auto & prev_p = bound[bound_idx]; + const auto & next_p = bound[bound_idx + 1]; + const auto intersection_point = + tier4_autoware_utils::intersect(offset_pose, path_p.position, prev_p, next_p); + if (intersection_point) { + lb_idx = bound_idx; + const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_pose); + if (dist > min_lateral_distances[bound_idx]) min_lateral_distances[bound_idx] = dist; + if (dist > min_lateral_distances[bound_idx + 1]) + min_lateral_distances[bound_idx + 1] = dist; + break; // TODO(Maxime): should we rm this break to handle multiple segments intersect ? + } + } + } + return min_lateral_distances; +} + +void apply_velocity_limit( + std::vector & distance_vector, const std::vector & bound_vector, + const double max_velocity) +{ + const auto apply_max_vel = [&](auto & dist_vector, const auto from, const auto to) { + if (dist_vector[from] > dist_vector[to]) { + const auto arc_length = + tier4_autoware_utils::calcDistance2d(bound_vector[from], bound_vector[to]); + const auto smoothed_dist = dist_vector[from] - arc_length * max_velocity; + dist_vector[to] = std::max(dist_vector[to], smoothed_dist); + } + }; + for (auto idx = 0LU; idx + 1 < distance_vector.size(); ++idx) + apply_max_vel(distance_vector, idx, idx + 1); + for (auto idx = distance_vector.size() - 1; idx > 0; --idx) + apply_max_vel(distance_vector, idx, idx - 1); +} + +void expand_bound( + std::vector & bound, const std::vector & path_points, + const std::vector new_distances) +{ + linestring_t path_ls; + for (const auto & p : path_points) path_ls.push_back(convert_point(p.point.pose.position)); + for (auto idx = 0LU; idx < bound.size(); ++idx) { + const auto bound_p = convert_point(bound[idx]); + const auto projection = point_to_linestring_projection(bound_p, path_ls); + const auto expansion_ratio = + (new_distances[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + if (expansion_ratio > 1.0) { + const auto & path_p = projection.projected_point; + const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + bound[idx].x = expanded_p.x(); + bound[idx].y = expanded_p.y(); + } + } +} + void expandDrivableArea( PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes) + const std::shared_ptr planner_data) { - if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return; + // Declare a stream and an SVG mapper + std::ofstream svg("/home/mclement/Pictures/da.svg"); // /!\ CHANGE PATH + boost::geometry::svg_mapper mapper(svg, 400, 400); + linestring_t left_ls, right_ls; + for (const auto & p : path.left_bound) left_ls.push_back(convert_point(p)); + for (const auto & p : path.right_bound) right_ls.push_back(convert_point(p)); + mapper.add(left_ls); + mapper.map(left_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); + mapper.add(right_ls); + mapper.map(right_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); - tier4_autoware_utils::StopWatch stopwatch; const auto & params = planner_data->drivable_area_expansion_parameters; - const auto & dynamic_objects = *planner_data->dynamic_object; const auto & route_handler = *planner_data->route_handler; stopwatch.tic("calc_replan"); @@ -82,269 +185,35 @@ void expandDrivableArea( for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) uncrossable_lines_in_range.push_back(line); - const auto extra_uncrossable_lines_duration = stopwatch.toc("extract_uncrossable_lines"); - - stopwatch.tic("crop"); - const auto points = crop_and_resample(path.points, params, replan_index); - const auto crop_duration = stopwatch.toc("crop"); - stopwatch.tic("footprints"); - const auto path_footprints = createPathFootprints(points, params); - const auto footprints_duration = stopwatch.toc("footprints"); - const auto predicted_paths = params.avoid_dynamic_objects - ? createObjectFootprints(dynamic_objects, params) - : multi_polygon_t{}; - stopwatch.tic("exp_polys"); - auto expansion_polygons = - params.expansion_method == "lanelet" - ? createExpansionLaneletPolygons( - path_lanes, route_handler, path_footprints, predicted_paths, params) - : createExpansionPolygons( - path, path_footprints, predicted_paths, uncrossable_lines_in_range, params); - if (is_replanning) expansion_polygons.push_back(prev_expanded_drivable_area); - const auto exp_polygons_duration = stopwatch.toc("exp_polys"); - stopwatch.tic("exp_da"); - const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons); - const auto exp_da_duration = stopwatch.toc("exp_da"); - - linestring_t path_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - - stopwatch.tic("update"); - updateDrivableAreaBounds(path, expanded_drivable_area); - const auto update_duration = stopwatch.toc("update"); - planner_data->drivable_area_expansion_replan_checker.set_previous(path); - - if (params.debug_print) { - std::printf("Dynamic drivable area expansion runtime: %2.2fms\n", stopwatch.toc()); - std::printf("\tcalc_replan: %2.2fms\n", calc_replan_duration); - std::printf("\textract_lines: %2.2fms\n", extra_uncrossable_lines_duration); - std::printf("\tcrop: %2.2fms\n", crop_duration); - std::printf("\tfootprints: %2.2fms\n", footprints_duration); - std::printf("\texp_polys: %2.2fms\n", exp_polygons_duration); - std::printf("\texp_da: %2.2fms\n", exp_da_duration); - std::printf("\tupdate: %2.2fms\n", update_duration); - std::cout << std::endl; - } -} - -point_t convert_point(const Point & p) -{ - return point_t{p.x, p.y}; -} - -Point convert_point(const point_t & p) -{ - return Point().set__x(p.x()).set__y(p.y()); -} - -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) -{ - polygon_t original_da_poly; - original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); - for (const auto & p : path.left_bound) original_da_poly.outer().push_back(convert_point(p)); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - original_da_poly.outer().push_back(convert_point(*it)); - original_da_poly.outer().push_back(original_da_poly.outer().front()); - - multi_polygon_t unions; - auto expanded_da_poly = original_da_poly; - for (const auto & p : expansion_polygons) { - unions.clear(); - boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() == 1) // union of overlapping polygons should produce a single polygon - expanded_da_poly = unions[0]; - } - return expanded_da_poly; -} - -void copy_z_over_arc_length( - const std::vector & from, std::vector & to) -{ - if (from.empty() || to.empty()) return; - to.front().z = from.front().z; - if (from.size() < 2 || to.size() < 2) return; - to.back().z = from.back().z; - auto i_from = 1lu; - auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); - auto s_to = 0.0; - auto s_from_prev = 0.0; - for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { - s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); - for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { - s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); - } - if (s_from - s_from_prev != 0.0) { - const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); - to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); - } else { - to[i_to].z = to[i_to - 1].z; - } + const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto predicted_paths = createObjectFootprints(*planner_data->dynamic_object, params); + + const auto curvatures = motion_utils::calcCurvature(points); + std::vector smoothed_curvatures(curvatures.size()); + constexpr auto move_avg_window = 3; + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= move_avg_window ? i - move_avg_window : 0); + const auto to_idx = std::min(i + move_avg_window, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); } -} -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) -{ - const auto original_left_bound = path.left_bound; - const auto original_right_bound = path.right_bound; - const auto is_left_of_path = [&](const point_t & p) { - return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; - }; - // prepare delimiting lines: start and end of the original expanded drivable area - const auto start_segment = - segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; - const auto end_segment = - segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; - point_t start_segment_center; - boost::geometry::centroid(start_segment, start_segment_center); - const auto path_start_segment = - segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; - point_t end_segment_center; - boost::geometry::centroid(end_segment, end_segment_center); - const auto path_end_segment = - segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; - const auto segment_to_line_intersection = - [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { - const auto line = Eigen::Hyperplane::Through(q1, q2); - const auto segment = Eigen::Hyperplane::Through(p1, p2); - const auto intersection = line.intersection(segment); - std::optional result; - const auto is_on_segment = - (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() - : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && - (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() - : intersection.y() <= p1.y() && intersection.y() >= p2.y()); - if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; - return result; - }; - // find intersection between the expanded drivable area and the delimiting lines - const auto & da = expanded_drivable_area.outer(); - struct Intersection - { - point_t intersection_point; - ring_t::const_iterator segment_it; - double distance = std::numeric_limits::max(); - explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} - void update(const point_t & p, const ring_t::const_iterator & it, const double dist) - { - intersection_point = p; - segment_it = it; - distance = dist; - } - }; - Intersection start_left(da.end()); - Intersection end_left(da.end()); - Intersection start_right(da.end()); - Intersection end_right(da.end()); - for (auto it = da.begin(); it != da.end(); ++it) { - if (boost::geometry::distance(*it, start_segment.first) < 1e-3) - start_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) - start_right.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) - end_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) - end_right.update(*it, it, 0.0); - const auto inter_start = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) - : segment_to_line_intersection( - *it, *std::next(it), start_segment.first, start_segment.second); - if (inter_start) { - const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left = is_left_of_path(*inter_start); - if (is_left && dist < start_left.distance) - start_left.update(*inter_start, it, dist); - else if (!is_left && dist < start_right.distance) - start_right.update(*inter_start, it, dist); - } - const auto inter_end = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) - : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); - if (inter_end) { - const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left = is_left_of_path(*inter_end); - if (is_left && dist < end_left.distance) - end_left.update(*inter_end, it, dist); - else if (!is_left && dist < end_right.distance) - end_right.update(*inter_end, it, dist); - } - } - if (start_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.first) < - boost::geometry::distance(b, start_segment.first); - }); - start_left.update(*closest_it, closest_it, 0.0); - } - if (start_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.second) < - boost::geometry::distance(b, start_segment.second); - }); - start_right.update(*closest_it, closest_it, 0.0); - } - if (end_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.first) < - boost::geometry::distance(b, end_segment.first); - }); - end_left.update(*closest_it, closest_it, 0.0); - } - if (end_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.second) < - boost::geometry::distance(b, end_segment.second); - }); - end_right.update(*closest_it, closest_it, 0.0); - } - - // extract the expanded left and right bound from the expanded drivable area - path.left_bound.clear(); - path.right_bound.clear(); - path.left_bound.push_back(convert_point(start_left.intersection_point)); - path.right_bound.push_back(convert_point(start_right.intersection_point)); - if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) - path.right_bound.push_back(convert_point(*start_right.segment_it)); - if (start_left.segment_it < end_left.segment_it) { - for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) - path.left_bound.push_back(convert_point(*it)); - for (auto it = da.begin(); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } - if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) - path.left_bound.push_back(convert_point(end_left.intersection_point)); - if (start_right.segment_it < end_right.segment_it) { - for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) - path.right_bound.push_back(convert_point(*it)); - for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } - if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) - path.right_bound.push_back(convert_point(end_right.intersection_point)); - // remove possible duplicated points - const auto point_cmp = [](const auto & p1, const auto & p2) { - return p1.x == p2.x && p1.y == p2.y; - }; - path.left_bound.erase( - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); - path.right_bound.erase( - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), - path.right_bound.end()); - copy_z_over_arc_length(original_left_bound, path.left_bound); - copy_z_over_arc_length(original_right_bound, path.right_bound); + auto new_left_distances = + calculate_minimum_lateral_distances(points, path.left_bound, smoothed_curvatures, LEFT, params); + auto new_right_distances = calculate_minimum_lateral_distances( + points, path.right_bound, smoothed_curvatures, RIGHT, params); + + // smooth the distances + constexpr auto max_bound_vel = 0.5; // TODO(Maxime): param + apply_velocity_limit(new_left_distances, path.left_bound, max_bound_vel); + apply_velocity_limit(new_right_distances, path.right_bound, max_bound_vel); + // limit the distances based on the uncrossable lines + // limit the distances based on the total width (left + right < min_lane_width) + + // update the points based on the distances + // TODO(Maxime): add an arc length offset / margin ? + expand_bound(path.left_bound, points, new_left_distances); + expand_bound(path.right_bound, points, new_right_distances); } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index bae591ca16d40..914885dc3c2bc 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1532,7 +1532,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); + drivable_area_expansion::expandDrivableArea(path, planner_data); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 7a5bb68decdfa..f5797a94ed0fb 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -275,7 +275,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) planner_data.route_handler = std::make_shared(route_handler); // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( - path, std::make_shared(planner_data), path_lanes); + path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { From f68dab777d53ad321d04747313d97dd086d9bb10 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 9 Oct 2023 12:30:25 +0900 Subject: [PATCH 02/17] Cleanup + remove the old code Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion/expansion.hpp | 53 ----- .../drivable_area_expansion/footprints.hpp | 8 +- .../drivable_area_expansion/map_utils.hpp | 11 +- .../path_projection.hpp | 36 +-- .../utils/drivable_area_expansion/types.hpp | 27 ++- .../drivable_area_expansion.cpp | 151 ++++++------ .../drivable_area_expansion/expansion.cpp | 214 ------------------ .../drivable_area_expansion/footprints.cpp | 46 ++-- .../drivable_area_expansion/map_utils.cpp | 23 +- .../test/test_drivable_area_expansion.cpp | 172 +++----------- 10 files changed, 196 insertions(+), 545 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp index 70cc8a8bc5925..ab9c36be0d4fc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp @@ -27,59 +27,6 @@ namespace drivable_area_expansion { -/// @brief Calculate the distance limit required for the polygon to not cross the limit lines -/// @details Calculate the minimum distance from base_ls to an intersection of limit_lines and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_lines lines we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines); - -/// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. -/// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_polygons polygons we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons); - -/// @brief Create a polygon from a base line with a given expansion distance -/// @param[in] base_ls base linestring from which the polygon is created -/// @param[in] dist desired expansion distance from the base line -/// @param[in] is_left_side desired side of the expansion from the base line -/// @return expansion polygon -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path path and its drivable area -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path_lanes lanelets of the current path -/// @param[in] route_handler route handler -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 8fc0157710dc8..fc36b7b946a73 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -43,19 +43,19 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y); +Polygon2d translatePolygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint); +Polygon2d createFootprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multi_polygon_t createObjectFootprints( +MultiPolygon2d createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); @@ -63,7 +63,7 @@ multi_polygon_t createObjectFootprints( /// @param[in] path the path for which to create a footprint /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path -multi_polygon_t createPathFootprints( +MultiPolygon2d createPathFootprints( const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 4524bd2be2299..8ec4fb5ba40cf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -24,12 +25,14 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map +/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map -/// @param[in] uncrossable_types types that cannot be crossed +/// @param[in] ego_point point of the current ego position +/// @param[in] params parameters with linestring types that cannot be crossed and maximum range /// @return the uncrossable linestrings -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); +MultiLineString2d extractUncrossableLines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params); /// @brief Determine if the given linestring has one of the given types /// @param[in] ls linestring to check diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 3e2b177f59167..93afaad825582 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -33,10 +33,10 @@ namespace drivable_area_expansion /// @param p2 second segment point /// @return projected point and corresponding distance inline PointDistance point_to_segment_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + 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; @@ -48,7 +48,7 @@ inline PointDistance point_to_segment_projection( if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -59,10 +59,10 @@ inline PointDistance point_to_segment_projection( /// @param p2 second line point /// @return projected point and corresponding distance inline PointDistance point_to_line_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + 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; @@ -70,7 +70,7 @@ inline PointDistance point_to_line_projection( 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 = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -78,7 +78,7 @@ inline PointDistance point_to_line_projection( /// @param p point to project /// @param ls linestring /// @return projected point, corresponding distance, and arc length along the linestring -inline Projection point_to_linestring_projection(const point_t & p, const linestring_t & ls) +inline Projection point_to_linestring_projection(const Point2d & p, const LineString2d & ls) { Projection closest; closest.distance = std::numeric_limits::max(); @@ -100,14 +100,14 @@ inline Projection point_to_linestring_projection(const point_t & p, const linest /// @param p2 second vector point /// @param dist distance /// @return point p such that (p1,p) is orthogonal to (p1,p2) at the given distance -inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const double dist) +inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const double dist) { auto base = p1; auto normal_vector = p2; boost::geometry::subtract_point(normal_vector, base); boost::geometry::detail::vec_normalize(normal_vector); boost::geometry::multiply_value(normal_vector, dist); - return point_t{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; + return Point2d{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; } /// @brief interpolate between two points @@ -115,7 +115,7 @@ inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const /// @param b second point /// @param ratio interpolation ratio such that 0 yields a, and 1 yields b /// @return point interpolated between a and b as per the given ratio -inline point_t lerp_point(const point_t & a, const point_t & b, const double ratio) +inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; } @@ -125,10 +125,10 @@ inline point_t lerp_point(const point_t & a, const point_t & b, const double rat /// @param arc_length arc length along the reference linestring of the resulting point /// @param distance distance from the reference linestring of the resulting point /// @return point at the distance and arc length relative to the reference linestring -inline segment_t linestring_to_point_projection( - const linestring_t & ls, const double arc_length, const double distance) +inline Segment2d linestring_to_point_projection( + const LineString2d & ls, const double arc_length, const double distance) { - if (ls.empty()) return segment_t{}; + if (ls.empty()) return Segment2d{}; if (ls.size() == 1lu) return {ls.front(), ls.front()}; auto ls_iterator = ls.begin(); auto prev_arc_length = 0.0; @@ -156,10 +156,10 @@ inline segment_t linestring_to_point_projection( /// @param from_arc_length arc length of the first point of the sub linestring /// @param to_arc_length arc length of the last point of the sub linestring /// @return sub linestring -inline linestring_t sub_linestring( - const linestring_t & ls, const double from_arc_length, const double to_arc_length) +inline LineString2d sub_linestring( + const LineString2d & ls, const double from_arc_length, const double to_arc_length) { - linestring_t sub_ls; + LineString2d sub_ls; if (from_arc_length >= to_arc_length || ls.empty()) throw(std::runtime_error("sub_linestring: bad inputs")); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index ce161d327cc0e..353e1eef04725 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -30,27 +30,32 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; -using point_t = tier4_autoware_utils::Point2d; -using multi_point_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using ring_t = tier4_autoware_utils::LinearRing2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::Segment2d; struct PointDistance { - point_t point; + Point2d point; double distance; }; struct Projection { - point_t projected_point; + Point2d projected_point; double distance; double arc_length; }; - +struct BoundExpansion +{ + double expansion_distance = 0.0; + Point expansion_point{}; + Point2d path_projection_point{}; + size_t original_bound_point_idx{}; +}; enum Side { LEFT, RIGHT }; } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 9e5a91e15b824..988dc1d7f4c5e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -38,7 +38,6 @@ namespace drivable_area_expansion { - std::vector crop_and_resample( const std::vector & points, const DrivableAreaExpansionParameters & params, const size_t first_idx) @@ -60,9 +59,9 @@ std::vector crop_and_resample( return resampled_path.points; } -point_t convert_point(const Point & p) +Point2d convert_point(const Point & p) { - return point_t{p.x, p.y}; + return Point2d{p.x, p.y}; } double calculate_minimum_lane_width( @@ -76,68 +75,72 @@ double calculate_minimum_lane_width( return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w); } -std::vector calculate_minimum_lateral_distances( +std::vector calculate_minimum_expansions( const std::vector & points, const std::vector bound, const std::vector curvatures, const Side side, const DrivableAreaExpansionParameters & params) { + std::vector bound_expansions(bound.size()); size_t lb_idx = 0; - std::vector min_lateral_distances(bound.size(), 0.0); for (auto path_idx = 0UL; path_idx < points.size(); ++path_idx) { const auto & path_p = points[path_idx].point.pose; if (curvatures[path_idx] == 0.0) continue; const auto curvature_radius = 1 / curvatures[path_idx]; const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? -1.0 : 1.0); - const auto offset_pose = + const auto offset_point = tier4_autoware_utils::calcOffsetPose(path_p, 0.0, side_distance, 0.0).position; for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { const auto & prev_p = bound[bound_idx]; const auto & next_p = bound[bound_idx + 1]; const auto intersection_point = - tier4_autoware_utils::intersect(offset_pose, path_p.position, prev_p, next_p); + tier4_autoware_utils::intersect(offset_point, path_p.position, prev_p, next_p); if (intersection_point) { lb_idx = bound_idx; - const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_pose); - if (dist > min_lateral_distances[bound_idx]) min_lateral_distances[bound_idx] = dist; - if (dist > min_lateral_distances[bound_idx + 1]) - min_lateral_distances[bound_idx + 1] = dist; + const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); + if (dist > bound_expansions[bound_idx].expansion_distance) { + bound_expansions[bound_idx].expansion_distance = dist; + bound_expansions[bound_idx].expansion_point = offset_point; + } + if (dist > bound_expansions[bound_idx + 1].expansion_distance) { + bound_expansions[bound_idx + 1].expansion_distance = dist; + bound_expansions[bound_idx + 1].expansion_point = offset_point; + } break; // TODO(Maxime): should we rm this break to handle multiple segments intersect ? } } } - return min_lateral_distances; + return bound_expansions; } -void apply_velocity_limit( - std::vector & distance_vector, const std::vector & bound_vector, +void apply_bound_velocity_limit( + std::vector & expansions, const std::vector & bound_vector, const double max_velocity) { - const auto apply_max_vel = [&](auto & dist_vector, const auto from, const auto to) { - if (dist_vector[from] > dist_vector[to]) { + const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { + if (exp[from].expansion_distance > exp[to].expansion_distance) { const auto arc_length = tier4_autoware_utils::calcDistance2d(bound_vector[from], bound_vector[to]); - const auto smoothed_dist = dist_vector[from] - arc_length * max_velocity; - dist_vector[to] = std::max(dist_vector[to], smoothed_dist); + const auto smoothed_dist = exp[from].expansion_distance - arc_length * max_velocity; + exp[to].expansion_distance = std::max(exp[to].expansion_distance, smoothed_dist); } }; - for (auto idx = 0LU; idx + 1 < distance_vector.size(); ++idx) - apply_max_vel(distance_vector, idx, idx + 1); - for (auto idx = distance_vector.size() - 1; idx > 0; --idx) - apply_max_vel(distance_vector, idx, idx - 1); + for (auto idx = 0LU; idx + 1 < expansions.size(); ++idx) apply_max_vel(expansions, idx, idx + 1); + for (auto idx = expansions.size() - 1; idx > 0; --idx) apply_max_vel(expansions, idx, idx - 1); } void expand_bound( std::vector & bound, const std::vector & path_points, - const std::vector new_distances) + const std::vector & expansions) { - linestring_t path_ls; + LineString2d path_ls; for (const auto & p : path_points) path_ls.push_back(convert_point(p.point.pose.position)); for (auto idx = 0LU; idx < bound.size(); ++idx) { const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); const auto expansion_ratio = - (new_distances[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + (expansions[idx].expansion_distance + std::abs(projection.distance)) / + std::abs(projection.distance); if (expansion_ratio > 1.0) { const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); @@ -147,14 +150,30 @@ void expand_bound( } } +std::vector calculate_smoothed_curvatures( + const std::vector & points, const size_t smoothing_window_size) +{ + const auto curvatures = motion_utils::calcCurvature(points); + std::vector smoothed_curvatures(curvatures.size()); + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= smoothing_window_size ? i - smoothing_window_size : 0); + const auto to_idx = std::min(i + smoothing_window_size, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); + } + return smoothed_curvatures; +} + void expandDrivableArea( PathWithLaneId & path, const std::shared_ptr planner_data) { // Declare a stream and an SVG mapper - std::ofstream svg("/home/mclement/Pictures/da.svg"); // /!\ CHANGE PATH + std::ofstream svg("/home/mclement/Pictures/da.svg"); boost::geometry::svg_mapper mapper(svg, 400, 400); - linestring_t left_ls, right_ls; + + LineString2d left_ls, right_ls; for (const auto & p : path.left_bound) left_ls.push_back(convert_point(p)); for (const auto & p : path.right_bound) right_ls.push_back(convert_point(p)); mapper.add(left_ls); @@ -164,56 +183,56 @@ void expandDrivableArea( const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; + const auto uncrossable_lines = extractUncrossableLines( + *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); + + for (const auto & l : uncrossable_lines) + mapper.map(l, "fill-opacity:1.0;fill:grey;stroke:grey;stroke-width:1"); - stopwatch.tic("calc_replan"); - const auto ego_index = planner_data->findEgoIndex(path.points); - auto & replan_checker = planner_data->drivable_area_expansion_replan_checker; - const auto replan_index = params.replan_enable ? replan_checker.calculate_replan_index( - path, ego_index, params.replan_max_deviation) - : 0; - const auto & prev_expanded_drivable_area = replan_checker.get_previous_expanded_drivable_area(); - const auto is_replanning = - params.expansion_method == "polygon" && !params.avoid_dynamic_objects && - !boost::geometry::is_empty(prev_expanded_drivable_area) && replan_index > ego_index; - const auto calc_replan_duration = stopwatch.toc("calc_replan"); - - stopwatch.tic("extract_uncrossable_lines"); - const auto uncrossable_lines = - extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multi_linestring_t uncrossable_lines_in_range; - const auto & p = path.points.front().point.pose.position; - for (const auto & line : uncrossable_lines) - if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); const auto predicted_paths = createObjectFootprints(*planner_data->dynamic_object, params); - const auto curvatures = motion_utils::calcCurvature(points); - std::vector smoothed_curvatures(curvatures.size()); - constexpr auto move_avg_window = 3; - for (auto i = 0UL; i < curvatures.size(); ++i) { - auto sum = 0.0; - const auto from_idx = (i >= move_avg_window ? i - move_avg_window : 0); - const auto to_idx = std::min(i + move_avg_window, curvatures.size() - 1); - for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); - smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); - } - - auto new_left_distances = - calculate_minimum_lateral_distances(points, path.left_bound, smoothed_curvatures, LEFT, params); - auto new_right_distances = calculate_minimum_lateral_distances( - points, path.right_bound, smoothed_curvatures, RIGHT, params); + const auto curvatures = calculate_smoothed_curvatures(points, 3 /*TODO(Maxime): param*/); + auto left_expansions = + calculate_minimum_expansions(points, path.left_bound, curvatures, LEFT, params); + auto right_expansions = + calculate_minimum_expansions(points, path.right_bound, curvatures, RIGHT, params); // smooth the distances constexpr auto max_bound_vel = 0.5; // TODO(Maxime): param - apply_velocity_limit(new_left_distances, path.left_bound, max_bound_vel); - apply_velocity_limit(new_right_distances, path.right_bound, max_bound_vel); + apply_bound_velocity_limit(left_expansions, path.left_bound, max_bound_vel); + apply_bound_velocity_limit(right_expansions, path.right_bound, max_bound_vel); + std::cout << "new_left_dist :\n\t"; + for (const auto & e : left_expansions) std::cout << e.expansion_distance << " "; + std::cout << std::endl; + std::cout << "new_right_dist :\n\t"; + for (const auto & e : right_expansions) std::cout << e.expansion_distance << " "; + std::cout << std::endl; // limit the distances based on the uncrossable lines + // const auto apply_bound_limits = [&]() { + // for(const auto & p : ) + + // }; // limit the distances based on the total width (left + right < min_lane_width) // update the points based on the distances // TODO(Maxime): add an arc length offset / margin ? - expand_bound(path.left_bound, points, new_left_distances); - expand_bound(path.right_bound, points, new_right_distances); + for (const auto & e : left_expansions) { + mapper.map( + convert_point(e.expansion_point), "fill-opacity:0.3;fill:orange;stroke:orange;stroke-width:2", + 2); + mapper.map( + Segment2d{convert_point(e.expansion_point), e.path_projection_point}, + "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); + } + for (const auto & e : right_expansions) { + mapper.map( + convert_point(e.expansion_point), "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2", 2); + mapper.map( + Segment2d{convert_point(e.expansion_point), e.path_projection_point}, + "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); + } + expand_bound(path.left_bound, points, left_expansions); + expand_bound(path.right_bound, points, right_expansions); } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 8fe8b12252aea..8a853b5d3c256 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -20,218 +20,4 @@ namespace drivable_area_expansion { -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & line : limit_lines) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - for (const auto & p : line) - if (boost::geometry::within(p, expansion_polygon)) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & polygon : limit_polygons) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, polygon, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side) -{ - namespace strategy = boost::geometry::strategy::buffer; - multi_polygon_t polygons; - // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer - constexpr auto zero = 0.1; - const auto left_dist = is_left_side ? dist : zero; - const auto right_dist = !is_left_side ? dist : zero; - const auto distance_strategy = strategy::distance_asymmetric(left_dist, right_dist); - boost::geometry::buffer( - base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), - strategy::end_flat(), strategy::point_square()); - return polygons.empty() ? polygon_t{} : polygons.front(); -} - -std::array calculate_arc_length_range_and_distance( - const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, - const bool is_left, const double path_length) -{ - multi_point_t intersections; - double expansion_dist = 0.0; - double from_arc_length = std::numeric_limits::max(); - double to_arc_length = std::numeric_limits::min(); - boost::geometry::intersection(footprint, bound, intersections); - if (!intersections.empty()) { - for (const auto & intersection : intersections) { - const auto projection = point_to_linestring_projection(intersection, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - for (const auto & p : footprint.outer()) { - const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; - if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { - expansion_dist = std::abs(projection.distance); - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - } - } - return std::array({from_arc_length, to_arc_length, expansion_dist}); -} - -polygon_t create_compensation_polygon( - const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) -{ - polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); - double dist_limit = std::min( - compensation_dist, calculateDistanceLimit(base_ls, compensation_polygon, uncrossable_lines)); - if (!predicted_paths.empty()) - dist_limit = - std::min(dist_limit, calculateDistanceLimit(base_ls, compensation_polygon, predicted_paths)); - if (dist_limit < compensation_dist) - compensation_polygon = createExpansionPolygon(base_ls, dist_limit, !is_left); - return compensation_polygon; -} - -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params) -{ - linestring_t path_ls; - linestring_t left_ls; - linestring_t right_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); - for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); - // extend the path linestring to the beginning and end of the drivable area - if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { - const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); - const auto right_proj_begin = - point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); - const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); - const auto right_ls_proj_begin = - point_to_linestring_projection(right_proj_begin.point, path_ls); - if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) - path_ls.insert(path_ls.begin(), left_proj_begin.point); - else - path_ls.insert(path_ls.begin(), right_proj_begin.point); - const auto left_proj_end = - point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto right_proj_end = - point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); - const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); - if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) - path_ls.push_back(left_proj_end.point); - else - path_ls.push_back(right_proj_end.point); - } - const auto path_length = static_cast(boost::geometry::length(path_ls)); - - multi_polygon_t expansion_polygons; - for (const auto & footprint : path_footprints) { - bool is_left = true; - for (const auto & bound : {left_ls, right_ls}) { - auto [from_arc_length, to_arc_length, footprint_dist] = - calculate_arc_length_range_and_distance(path_ls, footprint, bound, is_left, path_length); - if (footprint_dist > 0.0) { - from_arc_length -= params.extra_arc_length; - to_arc_length += params.extra_arc_length; - from_arc_length = std::max(0.0, from_arc_length); - to_arc_length = std::min(path_length, to_arc_length); - const auto base_ls = sub_linestring(path_ls, from_arc_length, to_arc_length); - const auto expansion_dist = params.max_expansion_distance != 0.0 - ? std::min(params.max_expansion_distance, footprint_dist) - : footprint_dist; - auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); - auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = std::max( - 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - - params.avoid_linestring_dist); - if (uncrossable_dist_limit < limited_dist) { - limited_dist = uncrossable_dist_limit; - if (params.compensate_uncrossable_lines) { - const auto compensation_dist = - footprint_dist - limited_dist + params.compensate_extra_dist; - expansion_polygons.push_back(create_compensation_polygon( - base_ls, compensation_dist, is_left, uncrossable_lines, predicted_paths)); - } - } - limited_dist = std::min( - limited_dist, calculateDistanceLimit(base_ls, expansion_polygon, predicted_paths)); - if (limited_dist < expansion_dist) - expansion_polygon = createExpansionPolygon(base_ls, limited_dist, is_left); - expansion_polygons.push_back(expansion_polygon); - } - is_left = false; - } - } - return expansion_polygons; -} - -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params) -{ - multi_polygon_t expansion_polygons; - lanelet::ConstLanelets candidates; - const auto already_added = [&](const auto & ll) { - return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) != candidates.end(); - }; - const auto add_if_valid = [&](const auto & ll, const auto is_left) { - const auto bound_to_check = is_left ? ll.rightBound() : ll.leftBound(); - if (std::find_if(path_lanes.begin(), path_lanes.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) == path_lanes.end()) - if (!already_added(ll) && !hasTypes(bound_to_check, params.avoid_linestring_types)) - candidates.push_back(ll); - }; - for (const auto & current_ll : path_lanes) { - for (const auto & left_ll : - route_handler.getLaneletsFromPoint(current_ll.leftBound3d().front())) - add_if_valid(left_ll, true); - for (const auto & left_ll : route_handler.getLaneletsFromPoint(current_ll.leftBound3d().back())) - add_if_valid(left_ll, true); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().front())) - add_if_valid(right_ll, false); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().back())) - add_if_valid(right_ll, false); - } - for (const auto & candidate : candidates) { - polygon_t candidate_poly; - for (const auto & p : candidate.polygon2d()) candidate_poly.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(candidate_poly); - if ( - !boost::geometry::overlaps(candidate_poly, predicted_paths) && - boost::geometry::overlaps(path_footprints, candidate_poly)) - expansion_polygons.push_back(candidate_poly); - } - return expansion_polygons; -} - } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index ae12ac438caf3..153119d492b94 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -25,53 +25,55 @@ namespace drivable_area_expansion { -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y) +Polygon2d translatePolygon(const Polygon2d & polygon, const double x, const double y) { - polygon_t translated_polygon; + Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); boost::geometry::transform(polygon, translated_polygon, translation); return translated_polygon; } -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint) +Polygon2d createFootprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multi_polygon_t createObjectFootprints( +MultiPolygon2d createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multi_polygon_t footprints; - for (const auto & object : objects.objects) { - const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; - const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; - const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; - const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - for (const auto & path : object.kinematics.predicted_paths) - for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); + MultiPolygon2d footprints; + if (params.avoid_dynamic_objects) { + for (const auto & object : objects.objects) { + const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; + const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; + const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; + const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; + Polygon2d base_footprint; + base_footprint.outer() = { + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; + for (const auto & path : object.kinematics.predicted_paths) + for (const auto & pose : path.path) + footprints.push_back(createFootprint(pose, base_footprint)); + } } return footprints; } -multi_polygon_t createPathFootprints( +MultiPolygon2d createPathFootprints( const std::vector & points, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; const auto right = params.ego_right_offset - params.ego_extra_right_offset; const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; const auto front = params.ego_front_offset + params.ego_extra_front_offset; - polygon_t base_footprint; + Polygon2d base_footprint; base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - multi_polygon_t footprints; + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; + MultiPolygon2d footprints; // skip the last footprint as its orientation is usually wrong footprints.reserve(points.size() - 1); double arc_length = 0.0; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index ded67c251f7ae..f6710e67de7e4 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -15,30 +15,31 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "lanelet2_core/primitives/LineString.h" - #include #include +#include #include namespace drivable_area_expansion { -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) +MultiLineString2d extractUncrossableLines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - multi_linestring_t lines; - linestring_t line; + MultiLineString2d uncrossable_lines_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, uncrossable_types)) { + if (hasTypes(ls, params.avoid_linestring_types)) { line.clear(); - for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); - lines.push_back(line); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length) + uncrossable_lines_in_range.push_back(line); } } - return lines; + return uncrossable_lines_in_range; } bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index f5797a94ed0fb..9381f5a001eb6 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -21,9 +21,9 @@ #include -using drivable_area_expansion::linestring_t; -using drivable_area_expansion::point_t; -using drivable_area_expansion::segment_t; +using drivable_area_expansion::LineString2d; +using drivable_area_expansion::Point2d; +using drivable_area_expansion::Segment2d; constexpr auto eps = 1e-9; TEST(DrivableAreaExpansionProjection, PointToSegment) @@ -31,56 +31,56 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) using drivable_area_expansion::point_to_segment_projection; { - point_t query(1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 1.0, eps); EXPECT_NEAR(projection.point.x(), 1.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(-1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(-1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(11.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(11.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 10.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(0.0, -10.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(0.0, -10.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), -5.0, eps); } { - point_t query(5.0, 5.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(5.0, 5.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 0.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } { - point_t query(0.0, 0.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(0.0, 0.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); @@ -92,11 +92,11 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) { using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; { - point_t query(0.0, 0.0); + Point2d query(0.0, 0.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 0.0, eps); EXPECT_NEAR(projection.distance, 0.0, eps); @@ -104,7 +104,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(2.0, 1.0); + Point2d query(2.0, 1.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 2.0, eps); EXPECT_NEAR(projection.distance, 1.0, eps); @@ -112,7 +112,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(0.0, 5.0); + Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); @@ -125,9 +125,9 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) { using drivable_area_expansion::linestring_to_point_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto arc_length = 0.0; arc_length <= 10.0; arc_length += 1.0) { const auto projection = linestring_to_point_projection(ls, arc_length, 0.0); EXPECT_NEAR(projection.first.x(), arc_length, eps); @@ -151,58 +151,18 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) } } -TEST(DrivableAreaExpansionProjection, SubLinestring) -{ - using drivable_area_expansion::sub_linestring; - - const linestring_t ls = { - point_t{0.0, 0.0}, point_t{1.0, 0.0}, point_t{2.0, 0.0}, point_t{3.0, 0.0}, - point_t{4.0, 0.0}, point_t{5.0, 0.0}, point_t{6.0, 0.0}, - }; - { - // arc lengths equal to the original range: same linestring is returned - const auto sub = sub_linestring(ls, 0.0, 6.0); - ASSERT_EQ(ls.size(), sub.size()); - for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - } - { - // arc lengths equal to existing point: sub-linestring with same points - const auto sub = sub_linestring(ls, 1.0, 5.0); - ASSERT_EQ(ls.size() - 2lu, sub.size()); - for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); - } - { - // arc lengths inside the original: sub-linestring with some interpolated points - const auto sub = sub_linestring(ls, 1.5, 2.5); - ASSERT_EQ(sub.size(), 3lu); - EXPECT_NEAR(sub[0].x(), 1.5, eps); - EXPECT_NEAR(sub[1].x(), 2.0, eps); - EXPECT_NEAR(sub[2].x(), 2.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } - { - // arc length outside of the original range: first & last point are replaced by interpolations - const auto sub = sub_linestring(ls, -0.5, 8.5); - ASSERT_EQ(sub.size(), ls.size()); - EXPECT_NEAR(sub.front().x(), -0.5, eps); - for (auto i = 1lu; i + 1 < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - EXPECT_NEAR(sub.back().x(), 8.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } -} - TEST(DrivableAreaExpansionProjection, InverseProjection) { using drivable_area_expansion::linestring_to_point_projection; using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto x = 0.0; x < 10.0; x += 0.1) { for (auto y = 0.0; x < 10.0; x += 0.1) { - point_t p(x, y); + Point2d p(x, y); const auto projection = point_to_linestring_projection(p, ls); const auto inverse = linestring_to_point_projection(ls, projection.arc_length, projection.distance); @@ -302,75 +262,3 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); } - -TEST(DrivableAreaExpansion, calculateDistanceLimit) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multi_linestring_t; - using drivable_area_expansion::polygon_t; - - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = {}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = { - {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, 1.0, 1e-9); - } -} - -TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::polygon_t; - - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } -} From 912ca35083b9de463f1b54f822ef4f17e7d0672a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 10 Oct 2023 10:39:34 +0900 Subject: [PATCH 03/17] Handle uncrossable lines/polygons (may not be accurate enough) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion/map_utils.hpp | 4 +- .../utils/drivable_area_expansion/types.hpp | 4 +- .../drivable_area_expansion.cpp | 104 +++++++++++++----- .../drivable_area_expansion/map_utils.cpp | 6 +- 4 files changed, 85 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 8ec4fb5ba40cf..6f96b83237310 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -30,7 +30,7 @@ namespace drivable_area_expansion /// @param[in] ego_point point of the current ego position /// @param[in] params parameters with linestring types that cannot be crossed and maximum range /// @return the uncrossable linestrings -MultiLineString2d extractUncrossableLines( +MultiLineString2d extract_uncrossable_lines( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params); @@ -38,7 +38,7 @@ MultiLineString2d extractUncrossableLines( /// @param[in] ls linestring to check /// @param[in] types type strings to check /// @return true if the linestring has one of the given types -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types); +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 353e1eef04725..69e73148b1280 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -53,8 +53,8 @@ struct BoundExpansion { double expansion_distance = 0.0; Point expansion_point{}; - Point2d path_projection_point{}; - size_t original_bound_point_idx{}; + size_t path_idx{}; + size_t bound_segment_idx{}; }; enum Side { LEFT, RIGHT }; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 988dc1d7f4c5e..b8b0e2c340cac 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -27,6 +27,8 @@ #include +#include + // for writing the svg file #include #include @@ -101,10 +103,14 @@ std::vector calculate_minimum_expansions( if (dist > bound_expansions[bound_idx].expansion_distance) { bound_expansions[bound_idx].expansion_distance = dist; bound_expansions[bound_idx].expansion_point = offset_point; + bound_expansions[bound_idx].path_idx = path_idx; + bound_expansions[bound_idx].bound_segment_idx = bound_idx; } if (dist > bound_expansions[bound_idx + 1].expansion_distance) { bound_expansions[bound_idx + 1].expansion_distance = dist; bound_expansions[bound_idx + 1].expansion_point = offset_point; + bound_expansions[bound_idx + 1].path_idx = path_idx; + bound_expansions[bound_idx + 1].bound_segment_idx = bound_idx; } break; // TODO(Maxime): should we rm this break to handle multiple segments intersect ? } @@ -117,6 +123,7 @@ void apply_bound_velocity_limit( std::vector & expansions, const std::vector & bound_vector, const double max_velocity) { + if (expansions.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from].expansion_distance > exp[to].expansion_distance) { const auto arc_length = @@ -129,6 +136,32 @@ void apply_bound_velocity_limit( for (auto idx = expansions.size() - 1; idx > 0; --idx) apply_max_vel(expansions, idx, idx - 1); } +std::vector calculate_maximum_distance( + const std::vector & path, const std::vector bound, + const std::vector & uncrossable_lines, + const std::vector & uncrossable_polygons) +{ + std::vector maximum_distances(bound.size(), std::numeric_limits::max()); + LineString2d path_ls; + LineString2d bound_ls; + for (const auto & p : bound) bound_ls.push_back(convert_point(p)); + for (const auto & p : path) path_ls.push_back(convert_point(p.point.pose.position)); + for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { + const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + for (const auto & uncrossable_line : uncrossable_lines) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_line_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_line_dist); + } + for (const auto & uncrossable_poly : uncrossable_polygons) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + } + } + return maximum_distances; +} + void expand_bound( std::vector & bound, const std::vector & path_points, const std::vector & expansions) @@ -183,14 +216,16 @@ void expandDrivableArea( const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; - const auto uncrossable_lines = extractUncrossableLines( + const auto uncrossable_lines = extract_uncrossable_lines( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); + const auto uncrossable_polygons = createObjectFootprints(*planner_data->dynamic_object, params); for (const auto & l : uncrossable_lines) mapper.map(l, "fill-opacity:1.0;fill:grey;stroke:grey;stroke-width:1"); + for (const auto & p : uncrossable_polygons) + mapper.map(p, "fill-opacity:0.2;fill:grey;stroke:grey;stroke-width:1"); const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); - const auto predicted_paths = createObjectFootprints(*planner_data->dynamic_object, params); const auto curvatures = calculate_smoothed_curvatures(points, 3 /*TODO(Maxime): param*/); auto left_expansions = @@ -198,6 +233,39 @@ void expandDrivableArea( auto right_expansions = calculate_minimum_expansions(points, path.right_bound, curvatures, RIGHT, params); + const auto max_left_expansions = + calculate_maximum_distance(points, path.left_bound, uncrossable_lines, uncrossable_polygons); + const auto max_right_expansions = + calculate_maximum_distance(points, path.right_bound, uncrossable_lines, uncrossable_polygons); + for (auto i = 0LU; i < left_expansions.size(); ++i) + left_expansions[i].expansion_distance = + std::min(left_expansions[i].expansion_distance, max_left_expansions[i]); + for (auto i = 0LU; i < right_expansions.size(); ++i) + right_expansions[i].expansion_distance = + std::min(right_expansions[i].expansion_distance, max_right_expansions[i]); + + for (const auto & e : left_expansions) { + if (e.expansion_distance > 0.0) { + mapper.map( + convert_point(e.expansion_point), + "fill-opacity:0.3;fill:orange;stroke:orange;stroke-width:2", 2); + mapper.map( + Segment2d{ + convert_point(e.expansion_point), convert_point(points[e.path_idx].point.pose.position)}, + "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); + } + } + for (const auto & e : right_expansions) { + if (e.expansion_distance > 0.0) { + mapper.map( + convert_point(e.expansion_point), "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2", 2); + mapper.map( + Segment2d{ + convert_point(e.expansion_point), convert_point(points[e.path_idx].point.pose.position)}, + "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); + } + } + // smooth the distances constexpr auto max_bound_vel = 0.5; // TODO(Maxime): param apply_bound_velocity_limit(left_expansions, path.left_bound, max_bound_vel); @@ -208,30 +276,14 @@ void expandDrivableArea( std::cout << "new_right_dist :\n\t"; for (const auto & e : right_expansions) std::cout << e.expansion_distance << " "; std::cout << std::endl; - // limit the distances based on the uncrossable lines - // const auto apply_bound_limits = [&]() { - // for(const auto & p : ) - - // }; - // limit the distances based on the total width (left + right < min_lane_width) - - // update the points based on the distances - // TODO(Maxime): add an arc length offset / margin ? - for (const auto & e : left_expansions) { - mapper.map( - convert_point(e.expansion_point), "fill-opacity:0.3;fill:orange;stroke:orange;stroke-width:2", - 2); - mapper.map( - Segment2d{convert_point(e.expansion_point), e.path_projection_point}, - "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); - } - for (const auto & e : right_expansions) { - mapper.map( - convert_point(e.expansion_point), "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2", 2); - mapper.map( - Segment2d{convert_point(e.expansion_point), e.path_projection_point}, - "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); - } + std::cout << "max_left_dist:\n\t"; + for (const auto & e : max_left_expansions) std::cout << e << " "; + std::cout << std::endl; + std::cout << "max_right_dist :\n\t"; + for (const auto & e : max_right_expansions) std::cout << e << " "; + std::cout << std::endl; + // TODO(Maxime): add an arc length shift or margin ? + // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) expand_bound(path.left_bound, points, left_expansions); expand_bound(path.right_bound, points, right_expansions); } diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index f6710e67de7e4..deeb787cf39f6 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -24,7 +24,7 @@ namespace drivable_area_expansion { -MultiLineString2d extractUncrossableLines( +MultiLineString2d extract_uncrossable_lines( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params) { @@ -32,7 +32,7 @@ MultiLineString2d extractUncrossableLines( LineString2d line; const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, params.avoid_linestring_types)) { + if (has_types(ls, params.avoid_linestring_types)) { line.clear(); for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length) @@ -42,7 +42,7 @@ MultiLineString2d extractUncrossableLines( return uncrossable_lines_in_range; } -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) { constexpr auto no_type = ""; const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); From 288e29824c3988934257cfc3fda88eb71a4bf6d8 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 10 Oct 2023 10:59:10 +0900 Subject: [PATCH 04/17] Add runtime measurements Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index b8b0e2c340cac..679c392917fac 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -202,6 +203,8 @@ void expandDrivableArea( PathWithLaneId & path, const std::shared_ptr planner_data) { + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("overall"); // Declare a stream and an SVG mapper std::ofstream svg("/home/mclement/Pictures/da.svg"); boost::geometry::svg_mapper mapper(svg, 400, 400); @@ -214,25 +217,32 @@ void expandDrivableArea( mapper.add(right_ls); mapper.map(right_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); + stop_watch.tic("preprocessing"); const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extract_uncrossable_lines( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); const auto uncrossable_polygons = createObjectFootprints(*planner_data->dynamic_object, params); + const auto preprocessing_ms = stop_watch.toc("preprocessing"); for (const auto & l : uncrossable_lines) mapper.map(l, "fill-opacity:1.0;fill:grey;stroke:grey;stroke-width:1"); for (const auto & p : uncrossable_polygons) mapper.map(p, "fill-opacity:0.2;fill:grey;stroke:grey;stroke-width:1"); + stop_watch.tic("crop"); const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto crop_ms = stop_watch.toc("crop"); + stop_watch.tic("curvatures_expansion"); const auto curvatures = calculate_smoothed_curvatures(points, 3 /*TODO(Maxime): param*/); auto left_expansions = calculate_minimum_expansions(points, path.left_bound, curvatures, LEFT, params); auto right_expansions = calculate_minimum_expansions(points, path.right_bound, curvatures, RIGHT, params); + const auto curv_expansion_ms = stop_watch.toc("curvatures_expansion"); + stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance(points, path.left_bound, uncrossable_lines, uncrossable_polygons); const auto max_right_expansions = @@ -243,6 +253,7 @@ void expandDrivableArea( for (auto i = 0LU; i < right_expansions.size(); ++i) right_expansions[i].expansion_distance = std::min(right_expansions[i].expansion_distance, max_right_expansions[i]); + const auto max_dist_ms = stop_watch.toc("max_dist"); for (const auto & e : left_expansions) { if (e.expansion_distance > 0.0) { @@ -266,10 +277,11 @@ void expandDrivableArea( } } - // smooth the distances + stop_watch.tic("smooth"); constexpr auto max_bound_vel = 0.5; // TODO(Maxime): param apply_bound_velocity_limit(left_expansions, path.left_bound, max_bound_vel); apply_bound_velocity_limit(right_expansions, path.right_bound, max_bound_vel); + const auto smooth_ms = stop_watch.toc("smooth"); std::cout << "new_left_dist :\n\t"; for (const auto & e : left_expansions) std::cout << e.expansion_distance << " "; std::cout << std::endl; @@ -284,7 +296,15 @@ void expandDrivableArea( std::cout << std::endl; // TODO(Maxime): add an arc length shift or margin ? // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) + stop_watch.tic("expand"); expand_bound(path.left_bound, points, left_expansions); expand_bound(path.right_bound, points, right_expansions); + const auto expand_ms = stop_watch.toc("expand"); + + const auto total_ms = stop_watch.toc("overall"); + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curv_expansion_ms, max_dist_ms, smooth_ms, expand_ms); } } // namespace drivable_area_expansion From 2b663f20fee2b938cdf9926661820cc3bca8ca48 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 11 Oct 2023 16:09:07 +0900 Subject: [PATCH 05/17] [WIP] Reuse previously calculated raw curvatures Signed-off-by: Maxime CLEMENT --- .../behavior_path_planner/data_manager.hpp | 4 +- .../utils/drivable_area_expansion/types.hpp | 1 + .../drivable_area_expansion.cpp | 114 +++++++++++------- 3 files changed, 75 insertions(+), 44 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 5b61efd1fbb96..e89708fccb7fc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -150,8 +150,8 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; - mutable std::optional drivable_area_expansion_prev_crop_pose; - mutable drivable_area_expansion::ReplanChecker drivable_area_expansion_replan_checker{}; + mutable std::vector drivable_area_expansion_prev_path_poses{}; + mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 69e73148b1280..e0c0d4cdc5405 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -29,6 +29,7 @@ using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiLineString2d; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 679c392917fac..ac59989422843 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -41,25 +41,43 @@ namespace drivable_area_expansion { -std::vector crop_and_resample( - const std::vector & points, const DrivableAreaExpansionParameters & params, - const size_t first_idx) +void reuse_previous_points( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const DrivableAreaExpansionParameters & params) { - if (first_idx >= points.size()) return points; - const auto & crop_pose = points[first_idx].point.pose; - const auto & crop_distance = motion_utils::calcSignedArcLength(points, 0, first_idx); - // crop - const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); - const auto cropped_points = motion_utils::cropPoints( - points, crop_pose.position, crop_seg_idx, params.max_path_arc_length, - params.max_path_arc_length - crop_distance); - // resample - PathWithLaneId cropped_path; - cropped_path.points = cropped_points; - const auto resampled_path = - motion_utils::resamplePath(cropped_path, params.resample_interval, true, true, false); + /* REUSE + crop input path up to last prev + resample after last prev + */ + constexpr auto max_deviation = 0.5 /* TODO(Maxime): param*/; + std::vector cropped_poses; + std::vector cropped_curvatures; + if (prev_poses.size() > 1) { + const auto first_idx = motion_utils::findNearestSegmentIndex( + prev_poses, path.points.front().point.pose, max_deviation); + if (first_idx) { + for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { + if (motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > max_deviation) + break; + cropped_poses.push_back(prev_poses[idx]); + cropped_curvatures.push_back(prev_curvatures[idx]); + } + } + } + auto arc_length = motion_utils::calcArcLength(cropped_poses); + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + const auto first_path_idx = + cropped_poses.empty() + ? 0LU + : *motion_utils::findNearestSegmentIndex(resampled_path_points, cropped_poses.back()); + for (auto idx = first_path_idx; idx < resampled_path_points.size(); ++idx) + cropped_poses.push_back(resampled_path_points[idx].point.pose); + cropped_poses = motion_utils::cropForwardPoints( + cropped_poses, cropped_poses.front().position, 0LU, params.max_path_arc_length); - return resampled_path.points; + prev_poses = cropped_poses; + prev_curvatures = cropped_curvatures; } Point2d convert_point(const Point & p) @@ -79,25 +97,25 @@ double calculate_minimum_lane_width( } std::vector calculate_minimum_expansions( - const std::vector & points, const std::vector bound, + const std::vector & path_poses, const std::vector bound, const std::vector curvatures, const Side side, const DrivableAreaExpansionParameters & params) { std::vector bound_expansions(bound.size()); size_t lb_idx = 0; - for (auto path_idx = 0UL; path_idx < points.size(); ++path_idx) { - const auto & path_p = points[path_idx].point.pose; + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + const auto & path_pose = path_poses[path_idx]; if (curvatures[path_idx] == 0.0) continue; const auto curvature_radius = 1 / curvatures[path_idx]; const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? -1.0 : 1.0); const auto offset_point = - tier4_autoware_utils::calcOffsetPose(path_p, 0.0, side_distance, 0.0).position; + tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { const auto & prev_p = bound[bound_idx]; const auto & next_p = bound[bound_idx + 1]; const auto intersection_point = - tier4_autoware_utils::intersect(offset_point, path_p.position, prev_p, next_p); + tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); if (intersection_point) { lb_idx = bound_idx; const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); @@ -138,15 +156,16 @@ void apply_bound_velocity_limit( } std::vector calculate_maximum_distance( - const std::vector & path, const std::vector bound, + const std::vector & path_poses, const std::vector bound, const std::vector & uncrossable_lines, const std::vector & uncrossable_polygons) { + // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) std::vector maximum_distances(bound.size(), std::numeric_limits::max()); LineString2d path_ls; LineString2d bound_ls; for (const auto & p : bound) bound_ls.push_back(convert_point(p)); - for (const auto & p : path) path_ls.push_back(convert_point(p.point.pose.position)); + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; for (const auto & uncrossable_line : uncrossable_lines) { @@ -164,11 +183,11 @@ std::vector calculate_maximum_distance( } void expand_bound( - std::vector & bound, const std::vector & path_points, + std::vector & bound, const std::vector & path_poses, const std::vector & expansions) { LineString2d path_ls; - for (const auto & p : path_points) path_ls.push_back(convert_point(p.point.pose.position)); + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto idx = 0LU; idx < bound.size(); ++idx) { const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); @@ -185,9 +204,9 @@ void expand_bound( } std::vector calculate_smoothed_curvatures( - const std::vector & points, const size_t smoothing_window_size) + const std::vector & poses, const size_t smoothing_window_size) { - const auto curvatures = motion_utils::calcCurvature(points); + const auto curvatures = motion_utils::calcCurvature(poses); std::vector smoothed_curvatures(curvatures.size()); for (auto i = 0UL; i < curvatures.size(); ++i) { auto sum = 0.0; @@ -218,6 +237,7 @@ void expandDrivableArea( mapper.map(right_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); stop_watch.tic("preprocessing"); + // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extract_uncrossable_lines( @@ -231,22 +251,31 @@ void expandDrivableArea( mapper.map(p, "fill-opacity:0.2;fill:grey;stroke:grey;stroke-width:1"); stop_watch.tic("crop"); - const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; + std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + reuse_previous_points(path, path_poses, curvatures, params); + for (const auto & p : path_poses) + mapper.map( + convert_point(p.position), "fill-opacity:0.5;fill:grey;stroke:grey;stroke-width:1", 1); const auto crop_ms = stop_watch.toc("crop"); stop_watch.tic("curvatures_expansion"); - const auto curvatures = calculate_smoothed_curvatures(points, 3 /*TODO(Maxime): param*/); + // Only add curvatures for the new points. Curvatures of reused path points are not updated. + const auto new_curvatures = calculate_smoothed_curvatures(path_poses, 3 /*TODO(Maxime): param*/); + const auto first_new_point_idx = curvatures.size(); + curvatures.insert( + curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); auto left_expansions = - calculate_minimum_expansions(points, path.left_bound, curvatures, LEFT, params); + calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); auto right_expansions = - calculate_minimum_expansions(points, path.right_bound, curvatures, RIGHT, params); + calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); const auto curv_expansion_ms = stop_watch.toc("curvatures_expansion"); stop_watch.tic("max_dist"); - const auto max_left_expansions = - calculate_maximum_distance(points, path.left_bound, uncrossable_lines, uncrossable_polygons); - const auto max_right_expansions = - calculate_maximum_distance(points, path.right_bound, uncrossable_lines, uncrossable_polygons); + const auto max_left_expansions = calculate_maximum_distance( + path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons); + const auto max_right_expansions = calculate_maximum_distance( + path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons); for (auto i = 0LU; i < left_expansions.size(); ++i) left_expansions[i].expansion_distance = std::min(left_expansions[i].expansion_distance, max_left_expansions[i]); @@ -261,8 +290,7 @@ void expandDrivableArea( convert_point(e.expansion_point), "fill-opacity:0.3;fill:orange;stroke:orange;stroke-width:2", 2); mapper.map( - Segment2d{ - convert_point(e.expansion_point), convert_point(points[e.path_idx].point.pose.position)}, + Segment2d{convert_point(e.expansion_point), convert_point(path_poses[e.path_idx].position)}, "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); } } @@ -271,8 +299,7 @@ void expandDrivableArea( mapper.map( convert_point(e.expansion_point), "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2", 2); mapper.map( - Segment2d{ - convert_point(e.expansion_point), convert_point(points[e.path_idx].point.pose.position)}, + Segment2d{convert_point(e.expansion_point), convert_point(path_poses[e.path_idx].position)}, "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); } } @@ -297,8 +324,8 @@ void expandDrivableArea( // TODO(Maxime): add an arc length shift or margin ? // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); - expand_bound(path.left_bound, points, left_expansions); - expand_bound(path.right_bound, points, right_expansions); + expand_bound(path.left_bound, path_poses, left_expansions); + expand_bound(path.right_bound, path_poses, right_expansions); const auto expand_ms = stop_watch.toc("expand"); const auto total_ms = stop_watch.toc("overall"); @@ -306,5 +333,8 @@ void expandDrivableArea( "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", total_ms, preprocessing_ms, crop_ms, curv_expansion_ms, max_dist_ms, smooth_ms, expand_ms); + + planner_data->drivable_area_expansion_prev_path_poses = path_poses; + planner_data->drivable_area_expansion_prev_curvatures = curvatures; } } // namespace drivable_area_expansion From 617924731967e502ceca2157859531bc27b35f6c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 12 Oct 2023 12:03:20 +0900 Subject: [PATCH 06/17] Fix bug with lateral offset distance and repeating path points Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 55 ++++++++++--------- 1 file changed, 29 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index ac59989422843..ffa076f03b251 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -43,34 +43,48 @@ namespace drivable_area_expansion { void reuse_previous_points( const PathWithLaneId & path, std::vector & prev_poses, - std::vector & prev_curvatures, const DrivableAreaExpansionParameters & params) + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - /* REUSE - crop input path up to last prev - resample after last prev - */ constexpr auto max_deviation = 0.5 /* TODO(Maxime): param*/; std::vector cropped_poses; std::vector cropped_curvatures; - if (prev_poses.size() > 1) { - const auto first_idx = motion_utils::findNearestSegmentIndex( - prev_poses, path.points.front().point.pose, max_deviation); - if (first_idx) { + const auto ego_is_behind = + motion_utils::calcLongitudinalOffsetToSegment(prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && + tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + const auto first_idx = + motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = + motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + if (first_idx && deviation < max_deviation) { for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { if (motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > max_deviation) break; cropped_poses.push_back(prev_poses[idx]); cropped_curvatures.push_back(prev_curvatures[idx]); } + } else { + std::cout << "/!\\ could not reuse prev points" << std::endl; } } - auto arc_length = motion_utils::calcArcLength(cropped_poses); const auto resampled_path_points = motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; - const auto first_path_idx = + auto first_path_idx = cropped_poses.empty() ? 0LU - : *motion_utils::findNearestSegmentIndex(resampled_path_points, cropped_poses.back()); + : *motion_utils::findNearestSegmentIndex(resampled_path_points, cropped_poses.back()) + 1; + // make sure we do not had the same last point or segment + if ( + !cropped_poses.empty() && resampled_path_points.size() > first_path_idx + 1 && + tier4_autoware_utils::calcDistance2d( + resampled_path_points[first_path_idx + 1], cropped_poses.back()) < 1e-1) + first_path_idx += 2; + else if ( + !cropped_poses.empty() && tier4_autoware_utils::calcDistance2d( + resampled_path_points[first_path_idx], cropped_poses.back()) < 1e-1) + ++first_path_idx; for (auto idx = first_path_idx; idx < resampled_path_points.size(); ++idx) cropped_poses.push_back(resampled_path_points[idx].point.pose); cropped_poses = motion_utils::cropForwardPoints( @@ -108,7 +122,7 @@ std::vector calculate_minimum_expansions( if (curvatures[path_idx] == 0.0) continue; const auto curvature_radius = 1 / curvatures[path_idx]; const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); - const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? -1.0 : 1.0); + const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); const auto offset_point = tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { @@ -253,7 +267,8 @@ void expandDrivableArea( stop_watch.tic("crop"); std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; - reuse_previous_points(path, path_poses, curvatures, params); + reuse_previous_points( + path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); for (const auto & p : path_poses) mapper.map( convert_point(p.position), "fill-opacity:0.5;fill:grey;stroke:grey;stroke-width:1", 1); @@ -309,18 +324,6 @@ void expandDrivableArea( apply_bound_velocity_limit(left_expansions, path.left_bound, max_bound_vel); apply_bound_velocity_limit(right_expansions, path.right_bound, max_bound_vel); const auto smooth_ms = stop_watch.toc("smooth"); - std::cout << "new_left_dist :\n\t"; - for (const auto & e : left_expansions) std::cout << e.expansion_distance << " "; - std::cout << std::endl; - std::cout << "new_right_dist :\n\t"; - for (const auto & e : right_expansions) std::cout << e.expansion_distance << " "; - std::cout << std::endl; - std::cout << "max_left_dist:\n\t"; - for (const auto & e : max_left_expansions) std::cout << e << " "; - std::cout << std::endl; - std::cout << "max_right_dist :\n\t"; - for (const auto & e : max_right_expansions) std::cout << e << " "; - std::cout << std::endl; // TODO(Maxime): add an arc length shift or margin ? // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); From 49a34d3f0dfff7b8105cc22ba73f238c4219f13d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 12 Oct 2023 16:43:39 +0900 Subject: [PATCH 07/17] Remove self crossings in the expanded bounds Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index ffa076f03b251..004afacac6c34 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -65,8 +65,6 @@ void reuse_previous_points( cropped_poses.push_back(prev_poses[idx]); cropped_curvatures.push_back(prev_curvatures[idx]); } - } else { - std::cout << "/!\\ could not reuse prev points" << std::endl; } } const auto resampled_path_points = @@ -215,6 +213,25 @@ void expand_bound( bound[idx].y = expanded_p.y(); } } + + // remove loops TODO(Maxime): move to separate function + std::vector no_loop_bound = {bound.front()}; + for (auto idx = 1LU; idx < bound.size(); ++idx) { + bool is_intersecting = false; + for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { + const auto intersection = tier4_autoware_utils::intersect( + bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + if ( + intersection && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + idx = succ_idx; + is_intersecting = true; + } + } + if (!is_intersecting) no_loop_bound.push_back(bound[idx]); + } + bound = no_loop_bound; } std::vector calculate_smoothed_curvatures( From 6d42fbc943877124e348949878620ea299fbab62 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 00:08:32 +0900 Subject: [PATCH 08/17] Big cleanup + update parameters Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.hpp | 2 +- .../drivable_area_expansion/footprints.hpp | 13 +--- .../drivable_area_expansion/parameters.hpp | 69 ++++++++----------- .../src/behavior_path_planner_node.cpp | 29 ++++++++ .../drivable_area_expansion.cpp | 42 ++++++----- .../drivable_area_expansion/footprints.cpp | 36 ++-------- .../behavior_path_planner/src/utils/utils.cpp | 2 +- .../test/test_drivable_area_expansion.cpp | 14 ++-- 8 files changed, 95 insertions(+), 112 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 79c1816443c5f..f692c97029ede 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -30,7 +30,7 @@ namespace drivable_area_expansion /// @brief Expand the drivable area based on the path curvature and the vehicle dimensions /// @param[inout] path path whose drivable area will be expanded /// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) -void expandDrivableArea( +void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data); } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index fc36b7b946a73..418a9a5a68572 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -43,27 +43,20 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -Polygon2d translatePolygon(const Polygon2d & polygon, const double x, const double y); +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -Polygon2d createFootprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -MultiPolygon2d createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); - -/// @brief create the footprint polygon from a path -/// @param[in] path the path for which to create a footprint -/// @param[in] params expansion parameters defining how to create the footprint -/// @return footprint polygons of the path -MultiPolygon2d createPathFootprints( - const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index 4529cb4e2962c..231992be1b853 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -32,12 +32,9 @@ struct DrivableAreaExpansionParameters static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; - static constexpr auto EGO_EXTRA_OFFSET_FRONT = - "dynamic_expansion.ego.extra_footprint_offset.front"; - static constexpr auto EGO_EXTRA_OFFSET_REAR = "dynamic_expansion.ego.extra_footprint_offset.rear"; - static constexpr auto EGO_EXTRA_OFFSET_LEFT = "dynamic_expansion.ego.extra_footprint_offset.left"; - static constexpr auto EGO_EXTRA_OFFSET_RIGHT = - "dynamic_expansion.ego.extra_footprint_offset.right"; + static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang"; + static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase"; + static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.front"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR = @@ -46,38 +43,33 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.left"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; - static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; - static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance"; static constexpr auto RESAMPLE_INTERVAL_PARAM = "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = "dynamic_expansion.path_preprocessing.max_arc_length"; - static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; + static constexpr auto MAX_REUSE_DEVIATION_PARAM = + "dynamic_expansion.path_preprocessing.reuse_max_deviation"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; - static constexpr auto COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.enable"; - static constexpr auto EXTRA_COMPENSATE_PARAM = - "dynamic_expansion.avoid_linestring.compensate.extra_distance"; - static constexpr auto REPLAN_ENABLE_PARAM = "dynamic_expansion.replan_checker.enable"; - static constexpr auto REPLAN_MAX_DEVIATION_PARAM = - "dynamic_expansion.replan_checker.max_deviation"; - static constexpr auto DEBUG_PRINT_PARAM = "dynamic_expansion.debug_print"; + static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = + "dynamic_expansion.smoothing.curvature_average_window"; + static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = + "dynamic_expansion.smoothing.max_bound_rate"; - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + // static expansion + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; + // dynamic expansion bool enabled = false; - std::string expansion_method{}; double avoid_linestring_dist{}; - double ego_left_offset{}; - double ego_right_offset{}; - double ego_rear_offset{}; - double ego_front_offset{}; - double ego_extra_left_offset{}; - double ego_extra_right_offset{}; - double ego_extra_rear_offset{}; - double ego_extra_front_offset{}; + double extra_front_overhang{}; + double extra_wheelbase{}; + double extra_width{}; + int curvature_average_window{}; + double max_bound_rate{}; double dynamic_objects_extra_left_offset{}; double dynamic_objects_extra_right_offset{}; double dynamic_objects_extra_rear_offset{}; @@ -85,11 +77,9 @@ struct DrivableAreaExpansionParameters double max_expansion_distance{}; double max_path_arc_length{}; double resample_interval{}; - double extra_arc_length{}; + double max_reuse_deviation{}; bool avoid_dynamic_objects{}; std::vector avoid_linestring_types{}; - bool compensate_uncrossable_lines = false; - double compensate_extra_dist{}; vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; @@ -105,12 +95,14 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); + extra_front_overhang = node.declare_parameter(EGO_EXTRA_FRONT_OVERHANG); + extra_wheelbase = node.declare_parameter(EGO_EXTRA_WHEELBASE); + extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); + curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); + max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); - ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); - ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); - ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); - ego_extra_right_offset = node.declare_parameter(EGO_EXTRA_OFFSET_RIGHT); + max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); dynamic_objects_extra_front_offset = node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_FRONT); dynamic_objects_extra_rear_offset = @@ -123,13 +115,6 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); - extra_arc_length = node.declare_parameter(EXTRA_ARC_LENGTH_PARAM); - compensate_uncrossable_lines = node.declare_parameter(COMPENSATE_PARAM); - compensate_extra_dist = node.declare_parameter(EXTRA_COMPENSATE_PARAM); - expansion_method = node.declare_parameter(EXPANSION_METHOD_PARAM); - replan_enable = node.declare_parameter(REPLAN_ENABLE_PARAM); - replan_max_deviation = node.declare_parameter(REPLAN_MAX_DEVIATION_PARAM); - debug_print = node.declare_parameter(DEBUG_PRINT_PARAM); vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index c2f122220fe48..2b9c2c6215fdf 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1006,15 +1006,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); +<<<<<<< HEAD updated |= updateParam( parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, planner_data_->drivable_area_expansion_parameters.expansion_method); updated |= updateParam( +======= + updateParam( +>>>>>>> 806fae180c (Big cleanup + update parameters) parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); +<<<<<<< HEAD updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); @@ -1028,6 +1033,18 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); updated |= updateParam( +======= + updateParam( + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, + planner_data_->drivable_area_expansion_parameters.extra_front_overhang); + updateParam( + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, + planner_data_->drivable_area_expansion_parameters.extra_wheelbase); + updateParam( + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, + planner_data_->drivable_area_expansion_parameters.extra_width); + updateParam( +>>>>>>> 806fae180c (Big cleanup + update parameters) parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); updated |= updateParam( @@ -1048,6 +1065,7 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); +<<<<<<< HEAD updated |= updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.extra_arc_length); @@ -1067,6 +1085,17 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::DEBUG_PRINT_PARAM, planner_data_->drivable_area_expansion_parameters.debug_print); if (updated) planner_data_->drivable_area_expansion_replan_checker.reset(); +======= + updateParam( + parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, + planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, + planner_data_->drivable_area_expansion_parameters.curvature_average_window); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, + planner_data_->drivable_area_expansion_parameters.max_bound_rate); +>>>>>>> 806fae180c (Big cleanup + update parameters) } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 004afacac6c34..b3f9548e38c5b 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -46,7 +46,6 @@ void reuse_previous_points( std::vector & prev_curvatures, const Point & ego_point, const DrivableAreaExpansionParameters & params) { - constexpr auto max_deviation = 0.5 /* TODO(Maxime): param*/; std::vector cropped_poses; std::vector cropped_curvatures; const auto ego_is_behind = @@ -58,9 +57,11 @@ void reuse_previous_points( motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); const auto deviation = motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); - if (first_idx && deviation < max_deviation) { + if (first_idx && deviation < params.max_reuse_deviation) { for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { - if (motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > max_deviation) + if ( + motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > + params.max_reuse_deviation) break; cropped_poses.push_back(prev_poses[idx]); cropped_curvatures.push_back(prev_curvatures[idx]); @@ -101,10 +102,9 @@ double calculate_minimum_lane_width( const double curvature_radius, const DrivableAreaExpansionParameters & params) { const auto k = curvature_radius; - const auto a = params.vehicle_info.front_overhang_m + params.ego_extra_front_offset; - // TODO(Maxime): update param - const auto w = params.vehicle_info.vehicle_width_m + params.ego_extra_left_offset; - const auto l = params.vehicle_info.wheel_base_m; + const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; + const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; + const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w); } @@ -143,7 +143,7 @@ std::vector calculate_minimum_expansions( bound_expansions[bound_idx + 1].path_idx = path_idx; bound_expansions[bound_idx + 1].bound_segment_idx = bound_idx; } - break; // TODO(Maxime): should we rm this break to handle multiple segments intersect ? + break; } } } @@ -170,7 +170,8 @@ void apply_bound_velocity_limit( std::vector calculate_maximum_distance( const std::vector & path_poses, const std::vector bound, const std::vector & uncrossable_lines, - const std::vector & uncrossable_polygons) + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params) { // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) std::vector maximum_distances(bound.size(), std::numeric_limits::max()); @@ -182,8 +183,9 @@ std::vector calculate_maximum_distance( const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; for (const auto & uncrossable_line : uncrossable_lines) { const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line); - maximum_distances[i] = std::min(maximum_distances[i], bound_to_line_dist); - maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_line_dist); + const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); + maximum_distances[i] = std::min(maximum_distances[i], dist_limit); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } for (const auto & uncrossable_poly : uncrossable_polygons) { const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); @@ -191,6 +193,8 @@ std::vector calculate_maximum_distance( maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); } } + if (params.max_expansion_distance > 0.0) + for (auto & d : maximum_distances) d = std::min(params.max_expansion_distance, d); return maximum_distances; } @@ -249,7 +253,7 @@ std::vector calculate_smoothed_curvatures( return smoothed_curvatures; } -void expandDrivableArea( +void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data) { @@ -273,7 +277,7 @@ void expandDrivableArea( const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extract_uncrossable_lines( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); - const auto uncrossable_polygons = createObjectFootprints(*planner_data->dynamic_object, params); + const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); const auto preprocessing_ms = stop_watch.toc("preprocessing"); for (const auto & l : uncrossable_lines) @@ -293,7 +297,8 @@ void expandDrivableArea( stop_watch.tic("curvatures_expansion"); // Only add curvatures for the new points. Curvatures of reused path points are not updated. - const auto new_curvatures = calculate_smoothed_curvatures(path_poses, 3 /*TODO(Maxime): param*/); + const auto new_curvatures = + calculate_smoothed_curvatures(path_poses, params.curvature_average_window); const auto first_new_point_idx = curvatures.size(); curvatures.insert( curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); @@ -305,9 +310,9 @@ void expandDrivableArea( stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons); + path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons, params); const auto max_right_expansions = calculate_maximum_distance( - path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons); + path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params); for (auto i = 0LU; i < left_expansions.size(); ++i) left_expansions[i].expansion_distance = std::min(left_expansions[i].expansion_distance, max_left_expansions[i]); @@ -337,9 +342,8 @@ void expandDrivableArea( } stop_watch.tic("smooth"); - constexpr auto max_bound_vel = 0.5; // TODO(Maxime): param - apply_bound_velocity_limit(left_expansions, path.left_bound, max_bound_vel); - apply_bound_velocity_limit(right_expansions, path.right_bound, max_bound_vel); + apply_bound_velocity_limit(left_expansions, path.left_bound, params.max_bound_rate); + apply_bound_velocity_limit(right_expansions, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); // TODO(Maxime): add an arc length shift or margin ? // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 153119d492b94..c03adf6c8a76e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -25,7 +25,7 @@ namespace drivable_area_expansion { -Polygon2d translatePolygon(const Polygon2d & polygon, const double x, const double y) +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y) { Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); @@ -33,13 +33,14 @@ Polygon2d translatePolygon(const Polygon2d & polygon, const double x, const doub return translated_polygon; } -Polygon2d createFootprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); - return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -MultiPolygon2d createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { @@ -56,32 +57,7 @@ MultiPolygon2d createObjectFootprints( Point2d{front, left}}; for (const auto & path : object.kinematics.predicted_paths) for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); - } - } - return footprints; -} - -MultiPolygon2d createPathFootprints( - const std::vector & points, const DrivableAreaExpansionParameters & params) -{ - const auto left = params.ego_left_offset + params.ego_extra_left_offset; - const auto right = params.ego_right_offset - params.ego_extra_right_offset; - const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; - const auto front = params.ego_front_offset + params.ego_extra_front_offset; - Polygon2d base_footprint; - base_footprint.outer() = { - Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, - Point2d{front, left}}; - MultiPolygon2d footprints; - // skip the last footprint as its orientation is usually wrong - footprints.reserve(points.size() - 1); - double arc_length = 0.0; - for (auto it = points.begin(); std::next(it) != points.end(); ++it) { - footprints.push_back(createFootprint(it->point.pose, base_footprint)); - if (params.max_path_arc_length > 0.0) { - arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); - if (arc_length > params.max_path_arc_length) break; + footprints.push_back(create_footprint(pose, base_footprint)); } } return footprints; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 914885dc3c2bc..ed2979bd1d5a1 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1532,7 +1532,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea(path, planner_data); + drivable_area_expansion::expand_drivable_area(path, planner_data); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 9381f5a001eb6..9b3c46e785ff5 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -172,7 +172,7 @@ TEST(DrivableAreaExpansionProjection, InverseProjection) } } -TEST(DrivableAreaExpansionProjection, expandDrivableArea) +TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; @@ -215,17 +215,13 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.avoid_dynamic_objects = false; params.avoid_linestring_dist = 0.0; params.avoid_linestring_types = {}; - params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit params.resample_interval = 1.0; - params.extra_arc_length = 1.0; - params.expansion_method = "polygon"; // 2m x 4m ego footprint - params.ego_front_offset = 1.0; - params.ego_rear_offset = -1.0; - params.ego_left_offset = 2.0; - params.ego_right_offset = -2.0; + params.vehicle_info.front_overhang_m = 0.0; + params.vehicle_info.wheel_base_m = 2.0; + params.vehicle_info.vehicle_width_m = 2.0; } behavior_path_planner::PlannerData planner_data; planner_data.drivable_area_expansion_parameters = params; @@ -234,7 +230,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) std::make_shared(dynamic_objects); planner_data.route_handler = std::make_shared(route_handler); // we expect the drivable area to be expanded by 1m on each side - drivable_area_expansion::expandDrivableArea( + drivable_area_expansion::expand_drivable_area( path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); From b977b09adeebbb9f70c6963f16ab24c3a6d7f96d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 00:13:34 +0900 Subject: [PATCH 09/17] Remove svg debug output Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 49 ------------------- 1 file changed, 49 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index b3f9548e38c5b..7dcf0bc7bb98f 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -30,15 +30,6 @@ #include -// for writing the svg file -#include -#include -// for the geometry types -#include -// for the svg mapper -#include -#include - namespace drivable_area_expansion { void reuse_previous_points( @@ -259,18 +250,6 @@ void expand_drivable_area( { tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); - // Declare a stream and an SVG mapper - std::ofstream svg("/home/mclement/Pictures/da.svg"); - boost::geometry::svg_mapper mapper(svg, 400, 400); - - LineString2d left_ls, right_ls; - for (const auto & p : path.left_bound) left_ls.push_back(convert_point(p)); - for (const auto & p : path.right_bound) right_ls.push_back(convert_point(p)); - mapper.add(left_ls); - mapper.map(left_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); - mapper.add(right_ls); - mapper.map(right_ls, "fill-opacity:0.3;fill:blue;stroke:blue;stroke-width:2"); - stop_watch.tic("preprocessing"); // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; @@ -280,19 +259,11 @@ void expand_drivable_area( const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); const auto preprocessing_ms = stop_watch.toc("preprocessing"); - for (const auto & l : uncrossable_lines) - mapper.map(l, "fill-opacity:1.0;fill:grey;stroke:grey;stroke-width:1"); - for (const auto & p : uncrossable_polygons) - mapper.map(p, "fill-opacity:0.2;fill:grey;stroke:grey;stroke-width:1"); - stop_watch.tic("crop"); std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; reuse_previous_points( path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); - for (const auto & p : path_poses) - mapper.map( - convert_point(p.position), "fill-opacity:0.5;fill:grey;stroke:grey;stroke-width:1", 1); const auto crop_ms = stop_watch.toc("crop"); stop_watch.tic("curvatures_expansion"); @@ -321,26 +292,6 @@ void expand_drivable_area( std::min(right_expansions[i].expansion_distance, max_right_expansions[i]); const auto max_dist_ms = stop_watch.toc("max_dist"); - for (const auto & e : left_expansions) { - if (e.expansion_distance > 0.0) { - mapper.map( - convert_point(e.expansion_point), - "fill-opacity:0.3;fill:orange;stroke:orange;stroke-width:2", 2); - mapper.map( - Segment2d{convert_point(e.expansion_point), convert_point(path_poses[e.path_idx].position)}, - "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); - } - } - for (const auto & e : right_expansions) { - if (e.expansion_distance > 0.0) { - mapper.map( - convert_point(e.expansion_point), "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2", 2); - mapper.map( - Segment2d{convert_point(e.expansion_point), convert_point(path_poses[e.path_idx].position)}, - "fill-opacity:0.3;fill:black;stroke:black;stroke-width:2"); - } - } - stop_watch.tic("smooth"); apply_bound_velocity_limit(left_expansions, path.left_bound, params.max_bound_rate); apply_bound_velocity_limit(right_expansions, path.right_bound, params.max_bound_rate); From 4327d60c3896222d70286c0508f5f77cf8ad631a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 00:20:35 +0900 Subject: [PATCH 10/17] Update parameter file Signed-off-by: Maxime CLEMENT --- .../config/drivable_area_expansion.param.yaml | 31 ++++++------------- 1 file changed, 10 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 706a4fc2e45c1..c4b0adb6819b0 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -5,16 +5,17 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false - debug_print: false # if true, print some debug runtime measurements + enabled: true + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + 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 ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + 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 + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -25,20 +26,8 @@ path_preprocessing: max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation - replan_checker: - enable: true # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly - # not compatible with dynamic_objects.avoid - max_deviation: 0.5 # [m] full replan is only done if the path changes by more than this distance From eca596d246e61bb920b24ede1d1a51f0728f9b28 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 00:45:38 +0900 Subject: [PATCH 11/17] Fix rebase mistakes Signed-off-by: Maxime CLEMENT --- .../behavior_path_planner/data_manager.hpp | 1 - .../replan_checker.hpp | 92 ------------------- .../src/behavior_path_planner_node.cpp | 55 +---------- 3 files changed, 5 insertions(+), 143 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index e89708fccb7fc..967607e4c143a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp deleted file mode 100644 index 278893ccae55c..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/replan_checker.hpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -namespace drivable_area_expansion -{ -class ReplanChecker -{ -private: - linestring_t prev_path_ls_{}; - polygon_t prev_expanded_drivable_area_{}; - -public: - /// @brief set the previous path and its expanded drivable area - /// @param path previous path - void set_previous(const PathWithLaneId & path) - { - prev_path_ls_.clear(); - for (const auto & p : path.points) - prev_path_ls_.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - boost::geometry::clear(prev_expanded_drivable_area_); - for (const auto & p : path.left_bound) - prev_expanded_drivable_area_.outer().emplace_back(p.x, p.y); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - prev_expanded_drivable_area_.outer().emplace_back(it->x, it->y); - if (!boost::geometry::is_empty(prev_expanded_drivable_area_)) - prev_expanded_drivable_area_.outer().push_back(prev_expanded_drivable_area_.outer().front()); - } - - /// @brief get the previous expanded drivable area - /// @return the previous expanded drivable area - polygon_t get_previous_expanded_drivable_area() { return prev_expanded_drivable_area_; } - - /// @brief reset the previous path and expanded drivable area - void reset() - { - boost::geometry::clear(prev_path_ls_); - boost::geometry::clear(prev_expanded_drivable_area_); - } - - /// @brief calculate the index of the input path from which replanning is necessary (starting from - /// ego pose) - /// @param [in] path input path - /// @param [in] ego_index path index before the current ego pose - /// @param [in] max_deviation [m] replan index will be the first path point that deviates by more - /// than this distance - /// @return path index from which to replan - size_t calculate_replan_index( - const PathWithLaneId & path, const size_t ego_index, const double max_deviation) const - { - linestring_t path_ls; - path_ls.reserve(path.points.size()); - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - // full replan if no prev path or if end of the previous path does not match with the new path - if ( - prev_path_ls_.empty() || - boost::geometry::distance(prev_path_ls_.back(), path_ls) > max_deviation) - return 0; - - for (size_t i = ego_index; i < path_ls.size(); ++i) { - const auto & point = path_ls[i]; - const auto deviation_distance = boost::geometry::distance(point, prev_path_ls_); - if (deviation_distance > max_deviation) { - return i; - } - } - return path_ls.size(); - } -}; -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__REPLAN_CHECKER_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 2b9c2c6215fdf..38e8a76d14034 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1006,45 +1006,22 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); -<<<<<<< HEAD updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, - planner_data_->drivable_area_expansion_parameters.expansion_method); - updated |= updateParam( -======= - updateParam( ->>>>>>> 806fae180c (Big cleanup + update parameters) parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); updated |= updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); -<<<<<<< HEAD - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, - planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, - planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, - planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, - planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); - updated |= updateParam( -======= - updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, planner_data_->drivable_area_expansion_parameters.extra_front_overhang); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, planner_data_->drivable_area_expansion_parameters.extra_wheelbase); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, planner_data_->drivable_area_expansion_parameters.extra_width); - updateParam( ->>>>>>> 806fae180c (Big cleanup + update parameters) + updated |= updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); updated |= updateParam( @@ -1065,37 +1042,15 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); -<<<<<<< HEAD - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); updated |= updateParam( - parameters, DrivableAreaExpansionParameters::REPLAN_ENABLE_PARAM, - planner_data_->drivable_area_expansion_parameters.replan_enable); - updated |= updateParam( - parameters, DrivableAreaExpansionParameters::REPLAN_MAX_DEVIATION_PARAM, - planner_data_->drivable_area_expansion_parameters.replan_max_deviation); - updateParam( - parameters, DrivableAreaExpansionParameters::DEBUG_PRINT_PARAM, - planner_data_->drivable_area_expansion_parameters.debug_print); - if (updated) planner_data_->drivable_area_expansion_replan_checker.reset(); -======= - updateParam( parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, planner_data_->drivable_area_expansion_parameters.curvature_average_window); - updateParam( + updated |= updateParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); ->>>>>>> 806fae180c (Big cleanup + update parameters) } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); From e9adb7e5b3779a6f9f9d92fdd0579c3d7ee23f8d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 12:07:41 +0900 Subject: [PATCH 12/17] Add parameter to enable/disable printing the runtime Signed-off-by: Maxime CLEMENT --- .../config/drivable_area_expansion.param.yaml | 1 + .../utils/drivable_area_expansion/parameters.hpp | 3 +++ .../src/behavior_path_planner_node.cpp | 3 +++ .../drivable_area_expansion/drivable_area_expansion.cpp | 9 +++++---- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index c4b0adb6819b0..82f5a5d2c6b6d 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -8,6 +8,7 @@ # Dynamic expansion by using the path curvature dynamic_expansion: enabled: true + print_runtime: false max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) smoothing: curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index 231992be1b853..93f003071cd0c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -57,6 +57,7 @@ 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 PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; // static expansion double drivable_area_right_bound_offset{}; @@ -79,6 +80,7 @@ struct DrivableAreaExpansionParameters double resample_interval{}; double max_reuse_deviation{}; bool avoid_dynamic_objects{}; + bool print_runtime{}; std::vector avoid_linestring_types{}; vehicle_info_util::VehicleInfo vehicle_info; @@ -115,6 +117,7 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 38e8a76d14034..fa2d69b8bd26e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1051,6 +1051,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); + updated |= updateParam( + parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, + planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 7dcf0bc7bb98f..48e5e19a95443 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -304,10 +304,11 @@ void expand_drivable_area( const auto expand_ms = stop_watch.toc("expand"); const auto total_ms = stop_watch.toc("overall"); - std::printf( - "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " - "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", - total_ms, preprocessing_ms, crop_ms, curv_expansion_ms, max_dist_ms, smooth_ms, expand_ms); + if (params.print_runtime) + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curv_expansion_ms, max_dist_ms, smooth_ms, expand_ms); planner_data->drivable_area_expansion_prev_path_poses = path_poses; planner_data->drivable_area_expansion_prev_curvatures = curvatures; From 94a512405f7eaf1e6423bd3f7dace420f5661495 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 13 Oct 2023 15:53:55 +0900 Subject: [PATCH 13/17] Fix append of new path points to satisfy the resampling interval Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 38 +++++++++---------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 48e5e19a95443..b8d27a39a7eda 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -59,27 +59,23 @@ void reuse_previous_points( } } } - const auto resampled_path_points = - motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; - auto first_path_idx = - cropped_poses.empty() - ? 0LU - : *motion_utils::findNearestSegmentIndex(resampled_path_points, cropped_poses.back()) + 1; - // make sure we do not had the same last point or segment - if ( - !cropped_poses.empty() && resampled_path_points.size() > first_path_idx + 1 && - tier4_autoware_utils::calcDistance2d( - resampled_path_points[first_path_idx + 1], cropped_poses.back()) < 1e-1) - first_path_idx += 2; - else if ( - !cropped_poses.empty() && tier4_autoware_utils::calcDistance2d( - resampled_path_points[first_path_idx], cropped_poses.back()) < 1e-1) - ++first_path_idx; - for (auto idx = first_path_idx; idx < resampled_path_points.size(); ++idx) - cropped_poses.push_back(resampled_path_points[idx].point.pose); - cropped_poses = motion_utils::cropForwardPoints( - cropped_poses, cropped_poses.front().position, 0LU, params.max_path_arc_length); - + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); + cropped_poses = + motion_utils::cropForwardPoints(cropped_poses, ego_point, 0LU, params.max_path_arc_length); + } else if (!path.points.empty()) { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + } prev_poses = cropped_poses; prev_curvatures = cropped_curvatures; } From 663754e1a5d3d43d87434278ad101b7e289354dc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 16 Oct 2023 09:06:48 +0900 Subject: [PATCH 14/17] Add smoothing.extra_arc_length param Signed-off-by: Maxime CLEMENT --- .../config/drivable_area_expansion.param.yaml | 1 + .../drivable_area_expansion/parameters.hpp | 4 ++ .../src/behavior_path_planner_node.cpp | 3 + .../drivable_area_expansion.cpp | 61 +++++++++++-------- 4 files changed, 45 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 82f5a5d2c6b6d..c05a874a1ad29 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -13,6 +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 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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index 93f003071cd0c..e7275960b0888 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -57,6 +57,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 PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; // static expansion @@ -78,6 +80,7 @@ struct DrivableAreaExpansionParameters double max_expansion_distance{}; double max_path_arc_length{}; double resample_interval{}; + double extra_arc_length{}; double max_reuse_deviation{}; bool avoid_dynamic_objects{}; bool print_runtime{}; @@ -102,6 +105,7 @@ struct DrivableAreaExpansionParameters extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); + extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index fa2d69b8bd26e..d5789f54b27cc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1051,6 +1051,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updated |= updateParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); + updated |= updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, + planner_data_->drivable_area_expansion_parameters.extra_arc_length); updated |= updateParam( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index b8d27a39a7eda..03fa29d39a49f 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -95,12 +95,12 @@ double calculate_minimum_lane_width( return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w); } -std::vector calculate_minimum_expansions( +std::vector calculate_minimum_expansions( const std::vector & path_poses, const std::vector bound, const std::vector curvatures, const Side side, const DrivableAreaExpansionParameters & params) { - std::vector bound_expansions(bound.size()); + std::vector minimum_expansions(bound.size()); size_t lb_idx = 0; for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { const auto & path_pose = path_poses[path_idx]; @@ -118,36 +118,52 @@ std::vector calculate_minimum_expansions( if (intersection_point) { lb_idx = bound_idx; const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); - if (dist > bound_expansions[bound_idx].expansion_distance) { - bound_expansions[bound_idx].expansion_distance = dist; - bound_expansions[bound_idx].expansion_point = offset_point; - bound_expansions[bound_idx].path_idx = path_idx; - bound_expansions[bound_idx].bound_segment_idx = bound_idx; + minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); + minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); + // apply the expansion to all bound points within the extra arc length + if (bound_idx + 2 < bound.size()) { + auto up_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]); + for (auto up_bound_idx = bound_idx + 2; + bound_idx < bound.size() && up_arc_length <= params.extra_arc_length; + ++up_bound_idx) { + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); + if (up_bound_idx + 1 < bound.size()) + up_arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]); + } } - if (dist > bound_expansions[bound_idx + 1].expansion_distance) { - bound_expansions[bound_idx + 1].expansion_distance = dist; - bound_expansions[bound_idx + 1].expansion_point = offset_point; - bound_expansions[bound_idx + 1].path_idx = path_idx; - bound_expansions[bound_idx + 1].bound_segment_idx = bound_idx; + if (bound_idx > 0) { + auto down_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + for (auto down_bound_idx = bound_idx - 1; + down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) { + minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist); + if (down_bound_idx > 1) + down_arc_length += tier4_autoware_utils::calcDistance2d( + bound[down_bound_idx], bound[down_bound_idx - 1]); + } } break; } } } - return bound_expansions; + return minimum_expansions; } void apply_bound_velocity_limit( - std::vector & expansions, const std::vector & bound_vector, + std::vector & expansions, const std::vector & bound_vector, const double max_velocity) { if (expansions.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { - if (exp[from].expansion_distance > exp[to].expansion_distance) { + if (exp[from] > exp[to]) { const auto arc_length = tier4_autoware_utils::calcDistance2d(bound_vector[from], bound_vector[to]); - const auto smoothed_dist = exp[from].expansion_distance - arc_length * max_velocity; - exp[to].expansion_distance = std::max(exp[to].expansion_distance, smoothed_dist); + const auto smoothed_dist = exp[from] - arc_length * max_velocity; + exp[to] = std::max(exp[to], smoothed_dist); } }; for (auto idx = 0LU; idx + 1 < expansions.size(); ++idx) apply_max_vel(expansions, idx, idx + 1); @@ -187,7 +203,7 @@ std::vector calculate_maximum_distance( void expand_bound( std::vector & bound, const std::vector & path_poses, - const std::vector & expansions) + const std::vector & expansions) { LineString2d path_ls; for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); @@ -195,8 +211,7 @@ void expand_bound( const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); const auto expansion_ratio = - (expansions[idx].expansion_distance + std::abs(projection.distance)) / - std::abs(projection.distance); + (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); if (expansion_ratio > 1.0) { const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); @@ -281,11 +296,9 @@ void expand_drivable_area( const auto max_right_expansions = calculate_maximum_distance( path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params); for (auto i = 0LU; i < left_expansions.size(); ++i) - left_expansions[i].expansion_distance = - std::min(left_expansions[i].expansion_distance, max_left_expansions[i]); + left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); for (auto i = 0LU; i < right_expansions.size(); ++i) - right_expansions[i].expansion_distance = - std::min(right_expansions[i].expansion_distance, max_right_expansions[i]); + right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); const auto max_dist_ms = stop_watch.toc("max_dist"); stop_watch.tic("smooth"); From 7d179746f3b40c70a7b8805ba750a05f310cbff4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 16 Oct 2023 13:15:02 +0900 Subject: [PATCH 15/17] Code cleanup + add docstrings Signed-off-by: Maxime CLEMENT --- planning/behavior_path_planner/CMakeLists.txt | 1 - .../drivable_area_expansion.hpp | 61 +++++++++++++++++++ .../drivable_area_expansion/expansion.hpp | 32 ---------- .../utils/drivable_area_expansion/types.hpp | 7 --- .../drivable_area_expansion.cpp | 52 ++++++++-------- .../drivable_area_expansion/expansion.cpp | 23 ------- .../test/test_drivable_area_expansion.cpp | 1 - 7 files changed, 89 insertions(+), 88 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp delete mode 100644 planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index efae65ae48813..5bfcf8ce0415b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp - src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index f692c97029ede..b0e2f76e0a23a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -24,6 +24,7 @@ #include #include +#include namespace drivable_area_expansion { @@ -33,6 +34,66 @@ namespace drivable_area_expansion void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data); + +/// @brief prepare path poses and try to reuse 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 posesto reuse +/// @param [inout] prev_curvatures previous curvaturesto 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( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params); + +/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions +/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of +/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021, +/// https://doi.org/10.4271/2021-01-0099 +/// @param [in] curvature curvature +/// @param [in] params parameters containing the vehicle dimensions +/// @return minimum lane width +double calculate_minimum_lane_width( + const double curvature_radius, 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) +/// @param [in] bound_points bound points +/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & 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_lines lines that limit the bound expansion +/// @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 +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const std::vector & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params); + +/// @brief expand a bound by the given lateral distances away from the path +/// @param [inout] bound bound points to expand +/// @param [in] path_poses input path +/// @param [in] distances new lateral distances of the bound points away from the path +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & distances); + +/// @brief calculate smoothed curvatures +/// @details smoothing is done using a moving average +/// @param [in] poses input poses used to calculate the curvatures +/// @param [in] smoothing_window_size size of the window used for the moving average +/// @return smoothed curvatures of the input poses +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size); + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp deleted file mode 100644 index ab9c36be0d4fc..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -#include -#include - -namespace drivable_area_expansion -{ -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e0c0d4cdc5405..7db92c163f567 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -50,13 +50,6 @@ struct Projection double distance; double arc_length; }; -struct BoundExpansion -{ - double expansion_distance = 0.0; - Point expansion_point{}; - size_t path_idx{}; - size_t bound_segment_idx{}; -}; enum Side { LEFT, RIGHT }; } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 03fa29d39a49f..aefc0d9d18474 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" @@ -32,15 +31,26 @@ namespace drivable_area_expansion { -void reuse_previous_points( + +namespace +{ + +Point2d convert_point(const Point & p) +{ + return Point2d{p.x, p.y}; +} + +} // namespace + +void reuse_previous_poses( const PathWithLaneId & path, std::vector & prev_poses, std::vector & prev_curvatures, const Point & ego_point, const DrivableAreaExpansionParameters & params) { std::vector cropped_poses; std::vector cropped_curvatures; - const auto ego_is_behind = - motion_utils::calcLongitudinalOffsetToSegment(prev_poses, 0, ego_point) < 0.0; + const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( + prev_poses, 0, ego_point) < 0.0; const auto ego_is_far = !prev_poses.empty() && tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { @@ -76,15 +86,10 @@ void reuse_previous_points( arc_length += params.resample_interval) cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); } - prev_poses = cropped_poses; + prev_poses = motion_utils::removeOverlapPoints(cropped_poses); prev_curvatures = cropped_curvatures; } -Point2d convert_point(const Point & p) -{ - return Point2d{p.x, p.y}; -} - double calculate_minimum_lane_width( const double curvature_radius, const DrivableAreaExpansionParameters & params) { @@ -153,21 +158,19 @@ std::vector calculate_minimum_expansions( return minimum_expansions; } -void apply_bound_velocity_limit( - std::vector & expansions, const std::vector & bound_vector, - const double max_velocity) +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate) { - if (expansions.empty()) return; + if (distances.empty()) return; const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { if (exp[from] > exp[to]) { - const auto arc_length = - tier4_autoware_utils::calcDistance2d(bound_vector[from], bound_vector[to]); - const auto smoothed_dist = exp[from] - arc_length * max_velocity; + const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto smoothed_dist = exp[from] - arc_length * max_rate; exp[to] = std::max(exp[to], smoothed_dist); } }; - for (auto idx = 0LU; idx + 1 < expansions.size(); ++idx) apply_max_vel(expansions, idx, idx + 1); - for (auto idx = expansions.size() - 1; idx > 0; --idx) apply_max_vel(expansions, idx, idx - 1); + for (auto idx = 0LU; idx + 1 < distances.size(); ++idx) apply_max_vel(distances, idx, idx + 1); + for (auto idx = distances.size() - 1; idx > 0; --idx) apply_max_vel(distances, idx, idx - 1); } std::vector calculate_maximum_distance( @@ -220,7 +223,7 @@ void expand_bound( } } - // remove loops TODO(Maxime): move to separate function + // remove any self intersection by skipping the points inside of the loop std::vector no_loop_bound = {bound.front()}; for (auto idx = 1LU; idx < bound.size(); ++idx) { bool is_intersecting = false; @@ -259,6 +262,8 @@ void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data) { + // skip if no bounds or not enough points to calculate path curvature + if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); @@ -273,7 +278,7 @@ void expand_drivable_area( stop_watch.tic("crop"); std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; - reuse_previous_points( + reuse_previous_poses( path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); const auto crop_ms = stop_watch.toc("crop"); @@ -302,10 +307,9 @@ void expand_drivable_area( const auto max_dist_ms = stop_watch.toc("max_dist"); stop_watch.tic("smooth"); - apply_bound_velocity_limit(left_expansions, path.left_bound, params.max_bound_rate); - apply_bound_velocity_limit(right_expansions, path.right_bound, params.max_bound_rate); + apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); - // TODO(Maxime): add an arc length shift or margin ? // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); expand_bound(path.left_bound, path_poses, left_expansions); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp deleted file mode 100644 index 8a853b5d3c256..0000000000000 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" - -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" - -namespace drivable_area_expansion -{ - -} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 9b3c46e785ff5..35ec1b36dee50 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" From bba9c57cba1ad391e3799b843df6f45be8cd2349 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 16 Oct 2023 13:51:56 +0900 Subject: [PATCH 16/17] Fix spellcheck Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion/drivable_area_expansion.hpp | 4 ++-- .../drivable_area_expansion/drivable_area_expansion.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index b0e2f76e0a23a..19ea89a3ce3c7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -38,8 +38,8 @@ void expand_drivable_area( /// @brief prepare path poses and try to reuse 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 posesto reuse -/// @param [inout] prev_curvatures previous curvaturesto reuse +/// @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( diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index aefc0d9d18474..18c9cbd7382a5 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -97,7 +97,7 @@ double calculate_minimum_lane_width( const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; - return (a * a + 2 * a * l + 2 * k * w + l * l + w * w) / (2 * k + w); + return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } std::vector calculate_minimum_expansions( @@ -293,7 +293,7 @@ void expand_drivable_area( calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); auto right_expansions = calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); - const auto curv_expansion_ms = stop_watch.toc("curvatures_expansion"); + const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( @@ -321,7 +321,8 @@ void expand_drivable_area( std::printf( "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", - total_ms, preprocessing_ms, crop_ms, curv_expansion_ms, max_dist_ms, smooth_ms, expand_ms); + total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, + expand_ms); planner_data->drivable_area_expansion_prev_path_poses = path_poses; planner_data->drivable_area_expansion_prev_curvatures = curvatures; From 2c169472ad1a87ee0cd94fc1399a12dbac45d875 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 16 Oct 2023 15:35:06 +0900 Subject: [PATCH 17/17] Fix initial path poses (no longer cropped) and fix test Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 2 -- .../test/test_drivable_area_expansion.cpp | 36 +++++++++++++------ 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 18c9cbd7382a5..decd145f7a57b 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -73,8 +73,6 @@ void reuse_previous_poses( const auto resampled_path_points = motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); - cropped_poses = - motion_utils::cropForwardPoints(cropped_poses, ego_point, 0LU, params.max_path_arc_length); } else if (!path.points.empty()) { const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); const auto max_path_arc_length = motion_utils::calcArcLength(path.points); diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 35ec1b36dee50..1cade95a96681 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -227,8 +227,8 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) planner_data.reference_path = std::make_shared(path); planner_data.dynamic_object = std::make_shared(dynamic_objects); + planner_data.self_odometry = std::make_shared(); planner_data.route_handler = std::make_shared(route_handler); - // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expand_drivable_area( path, std::make_shared(planner_data)); // unchanged path points @@ -237,23 +237,37 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } - + // straight path: no expansion // expanded left bound - ASSERT_EQ(path.left_bound.size(), 4ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); + + // add some curvature + path.points[1].point.pose.position.y = 0.5; + + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); + // expanded left bound + ASSERT_EQ(path.left_bound.size(), 3ul); + EXPECT_GT(path.left_bound[0].y, 1.0); + EXPECT_GT(path.left_bound[1].y, 1.0); + EXPECT_GT(path.left_bound[2].y, 1.0); + // expanded right bound + ASSERT_EQ(path.right_bound.size(), 3ul); + EXPECT_LT(path.right_bound[0].y, -1.0); + EXPECT_LT(path.right_bound[1].y, -1.0); + EXPECT_LT(path.right_bound[2].y, -1.0); }