From 02d58699a4bf0a7bd255b0aaecb81cddd48081ad Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 9 Feb 2024 17:15:22 +0900 Subject: [PATCH] fix(out_of_lane): ignore overlaps over all lanelets crossed by the ego path (#6334) Signed-off-by: Maxime CLEMENT --- .../src/footprint.cpp | 8 +++- .../src/lanelets_selection.cpp | 39 ++++++++++--------- .../src/lanelets_selection.hpp | 8 ++++ .../src/overlapping_range.cpp | 1 + .../src/scene_out_of_lane.cpp | 15 ++++--- 5 files changed, 45 insertions(+), 26 deletions(-) diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 31cc035703a7b..d136f4a8598f3 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace behavior_velocity_planner::out_of_lane @@ -58,7 +59,10 @@ std::vector calculate_path_footprints( const auto base_footprint = make_base_footprint(params); std::vector path_footprints; path_footprints.reserve(ego_data.path.points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + double length = 0.0; + const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + + params.front_offset + params.extra_front_offset; + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) { const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); @@ -66,6 +70,8 @@ std::vector calculate_path_footprints( for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y); path_footprints.push_back(footprint); + if (i + 1 < ego_data.path.points.size()) + length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point); } return path_footprints; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index c5dedf23785ab..3c90a8cbf0dce 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -24,15 +24,28 @@ namespace behavior_velocity_planner::out_of_lane { +lanelet::ConstLanelets calculate_path_lanelets( + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) +{ + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); + lanelet::ConstLanelets path_lanelets = + planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose); + lanelet::BasicLineString2d path_ls; + for (const auto & p : ego_data.path.points) + path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) { + if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) + path_lanelets.push_back(dist_lanelet.second); + } + return path_lanelets; +} + lanelet::ConstLanelets calculate_ignored_lanelets( const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler, const PlannerParam & params) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets that follows path lanelets - for (const auto & path_lanelet : path_lanelets) - for (const auto & following : route_handler.getRoutingGraphPtr()->following(path_lanelet)) - if (!contains_lanelet(path_lanelets, following.id())) ignored_lanelets.push_back(following); // ignore lanelets directly behind ego const auto behind = planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0); @@ -55,26 +68,14 @@ lanelet::ConstLanelets calculate_other_lanelets( const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold)); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) { - if ( - boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || - boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || - boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + + params.extra_front_offset); for (const auto & ll : lanelets_within_range) { if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) - other_lanelets.push_back(ll.second); + if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); } return other_lanelets; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index cd0b23768872d..9c3aee46c8be5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -33,6 +33,14 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane return l.id() == id; }) != lanelets.end(); }; + +/// @brief calculate lanelets crossed by the ego path +/// @details calculated from the ids of the path msg, the lanelets containing path points +/// @param [in] ego_data data about the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets crossed by the ego vehicle +lanelet::ConstLanelets calculate_path_lanelets( + const EgoData & ego_data, const route_handler::RouteHandler & route_handler); /// @brief calculate lanelets that should be ignored /// @param [in] ego_data data about the ego vehicle /// @param [in] path_lanelets lanelets driven by the ego vehicle diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index 6f74f42af2056..6dfb41ccfbfcb 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -34,6 +34,7 @@ Overlap calculate_overlap( Overlap overlap; const auto & left_bound = lanelet.leftBound2d().basicLineString(); const auto & right_bound = lanelet.rightBound2d().basicLineString(); + // TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound); const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 1f6a332e27e8d..ef4434e235244 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -66,7 +66,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto ego_data.pose = planner_data_->current_odometry->pose; ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); motion_utils::removeOverlapPoints(ego_data.path.points); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; @@ -75,13 +75,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto path_footprints = calculate_path_footprints(ego_data, params_); const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider - const auto path_lanelets = planning_utils::getLaneletsOnPath( - ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_odometry->pose); + stopwatch.tic("calculate_lanelets"); + const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); const auto other_lanelets = calculate_other_lanelets( ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); debug_data_.footprints = path_footprints; debug_data_.path_lanelets = path_lanelets; @@ -98,6 +98,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto }); if (overlapped_lanelet_it != other_lanelets.end()) { debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); + // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n"); return true; } @@ -183,13 +184,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto RCLCPP_DEBUG( logger_, "Total time = %2.2fus\n" + "\tcalculate_lanelets = %2.0fus\n" "\tcalculate_path_footprints = %2.0fus\n" "\tcalculate_overlapping_ranges = %2.0fus\n" "\tcalculate_decisions = %2.0fus\n" "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", - total_time_us, calculate_path_footprints_us, calculate_overlapping_ranges_us, - calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us); + total_time_us, calculate_lanelets_us, calculate_path_footprints_us, + calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us, + insert_slowdown_points_us); return true; }