diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index ca58144464731..6303455c03b0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -156,7 +156,7 @@ struct PlannerData std::optional prev_modified_goal{}; std::optional prev_route_id{}; std::shared_ptr route_handler{std::make_shared()}; - std::map traffic_light_id_map; + std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; @@ -174,7 +174,7 @@ struct PlannerData route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } - std::optional getTrafficSignal(const int id) const + std::optional getTrafficSignal(const int64_t id) const { if (traffic_light_id_map.count(id) == 0) { return std::nullopt; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 472564a061f04..c9b2d510cb635 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -107,6 +107,8 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; + virtual bool isStoppedAtRedTrafficLight() const = 0; + virtual bool getAbortPath() = 0; virtual bool specialRequiredCheck() const { return false; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index dc9f44fbd53da..9e113009c921e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -96,6 +96,8 @@ class NormalLaneChange : public LaneChangeBase bool isLaneChangeRequired() const override; + bool isStoppedAtRedTrafficLight() const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp index aa3e7f4833134..202a5d3473620 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp @@ -27,6 +27,7 @@ #include #include +#include #include namespace behavior_path_planner::utils::traffic_light @@ -107,6 +108,27 @@ double getDistanceToNextTrafficLight( std::optional calcDistanceToRedTrafficLight( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data); + +/** + * @brief Checks if the vehicle is stationary within a specified distance from a red traffic light. + * + * This function first checks if the vehicle's velocity is below a minimum threshold, indicating it + * is stopped. It then calculates the distance to the nearest red traffic light using the + * calcDistanceToRedTrafficLight function. If the vehicle is within the specified distance threshold + * from the red traffic light, the function returns true, otherwise false. + * + * @param lanelets The lanelets to search for traffic lights. + * @param path The path along which to measure the distance to the traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @param distance_threshold The maximum allowable distance from a red traffic light to consider the + * vehicle stopped. + * @return True if the vehicle is stopped within the distance threshold from a red traffic light, + * false otherwise. + */ +bool isStoppedAtRedTrafficLightWithinDistance( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const double distance_threshold = std::numeric_limits::infinity()); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 7534c2c1fe45f..e86bf0c090997 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -108,6 +108,14 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::SUCCESS; } + if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Ego stopped at traffic light. Canceling lane change"); + module_type_->toCancelState(); + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; + } + const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); setObjectDebugVisualization(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index f2c6782b3ef6e..bd6bbe51b91cf 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/traffic_light_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -125,6 +126,13 @@ bool NormalLaneChange::isLaneChangeRequired() const return !target_lanes.empty(); } +bool NormalLaneChange::isStoppedAtRedTrafficLight() const +{ + return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + status_.current_lanes, status_.lane_change_path.path, planner_data_, + status_.lane_change_path.info.length.sum()); +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return isAbortState() ? *abort_path_ : status_.lane_change_path; @@ -1340,6 +1348,12 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + lane_change_info.length.sum())) { + debug_print("Ego is stopping near traffic light. Do not allow lane change"); + continue; + } candidate_paths->push_back(*candidate_path); std::vector filtered_objects = diff --git a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp index 9ec8531818d83..9069c86a79b29 100644 --- a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp @@ -154,4 +154,27 @@ std::optional calcDistanceToRedTrafficLight( return std::nullopt; } + +bool isStoppedAtRedTrafficLightWithinDistance( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const double distance_threshold) +{ + const auto ego_velocity = std::hypot( + planner_data->self_odometry->twist.twist.linear.x, + planner_data->self_odometry->twist.twist.linear.y); + constexpr double minimum_speed = 0.1; + if (ego_velocity > minimum_speed) { + return false; + } + + const auto distance_to_red_traffic_light = + calcDistanceToRedTrafficLight(lanelets, path, planner_data); + + if (!distance_to_red_traffic_light) { + return false; + } + + return (distance_to_red_traffic_light < distance_threshold); +} + } // namespace behavior_path_planner::utils::traffic_light