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..e69774a4f5ac2 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 @@ -95,6 +95,8 @@ class LaneChangeBase virtual bool isAbleToReturnCurrentLane() const = 0; + virtual bool isStoppedAtTrafficSignal() const = 0; + virtual LaneChangePath getLaneChangePath() const = 0; virtual bool isEgoOnPreparePhase() const = 0; 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..978bae6026735 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 @@ -90,6 +90,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbleToStopSafely() const override; + bool isStoppedAtTrafficSignal() const override; + bool hasFinishedAbort() const override; bool isAbortState() const override; @@ -135,6 +137,10 @@ class NormalLaneChange : public LaneChangeBase bool hasEnoughLengthToIntersection( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, + lanelet::Id & traffic_light_id) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index c664ae3e15aef..3f53f4b9c51e8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,7 @@ struct LaneChangeParameters // regulatory elements bool regulate_on_crosswalk{false}; bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; // ego vehicle stuck detection double stop_velocity_threshold{0.1}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index bfa83c43b061c..84c194ed8ad70 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -130,7 +130,9 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); -double getDistanceToNextTrafficLight( +bool isRedTrafficSignal(const TrafficSignalStamped & traffic_signal); + +std::pair getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); double getDistanceToNextIntersection( 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 58a8e7e181e35..3f64eb7046706 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 @@ -125,6 +125,26 @@ bool NormalLaneChange::isLaneChangeRequired() const return !target_lanes.empty(); } +bool NormalLaneChange::isStoppedAtTrafficSignal() const +{ + lanelet::Id next_traffic_light_id = lanelet::InvalId; + const auto has_enough_length_to_traffic_light = hasEnoughLengthToTrafficLight( + status_.lane_change_path, status_.current_lanes, next_traffic_light_id); + if (has_enough_length_to_traffic_light) { + return false; + } + + const auto traffic_light_id_map = planner_data_->traffic_light_id_map; + + if (traffic_light_id_map.find(next_traffic_light_id) == traffic_light_id_map.end()) { + return false; + } + + const auto traffic_signal = planner_data_->traffic_light_id_map.at(next_traffic_light_id); + + return utils::isRedTrafficSignal(traffic_signal); +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return isAbortState() ? *abort_path_ : status_.lane_change_path; @@ -1063,6 +1083,25 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( return true; } +bool NormalLaneChange::hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, + lanelet::Id & traffic_light_id) const +{ + const auto current_pose = getEgoPose(); + const auto [next_traffic_light_id, dist_to_next_traffic_light] = + utils::getDistanceToNextTrafficLight(current_pose, current_lanes); + traffic_light_id = next_traffic_light_id; + const auto dist_to_next_traffic_light_from_lc_start_pose = + dist_to_next_traffic_light - path.info.length.prepare; + + std::cerr << __func__ << "dist to traffic light = " << dist_to_next_traffic_light + << "\tdist from start pose " << dist_to_next_traffic_light_from_lc_start_pose + << "\tlength.prapare " << path.info.length.prepare << '\n'; + + return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || + dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; +} + bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index f1bbb3bc28a36..46fb32ad1bf3a 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -426,7 +426,8 @@ bool isForceAvoidanceTarget( bool not_parked_object = true; // check traffic light - const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); + const auto [next_traffic_light_id, to_traffic_light] = + utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); { not_parked_object = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 37a628f54607e..95242f2ab267c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -26,6 +26,8 @@ #include #include +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" + #include #include @@ -136,6 +138,7 @@ namespace behavior_path_planner::utils { using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::TrafficSignalElement; using geometry_msgs::msg::PoseWithCovarianceStamped; using tf2::fromMsg; using tier4_autoware_utils::Point2d; @@ -2289,12 +2292,20 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan return lanelet_length - arc_coordinates.length; } -double getDistanceToNextTrafficLight( +bool isRedTrafficSignal(const TrafficSignalStamped & traffic_signal) +{ + if (traffic_signal.signal.elements.empty()) { + return false; + } + return traffic_signal.signal.elements.front().color == TrafficSignalElement::RED; +} + +std::pair getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { lanelet::ConstLanelet current_lanelet; if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::infinity(); + return {lanelet::InvalId, std::numeric_limits::infinity()}; } const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); @@ -2312,7 +2323,7 @@ double getDistanceToNextTrafficLight( const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; if (distance_object_to_stop_line > 0.0) { - return distance_object_to_stop_line; + return {element->id(), distance_object_to_stop_line}; } } @@ -2336,13 +2347,13 @@ double getDistanceToNextTrafficLight( lanelet::utils::to2D(llt.centerline()), lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); - return distance + to_stop_line.length - to_object.length; + return {element->id(), distance + to_stop_line.length - to_object.length}; } distance += lanelet::utils::getLaneletLength3d(llt); } - return std::numeric_limits::infinity(); + return {lanelet::InvalId, std::numeric_limits::infinity()}; } double getDistanceToNextIntersection(