From 99ae44dab392a52154a827907c0997865d65bc22 Mon Sep 17 00:00:00 2001 From: yuki-takagi-66 Date: Tue, 14 Jan 2025 20:59:26 +0900 Subject: [PATCH] po Signed-off-by: yuki-takagi-66 --- .../src/manager.cpp | 600 ------------------ .../src/scene_module_interface.cpp | 1 + 2 files changed, 1 insertion(+), 600 deletions(-) delete mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp 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 deleted file mode 100644 index dcb0ebb5ba688..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ /dev/null @@ -1,600 +0,0 @@ -// 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/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index a284500b1a591..5680c8a23d594 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -163,6 +163,7 @@ void SceneModuleManagerInterface::deleteExpiredModules( auto itr = scene_modules_.begin(); while (itr != scene_modules_.end()) { if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); itr = scene_modules_.erase(itr); } else { itr++;