diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 786eb0b512696..edee62cf4ac52 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -297,18 +297,12 @@ PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const const int num_lane_change = std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - double optional_lengths{0.0}; - const auto isInIntersection = util::checkLaneIsInIntersection( - *route_handler, reference_path, current_lanes, common_parameters, num_lane_change, - optional_lengths); - if (isInIntersection) { - reference_path = util::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, optional_lengths); - } + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters, 0.0); const double lane_change_buffer = - util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); + util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 67c2aa296b1d8..d54283b853f26 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -305,18 +305,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const int num_lane_change = std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); - double optional_lengths{0.0}; - const auto isInIntersection = util::checkLaneIsInIntersection( - *route_handler, reference_path, current_lanes, common_parameters, num_lane_change, - optional_lengths); - if (isInIntersection) { - reference_path = util::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, optional_lengths); - } + + reference_path = util::getCenterLinePath( + *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters, 0.0); const double lane_change_buffer = - util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); + util::calcLaneChangeBuffer(common_parameters, num_lane_change, 0.0); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index a1c59c38365ed..82ac9daf973fb 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -455,12 +455,6 @@ bool hasEnoughDistance( return false; } - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToNextIntersection(current_pose, current_lanes)) { - return false; - } - if ( route_handler.isInGoalRouteSection(current_lanes.back()) && lane_change_total_distance + lane_change_required_distance > @@ -468,17 +462,15 @@ bool hasEnoughDistance( return false; } - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs)) { - return false; - } - // return is there is no target lanes. Else continue checking if (target_lanes.empty()) { return true; } + if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, target_lanes)) { + return false; + } + return true; } @@ -1001,12 +993,6 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( return false; } - if ( - abort_plus_lane_change_distance > - util::getDistanceToNextIntersection(current_pose, current_lanes)) { - return false; - } - if ( route_handler.isInGoalRouteSection(current_lanes.back()) && abort_plus_lane_change_distance > @@ -1014,13 +1000,6 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( return false; } - if ( - abort_plus_lane_change_distance > - util::getDistanceToCrosswalk( - current_pose, current_lanes, *route_handler.getOverallGraphPtr())) { - return false; - } - return true; } } // namespace behavior_path_planner::lane_change_utils 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 bcbe4a4bba40a..c42a311ffa623 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -42,6 +42,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -58,6 +60,8 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp index 6bdc554fff50c..a27d8b8d9e3c7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp @@ -43,6 +43,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + bool isModuleRegisteredFromRegElement(const lanelet::Id & id) const; + + bool hasSameTrafficLight( + const lanelet::TrafficLightConstPtr element, + const lanelet::TrafficLightConstPtr registered_element) const; + // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; 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 08e39abedb6f3..e56bb7258f585 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -26,6 +26,18 @@ namespace behavior_velocity_planner { +bool hasSameLanelet(const lanelet::ConstLanelets & lanes1, const lanelet::ConstLanelets & lanes2) +{ + for (const auto & lane1 : lanes1) { + for (const auto & lane2 : lanes2) { + if (lane1.id() == lane2.id()) { + return true; + } + } + } + return false; +} + IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC(node, getModuleName()) { @@ -84,7 +96,7 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (isModuleRegistered(module_id)) { + if (hasSameParentLanelet(ll)) { continue; } @@ -161,23 +173,86 @@ std::function &)> IntersectionModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); - return [lane_id_set](const std::shared_ptr & scene_module) { - return lane_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { + 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 (hasSameParentLanelet(lane)) { + return false; + } + } + return true; + }; } + std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); - return [lane_id_set](const std::shared_ptr & scene_module) { - return lane_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { + 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 (hasSameParentLanelet(lane)) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +{ + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + + for (const auto & id : registered_module_id_set_) { + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + 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)) { + return true; + } + } + } + return false; +} + +bool MergeFromPrivateModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +{ + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + + for (const auto & id : registered_module_id_set_) { + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + 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)) { + return true; + } + } + } + return false; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 523750c833889..58da4bbd4365b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); const auto module_id = lane_id; - if (!isModuleRegistered(module_id)) { + if (!isModuleRegisteredFromRegElement(module_id)) { registerModule(std::make_shared( module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_)); @@ -143,9 +143,47 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose); - return [lanelet_id_set](const std::shared_ptr & scene_module) { - return lanelet_id_set.count(scene_module->getModuleId()) == 0; + return [this, lanelet_id_set]( + [[maybe_unused]] const std::shared_ptr & scene_module) { + for (const auto & id : lanelet_id_set) { + if (isModuleRegisteredFromRegElement(id)) { + return false; + } + } + return true; }; } +bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(const lanelet::Id & id) const +{ + const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); + + for (const auto & registered_id : registered_module_id_set_) { + const auto registered_lane = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(registered_id); + for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { + for (const auto & element : lane.regulatoryElementsAs()) { + if (hasSameTrafficLight(element, registered_element)) { + return true; + } + } + } + } + return false; +} + +bool TrafficLightModuleManager::hasSameTrafficLight( + const lanelet::TrafficLightConstPtr element, + const lanelet::TrafficLightConstPtr registered_element) const +{ + for (const auto & traffic_light : element->trafficLights()) { + for (const auto & registered_traffic_light : registered_element->trafficLights()) { + if (traffic_light.id() == registered_traffic_light.id()) { + return true; + } + } + } + return false; +} + } // namespace behavior_velocity_planner diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 89576148f6cad..66c312431c50f 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -102,6 +102,7 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; + lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const; /** * @brief Check if same-direction lane is available at the right side of the lanelet diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 288b3884baaa6..74a17ffb31c7f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -426,6 +426,12 @@ bool RouteHandler::isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const return !getNextLaneletWithinRoute(lanelet, &next_lanelet); } +lanelet::ConstLanelets RouteHandler::getLaneChangeableNeighbors( + const lanelet::ConstLanelet & lanelet) const +{ + return lanelet::utils::query::getLaneChangeableNeighbors(routing_graph_ptr_, lanelet); +} + lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( const lanelet::ConstLanelet & lanelet, const double min_length) const {