From 1e1011e93c56145b000a02760f14ac55e4217518 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Mar 2024 17:34:57 +0900 Subject: [PATCH 1/2] add function to add path lanelets that may be missing during lane change Signed-off-by: Maxime CLEMENT --- .../src/lanelets_selection.cpp | 37 ++++++++++++++++++- .../src/lanelets_selection.hpp | 7 ++++ 2 files changed, 42 insertions(+), 2 deletions(-) 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 3c90a8cbf0dce..9380c1eaba702 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,12 +24,43 @@ namespace behavior_velocity_planner::out_of_lane { + +lanelet::ConstLanelets consecutive_lanelets( + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : path_lanelets) { + for (const auto & consecutive : consecutive_lanelets(route_handler, ll)) + if (!contains_lanelet(consecutives, consecutive.id())) consecutives.push_back(consecutive); + for (const auto & adjacent : routing_graph.besides(ll)) + if (!contains_lanelet(adjacents, adjacent.id())) adjacents.push_back(adjacent); + } + for (const auto & ll : adjacents) { + if ( + !contains_lanelet(missing_lane_change_lanelets, ll.id()) && + !contains_lanelet(path_lanelets, ll.id()) && contains_lanelet(consecutives, ll.id())) + missing_lane_change_lanelets.push_back(ll); + } + return missing_lane_change_lanelets; +} + 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::ConstLanelets path_lanelets; 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); @@ -38,6 +69,8 @@ lanelet::ConstLanelets calculate_path_lanelets( if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) path_lanelets.push_back(dist_lanelet.second); } + const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler); + path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); return path_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 9c3aee46c8be5..87757a0cb2230 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 @@ -41,6 +41,13 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @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 may not be crossed by the path but may be overlapped during a +/// lane change +/// @param [in] path_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, 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 From 905f170064dd12893a0cbd3131a5e2ceee684eb1 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Mar 2024 20:07:06 +0900 Subject: [PATCH 2/2] simplify get_missing_lane_change_lanelets function Signed-off-by: Maxime CLEMENT --- .../src/lanelets_selection.cpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) 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 9380c1eaba702..738ea22106b29 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 @@ -42,17 +42,21 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( lanelet::ConstLanelets adjacents; lanelet::ConstLanelets consecutives; for (const auto & ll : path_lanelets) { - for (const auto & consecutive : consecutive_lanelets(route_handler, ll)) - if (!contains_lanelet(consecutives, consecutive.id())) consecutives.push_back(consecutive); - for (const auto & adjacent : routing_graph.besides(ll)) - if (!contains_lanelet(adjacents, adjacent.id())) adjacents.push_back(adjacent); - } - for (const auto & ll : adjacents) { - if ( - !contains_lanelet(missing_lane_change_lanelets, ll.id()) && - !contains_lanelet(path_lanelets, ll.id()) && contains_lanelet(consecutives, ll.id())) - missing_lane_change_lanelets.push_back(ll); + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id()); + }); return missing_lane_change_lanelets; }