diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index 735f807949492..01c21d2197202 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -43,9 +44,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; - bool hasSameParentLaneletWithRegistered(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -63,7 +67,12 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 70813af895d82..0ebd31bcedd28 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -97,10 +97,6 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (hasSameParentLaneletWithRegistered(ll)) { - continue; - } - // Is intersection? const std::string turn_direction = ll.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -108,6 +104,11 @@ void IntersectionModuleManager::launchNewModules( if (!is_intersection) { continue; } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll, turn_direction)) { + continue; + } + registerModule(std::make_shared( module_id, lane_id, planner_data_, intersection_param_, logger_.get_child("intersection_module"), clock_)); @@ -142,29 +143,31 @@ void MergeFromPrivateModuleManager::launchNewModules( // 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") { - 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") { + if (lane_location != "private") { + 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") { + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, + 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") { registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, 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") { - registerModule(std::make_shared( - module_id, lane_id, planner_data_, merge_from_private_area_param_, - logger_.get_child("merge_from_private_road_module"), clock_)); - continue; - } - } } } } @@ -185,7 +188,8 @@ IntersectionModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { return false; } } @@ -208,7 +212,8 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { return false; } } @@ -216,8 +221,9 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( }; } -bool IntersectionModuleManager::hasSameParentLaneletWith( - const lanelet::ConstLanelet & lane, const size_t module_id) const +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); @@ -227,15 +233,16 @@ bool IntersectionModuleManager::hasSameParentLaneletWith( for (const auto & ll : registered_parents) { auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { + const auto turn_direction_ll = ll.attributeOr("turn_direction", "else"); + if (turn_direction_ll == turn_direction_lane && hasSameLanelet(parents, neighbor_lanes)) { return true; } } return false; } -bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( - const lanelet::ConstLanelet & lane) const +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); @@ -246,7 +253,8 @@ bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( for (const auto & ll : registered_parents) { auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { + const auto turn_direction_ll = ll.attributeOr("turn_direction", "else"); + if (turn_direction == turn_direction_ll && hasSameLanelet(parents, neighbor_lanes)) { return true; } } @@ -254,8 +262,9 @@ bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( return false; } -bool MergeFromPrivateModuleManager::hasSameParentLaneletWith( - const lanelet::ConstLanelet & lane, const size_t module_id) const +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); @@ -265,7 +274,8 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletWith( for (const auto & ll : registered_parents) { auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { + const auto turn_direction_ll = ll.attributeOr("turn_direction", "else"); + if (turn_direction == turn_direction_ll && hasSameLanelet(parents, neighbor_lanes)) { return true; } }