diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index 62d4c5b7188ee..f535fe1184d74 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs):
"~/input/costmap",
"/planning/scenario_planning/parking/costmap_generator/occupancy_grid",
),
+ (
+ "~/input/traffic_signals",
+ "/perception/traffic_light_recognition/traffic_signals",
+ ),
("~/input/odometry", "/localization/kinematic_state"),
("~/input/accel", "/localization/acceleration"),
("~/input/scenario", "/planning/scenario_planning/scenario"),
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 4ff862c7852c6..8ffa682030a99 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -8,6 +8,7 @@
+
diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
index d333cbfd778cb..b4d323de45cde 100644
--- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
+++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -2,6 +2,7 @@
ros__parameters:
verbose: false
max_iteration_num: 100
+ traffic_light_signal_timeout: 1.0
planning_hz: 10.0
backward_path_length: 5.0
forward_path_length: 300.0
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
index 420a5a8aa6ee5..1a9b23be00a3f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -58,6 +58,7 @@ using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_perception_msgs::msg::TrafficSignalArray;
using autoware_planning_msgs::msg::LaneletRoute;
using autoware_planning_msgs::msg::PoseWithUuidStamped;
using nav_msgs::msg::OccupancyGrid;
@@ -93,6 +94,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Subscription::SharedPtr perception_subscriber_;
rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_;
rclcpp::Subscription::SharedPtr costmap_subscriber_;
+ rclcpp::Subscription::SharedPtr traffic_signals_subscriber_;
rclcpp::Subscription::SharedPtr lateral_offset_subscriber_;
rclcpp::Subscription::SharedPtr operation_mode_subscriber_;
rclcpp::Publisher::SharedPtr path_publisher_;
@@ -136,6 +138,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
void onPerception(const PredictedObjects::ConstSharedPtr msg);
void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
void onCostMap(const OccupancyGrid::ConstSharedPtr msg);
+ void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg);
void onMap(const HADMapBin::ConstSharedPtr map_msg);
void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
void onOperationMode(const OperationModeState::ConstSharedPtr msg);
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 9280a81e643ca..ca58144464731 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
@@ -20,6 +20,7 @@
#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
+#include
#include
#include
@@ -28,6 +29,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -39,6 +41,7 @@
#include
#include
+#include