From 61f4b5be5b9eef96e6f383eddc76d50d7dca8b7b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 1 Nov 2024 18:52:48 +0900 Subject: [PATCH] fix(bvp): remove expired module safely (#9212) * fix(bvp): remove expired module safely Signed-off-by: satoshi-ota * fix: remove module id set Signed-off-by: satoshi-ota * fix: use itr to erase expired module Signed-off-by: satoshi-ota * fix: remove unused function Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 20 +- .../src/manager.cpp | 600 ++++++++++++++++++ .../scene_module_interface.hpp | 2 - .../src/scene_module_interface.cpp | 41 +- 4 files changed, 625 insertions(+), 38 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 79d31352ab911..fe35e37920bc5 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -303,20 +303,20 @@ void IntersectionModuleManager::deleteExpiredModules( { const auto isModuleExpired = getModuleExpiredFunction(path); - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { // default - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); // occlusion - const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto intersection_module = std::dynamic_pointer_cast(*itr); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); - unregisterModule(scene_module); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp new file mode 100644 index 0000000000000..dcb0ebb5ba688 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -0,0 +1,600 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + occlusion_rtc_interface_( + &node, "intersection_occlusion", + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) +{ + const std::string ns(IntersectionModuleManager::getModuleName()); + auto & ip = intersection_param_; + + // common + { + ip.common.attention_area_length = + getOrDeclareParameter(node, ns + ".common.attention_area_length"); + ip.common.attention_area_margin = + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + ip.common.attention_area_angle_threshold = + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.use_intersection_area = + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + ip.common.default_stopline_margin = + getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.stopline_overshoot_margin = + getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); + ip.common.path_interpolation_ds = + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); + } + + // stuck + { + // target_type + { + ip.stuck_vehicle.target_type.car = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.car"); + ip.stuck_vehicle.target_type.bus = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bus"); + ip.stuck_vehicle.target_type.truck = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.truck"); + ip.stuck_vehicle.target_type.trailer = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.trailer"); + ip.stuck_vehicle.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); + ip.stuck_vehicle.target_type.bicycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bicycle"); + ip.stuck_vehicle.target_type.unknown = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.unknown"); + } + + // turn_direction + { + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); + } + + ip.stuck_vehicle.use_stuck_stopline = + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); + ip.stuck_vehicle.stuck_vehicle_detect_dist = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); + } + + // yield_stuck + { + // target_type + { + ip.yield_stuck.target_type.car = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.car"); + ip.yield_stuck.target_type.bus = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bus"); + ip.yield_stuck.target_type.truck = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.truck"); + ip.yield_stuck.target_type.trailer = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.trailer"); + ip.yield_stuck.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.motorcycle"); + ip.yield_stuck.target_type.bicycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bicycle"); + ip.yield_stuck.target_type.unknown = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.unknown"); + } + + // turn_direction + { + ip.yield_stuck.turn_direction.left = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + ip.yield_stuck.turn_direction.right = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + ip.yield_stuck.turn_direction.straight = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + } + + ip.yield_stuck.distance_threshold = + getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + } + + // collision_detection + { + ip.collision_detection.consider_wrong_direction_vehicle = getOrDeclareParameter( + node, ns + ".collision_detection.consider_wrong_direction_vehicle"); + ip.collision_detection.collision_detection_hold_time = getOrDeclareParameter( + node, ns + ".collision_detection.collision_detection_hold_time"); + ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter( + node, ns + ".collision_detection.min_predicted_path_confidence"); + + // target_type + { + ip.collision_detection.target_type.car = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.car"); + ip.collision_detection.target_type.bus = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bus"); + ip.collision_detection.target_type.truck = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.truck"); + ip.collision_detection.target_type.trailer = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.trailer"); + ip.collision_detection.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.motorcycle"); + ip.collision_detection.target_type.bicycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bicycle"); + ip.collision_detection.target_type.unknown = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.unknown"); + } + + // velocity_profile + { + ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.use_upstream"); + ip.collision_detection.velocity_profile.minimum_upstream_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); + ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.default_velocity"); + ip.collision_detection.velocity_profile.minimum_default_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); + } + + // fully_prioritized + { + ip.collision_detection.fully_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + } + + // partially_prioritized + { + ip.collision_detection.partially_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + } + + // not_prioritized + { + ip.collision_detection.not_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); + } + + // yield_on_green_traffic_light + { + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + } + + // ignore_on_amber_traffic_light, ignore_on_red_traffic_light + { + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + } + + ip.collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point = + getOrDeclareParameter( + node, ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_" + "collision_point"); + } + + // occlusion + { + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + ip.occlusion.occlusion_attention_area_length = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.free_space_max = + getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); + ip.occlusion.attention_lane_crop_curvature_threshold = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + + // creep_during_peeking + { + ip.occlusion.creep_during_peeking.enable = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + } + + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); + ip.occlusion.possible_object_bbox = + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.occlusion_detection_hold_time = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + ip.occlusion.temporal_stop_time_before_peeking = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.creep_velocity_without_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + } + + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + + decision_state_pub_ = + node.create_publisher("~/debug/intersection/decision_state", 1); + tl_observation_pub_ = node.create_publisher( + "~/debug/intersection_traffic_signal", 1); +} + +void IntersectionModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + // run occlusion detection only in the first intersection + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + const auto new_module = std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + generateUUID(module_id); + /* set RTC status as non_occluded status initially */ + const UUID uuid = getUUID(new_module->getModuleId()); + const auto occlusion_uuid = new_module->getOcclusionUUID(); + rtc_interface_.updateCooperateStatus( + uuid, true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + registerModule(std::move(new_module)); + } +} + +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +void IntersectionModuleManager::sendRTC(const Time & stamp) +{ + double min_distance = std::numeric_limits::infinity(); + std::optional nearest_tl_observation{std::nullopt}; + std_msgs::msg::String decision_type; + + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const UUID uuid = getUUID(scene_module->getModuleId()); + const bool safety = + scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); + updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance, + stamp); + + // ========================================================================================== + // module debug data + // ========================================================================================== + const auto internal_debug_data = intersection_module->getInternalDebugData(); + if (internal_debug_data.distance < min_distance) { + min_distance = internal_debug_data.distance; + nearest_tl_observation = internal_debug_data.tl_observation; + } + decision_type.data += (internal_debug_data.decision_type + "\n"); + } + rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() + occlusion_rtc_interface_.publishCooperateStatus(stamp); + + // ========================================================================================== + // publish module debug data + // ========================================================================================== + decision_state_pub_->publish(decision_type); + if (nearest_tl_observation) { + tl_observation_pub_->publish(nearest_tl_observation.value().signal); + } +} + +void IntersectionModuleManager::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(getUUID(scene_module->getModuleId()))); + } +} + +void IntersectionModuleManager::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + // default + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(*itr); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(MergeFromPrivateModuleManager::getModuleName()); + auto & mp = merge_from_private_area_param_; + mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); + mp.attention_area_length = + node.get_parameter("intersection.common.attention_area_length").as_double(); + mp.stopline_margin = getOrDeclareParameter(node, ns + ".stopline_margin"); + mp.path_interpolation_ds = + node.get_parameter("intersection.common.path_interpolation_ds").as_double(); + mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); +} + +void MergeFromPrivateModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + // Is merging from private road? + // In case the goal is in private road, check if this lanelet is conflicting with urban lanelet + const std::string lane_location = ll.attributeOr("location", "else"); + if (lane_location != "private") { + continue; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (next_lane_location != "private") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } else { + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); + for (auto && conflicting_lanelet : conflicting_lanelets) { + const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); + if (conflicting_attr == "urban") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } + } + } +} + +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index fcde16d8a871c..13a3e2dc7e1d9 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -185,8 +185,6 @@ class SceneModuleManagerInterface void registerModule(const std::shared_ptr & scene_module); - void unregisterModule(const std::shared_ptr & scene_module); - size_t findEgoSegmentIndex( const std::vector & points) const; diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 362493005eb17..a284500b1a591 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -160,13 +160,12 @@ void SceneModuleManagerInterface::deleteExpiredModules( { const auto isModuleExpired = getModuleExpiredFunction(path); - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - unregisterModule(scene_module); + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + itr = scene_modules_.erase(itr); + } else { + itr++; } } } @@ -180,16 +179,6 @@ void SceneModuleManagerInterface::registerModule( scene_modules_.insert(scene_module); } -void SceneModuleManagerInterface::unregisterModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "unregister task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.erase(scene_module->getModuleId()); - scene_modules_.erase(scene_module); -} - SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc) : SceneModuleManagerInterface(node, module_name), @@ -265,15 +254,15 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( { const auto isModuleExpired = getModuleExpiredFunction(path); - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); - unregisterModule(scene_module); + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; } } }