From 66f333e2b252dd085fe966b5adf9fc317a776253 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 25 Aug 2023 17:34:28 +0900 Subject: [PATCH] fix(detection_area): search collision index only in lanelet (#695) * fix(detection_area): search collision index only in lanelet * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene_module/detection_area/scene.hpp | 5 ++++- .../scene_module/detection_area/manager.cpp | 21 +++++++++++-------- .../src/scene_module/detection_area/scene.cpp | 18 +++++++++++----- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 35b9c9d4ab55b..38f977dfc902f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface public: DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; + + int64_t lane_id_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index 8dc92269a4e1d..b508dbf353d6d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -28,22 +28,24 @@ namespace behavior_velocity_planner { namespace { -std::vector getDetectionAreaRegElemsOnPath( +using DetectionAreaWithLaneId = std::pair; + +std::vector getDetectionAreaRegElemsOnPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { - std::vector detection_area_reg_elems; + std::vector detection_areas_with_lane_id; for (const auto & p : path.points) { const auto lane_id = p.lane_ids.at(0); const auto ll = lanelet_map->laneletLayer.get(lane_id); const auto detection_areas = ll.regulatoryElementsAs(); for (const auto & detection_area : detection_areas) { - detection_area_reg_elems.push_back(detection_area); + detection_areas_with_lane_id.push_back(std::make_pair(detection_area, lane_id)); } } - return detection_area_reg_elems; + return detection_areas_with_lane_id; } std::set getDetectionAreaIdSetOnPath( @@ -52,7 +54,7 @@ std::set getDetectionAreaIdSetOnPath( { std::set detection_area_id_set; for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) { - detection_area_id_set.insert(detection_area->id()); + detection_area_id_set.insert(detection_area.first->id()); } return detection_area_id_set; } @@ -72,14 +74,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) void DetectionAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & detection_area : + for (const auto & detection_area_with_lane_id : getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = detection_area->id(); + const auto module_id = detection_area_with_lane_id.first->id(); + const auto lane_id = detection_area_with_lane_id.second; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"), - clock_)); + module_id, lane_id, *(detection_area_with_lane_id.first), planner_param_, + logger_.get_child("detection_area_module"), clock_)); } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 8d0454d2609dd..c383418c8eac8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -69,9 +69,13 @@ boost::optional getNearestCollisionPoint( } boost::optional findCollisionSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const SearchRangeIndex & search_index) { - for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto min_search_index = std::max(static_cast(0), search_index.min_idx); + const auto max_search_index = std::min(search_index.max_idx, path.points.size() - 1); + + for (size_t i = min_search_index; i < max_search_index; ++i) { const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point @@ -186,13 +190,15 @@ geometry_msgs::msg::Pose calcTargetPose( } // namespace DetectionAreaModule::DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), - planner_param_(planner_param) + planner_param_(planner_param), + lane_id_(lane_id) { } @@ -392,8 +398,10 @@ boost::optional DetectionAreaModule::createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin) const { + const auto dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_); + // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, dst_search_range); if (!collision_segment) { // No collision return {};