From a50469459a0c1fd8f2ee57767c98ea0fec3f5b9d Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 13 Jul 2022 14:41:57 +0900 Subject: [PATCH] fix(behavior_velocity_planner): sort unique_lane_ids for merge_from_private_area (#1323) * fix(behavior_velocity_planner): sort unique_lane_ids for merge_from_private_area Signed-off-by: tomoya.kimura * use same function Signed-off-by: tomoya.kimura * Apply suggestions from code review * fix Signed-off-by: tomoya.kimura * Apply suggestions from code review Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Apply suggestions from code review Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- .../include/utilization/util.hpp | 36 ++++++------ .../src/scene_module/crosswalk/manager.cpp | 30 +++++----- .../src/utilization/util.cpp | 56 +++++++++++++++---- 3 files changed, 75 insertions(+), 47 deletions(-) diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 80409f3ec92f4..ce539be3e2050 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -296,36 +296,36 @@ std::vector concatVector(const std::vector & vec1, const std::vector & return concat_vec; } +boost::optional getNearestLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose, + boost::optional & nearest_segment_idx); + template std::unordered_map, lanelet::ConstLanelet> getRegElemMapOnPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) { std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; - std::set unique_lane_ids; - auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( - path.points, current_pose, std::numeric_limits::max(), M_PI_2); // Add current lane id - lanelet::ConstLanelets current_lanes; - if ( - lanelet::utils::query::getCurrentLanelets( - lanelet::utils::query::laneletLayer(lanelet_map), current_pose, ¤t_lanes) && - nearest_segment_idx) { - for (const auto & ll : current_lanes) { - if ( - ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) || - ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) { - unique_lane_ids.insert(ll.id()); - } - } + boost::optional nearest_segment_idx; + const auto nearest_lane_id = + getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + unique_lane_ids.emplace_back(*nearest_lane_id); } // Add forward path lane_id - const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0; + const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0; for (size_t i = start_idx; i < path.points.size(); i++) { - unique_lane_ids.insert( - path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was. + const int64_t lane_id = path.points.at(i).lane_ids.at(0); + if ( + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) { + unique_lane_ids.emplace_back(lane_id); + } } for (const auto lane_id : unique_lane_ids) { diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 569f8217eff15..43b2c331217ff 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -28,33 +29,28 @@ std::vector getCrosswalksOnPath( const std::shared_ptr & overall_graphs) { std::vector crosswalks; - std::set unique_lane_ids; auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( path.points, current_pose, std::numeric_limits::max(), M_PI_2); // Add current lane id - lanelet::ConstLanelets current_lanes; - if ( - lanelet::utils::query::getCurrentLanelets( - lanelet::utils::query::laneletLayer(lanelet_map), current_pose, ¤t_lanes) && - nearest_segment_idx) { - for (const auto & ll : current_lanes) { - if ( - ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) || - ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) { - unique_lane_ids.insert(ll.id()); - } - } + const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId( + path, lanelet_map, current_pose, nearest_segment_idx); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + unique_lane_ids.emplace_back(*nearest_lane_id); } // Add forward path lane_id - const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0; + const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0; for (size_t i = start_idx; i < path.points.size(); i++) { - unique_lane_ids.insert( - path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was. + const int64_t lane_id = path.points.at(i).lane_ids.at(0); + if ( + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) { + unique_lane_ids.emplace_back(lane_id); + } } - for (const auto lane_id : unique_lane_ids) { const auto ll = lanelet_map->laneletLayer.get(lane_id); diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 42d0b973d51a0..acbb6773c23ad 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -593,34 +593,66 @@ LineString2d extendLine( {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}}; } -std::vector getLaneletsOnPath( +boost::optional getNearestLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) + const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose, + boost::optional & nearest_segment_idx) { - std::set unique_lane_ids; - auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( + boost::optional nearest_lane_id; + + nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( path.points, current_pose, std::numeric_limits::max(), M_PI_2); - // Add current lane id + if (!nearest_segment_idx) { + return boost::none; + } + lanelet::ConstLanelets current_lanes; if ( lanelet::utils::query::getCurrentLanelets( lanelet::utils::query::laneletLayer(lanelet_map), current_pose, ¤t_lanes) && nearest_segment_idx) { for (const auto & ll : current_lanes) { - if ( - ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) || - ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) { - unique_lane_ids.insert(ll.id()); + if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) { + nearest_lane_id = ll.id(); + return nearest_lane_id; + } + } + + // if the lane_id of nearest_segment_idx does not involved in current_lanes, + // search the lane_id of nearest_segment_idx + 1 + *nearest_segment_idx += 1; + for (const auto & ll : current_lanes) { + if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) { + nearest_lane_id = ll.id(); + return nearest_lane_id; } } } + return boost::none; +} + +std::vector getLaneletsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) +{ + boost::optional nearest_segment_idx; + const auto nearest_lane_id = + getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + unique_lane_ids.emplace_back(*nearest_lane_id); + } // Add forward path lane_id - const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0; + const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0; for (size_t i = start_idx; i < path.points.size(); i++) { - unique_lane_ids.insert( - path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was. + const int64_t lane_id = path.points.at(i).lane_ids.at(0); + if ( + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) { + unique_lane_ids.emplace_back(lane_id); + } } std::vector lanelets;