From 6e44bfa64d6d6d5ab3d130a133b715ba20a07d4e Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 1 Aug 2022 15:32:04 +0900 Subject: [PATCH] fix(behavior_velocity_planner): avoid duplicates in intersection module Signed-off-by: Maxime CLEMENT --- .../scene_module/intersection/util.hpp | 5 ++- .../intersection/scene_intersection.cpp | 8 ++-- .../scene_merge_from_private_road.cpp | 2 +- .../src/scene_module/intersection/util.cpp | 38 ++++++++++++++----- 4 files changed, 37 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 2abe51203593..c1be351e1fe7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -58,7 +58,7 @@ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const double right_margin, const double left_margin, std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, + lanelet::ConstLanelets * objective_lanelets_result, std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger); @@ -118,6 +118,9 @@ bool getStopPoseIndexFromMap( std::vector getPolygon3dFromLaneletsVec( const std::vector & ll_vec, double clip_length); +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec, double clip_length); + std::vector getLaneletIdsFromLaneletsVec(const std::vector & ll_vec); double calcArcLengthFromPath( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 41e869f1ec7c..0dcd599de928 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -90,7 +90,7 @@ bool IntersectionModule::modifyPathVelocity( const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area and conflicting area */ - std::vector detection_area_lanelets; + lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; std::vector detection_area_lanelets_with_margin; @@ -101,15 +101,15 @@ bool IntersectionModule::modifyPathVelocity( logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); - std::vector detection_areas = util::getPolygon3dFromLaneletsVec( - detection_area_lanelets, planner_param_.detection_area_length); + std::vector detection_areas = + util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas_with_margin = util::getPolygon3dFromLaneletsVec( detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = - util::getLaneletIdsFromLaneletsVec(detection_area_lanelets); + util::getLaneletIdsFromLaneletsVec({detection_area_lanelets}); if (detection_areas.empty()) { RCLCPP_DEBUG(logger_, "no detection area. skip computation."); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index bf1d09f45551..7bfbcea3f2eb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -65,7 +65,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ - std::vector detection_area_lanelets; + lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; std::vector detection_area_lanelets_with_margin; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index d8e06a83ea34..594e2e2e35d2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -337,7 +337,7 @@ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, double right_margin, double left_margin, std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, + lanelet::ConstLanelets * objective_lanelets_result, std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger) { @@ -399,21 +399,28 @@ bool getObjectiveLanelets( // get possible lanelet path that reaches conflicting_lane longer than given length const double length = detection_area_length; - std::vector objective_lanelets_sequences; + lanelet::ConstLanelets objective_and_preceding_lanelets; + std::set objective_ids; for (const auto & ll : objective_lanelets) { // Preceding lanes does not include objective_lane so add them at the end - objective_lanelets_sequences.push_back(ll); + for (const auto & l : ll) { + const auto & inserted = objective_ids.insert(l.id()); + if (inserted.second) objective_and_preceding_lanelets.push_back(l); + } // get preceding lanelets without ego_lanelets // to prevent the detection area from including the ego lanes and its' preceding lanes. const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( routing_graph_ptr, ll.front(), length, ego_lanelets); - for (auto & l : lanelet_sequences) { - objective_lanelets_sequences.push_back(l); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = objective_ids.insert(l.id()); + if (inserted.second) objective_and_preceding_lanelets.push_back(l); + } } } *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; - *objective_lanelets_result = objective_lanelets_sequences; + *objective_lanelets_result = objective_and_preceding_lanelets; *objective_lanelets_with_margin_result = objective_lanelets_with_margin; // set this flag true when debugging @@ -432,10 +439,8 @@ bool getObjectiveLanelets( for (const auto & l : objective_lanelets) { ss_o << l.front().id() << ", "; } - for (const auto & l : objective_lanelets_sequences) { - for (const auto & ll : l) { - ss_os << ll.id() << ", "; - } + for (const auto & l : objective_and_preceding_lanelets) { + ss_os << l.id() << ", "; } RCLCPP_INFO( logger, "getObjectiveLanelets() conflict = %s yield = %s ego = %s", ss_c.str().c_str(), @@ -459,6 +464,19 @@ std::vector getPolygon3dFromLaneletsVec( return p_vec; } +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec, double clip_length) +{ + std::vector p_vec; + for (const auto & ll : ll_vec) { + const double path_length = lanelet::utils::getLaneletLength3d({ll}); + const auto polygon3d = + lanelet::utils::getPolygonFromArcLength({ll}, path_length - clip_length, path_length); + p_vec.push_back(polygon3d); + } + return p_vec; +} + std::vector getLaneletIdsFromLaneletsVec(const std::vector & ll_vec) { std::vector id_list;