diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt
index ac33966a1bf7c..35fa1584c4635 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt
@@ -35,6 +35,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities
     test/test_utils.cpp
     test/test_path_utils.cpp
+    test/test_traffic_light_utils.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}_utilities
@@ -93,4 +94,6 @@ if(BUILD_TESTING)
   )
 endif()
 
-ament_auto_package()
+ament_auto_package(INSTALL_TO_SHARE
+  test_data
+)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp
index 19c9bbcf365a3..c31bddd921f99 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp
@@ -130,7 +130,7 @@ bool isStoppedAtRedTrafficLightWithinDistance(
     return false;
   }
 
-  return (distance_to_red_traffic_light < distance_threshold);
+  return (distance_to_red_traffic_light.value() < distance_threshold);
 }
 
 bool isTrafficSignalStop(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp
new file mode 100644
index 0000000000000..a5d77b96292f9
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_traffic_light_utils.cpp
@@ -0,0 +1,174 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "autoware/behavior_path_planner_common/data_manager.hpp"
+#include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp"
+#include "autoware_test_utils/autoware_test_utils.hpp"
+#include "autoware_test_utils/mock_data_parser.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <autoware_perception_msgs/msg/detail/traffic_light_group__struct.hpp>
+#include <autoware_planning_msgs/msg/detail/lanelet_route__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/twist__struct.hpp>
+#include <tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp>
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/Forward.h>
+#include <yaml-cpp/node/node.h>
+
+#include <limits>
+#include <memory>
+#include <string>
+
+using autoware::behavior_path_planner::PlannerData;
+using autoware::behavior_path_planner::TrafficSignalStamped;
+using autoware_perception_msgs::msg::TrafficLightGroupArray;
+using autoware_planning_msgs::msg::LaneletRoute;
+using geometry_msgs::msg::Pose;
+
+const double epsilon = 1e-06;
+
+class TrafficLightTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    planner_data_ = std::make_shared<PlannerData>();
+    const auto test_data_file =
+      ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner_common") +
+      "/test_data/test_traffic_light.yaml";
+    YAML::Node config = YAML::LoadFile(test_data_file);
+
+    set_current_pose(config);
+    set_route_handler(config);
+    set_traffic_signal(config);
+  }
+
+  void set_current_pose(YAML::Node config)
+  {
+    const auto self_odometry =
+      autoware::test_utils::parse<nav_msgs::msg::Odometry>(config["self_odometry"]);
+    planner_data_->self_odometry = std::make_shared<const nav_msgs::msg::Odometry>(self_odometry);
+  }
+
+  void set_route_handler(YAML::Node config)
+  {
+    const auto route = autoware::test_utils::parse<LaneletRoute>(config["route"]);
+    const auto map_path =
+      autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as<std::string>());
+    if (!map_path.has_value()) return;
+    const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value());
+    planner_data_->route_handler->setMap(intersection_map);
+    planner_data_->route_handler->setRoute(route);
+
+    for (const auto & segment : route.segments) {
+      lanelets.push_back(
+        planner_data_->route_handler->getLaneletsFromId(segment.preferred_primitive.id));
+    }
+  }
+
+  void set_traffic_signal(YAML::Node config)
+  {
+    const auto traffic_light =
+      autoware::test_utils::parse<TrafficLightGroupArray>(config["traffic_signal"]);
+    for (const auto & signal : traffic_light.traffic_light_groups) {
+      TrafficSignalStamped traffic_signal;
+      traffic_signal.stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
+      traffic_signal.signal = signal;
+      planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal;
+    }
+  }
+
+  void set_zero_velocity()
+  {
+    nav_msgs::msg::Odometry odometry;
+    odometry.pose.pose = planner_data_->self_odometry->pose.pose;
+    odometry.twist.twist.linear = autoware::universe_utils::createVector3(0.0, 0.0, 0.0);
+    planner_data_->self_odometry = std::make_shared<const nav_msgs::msg::Odometry>(odometry);
+  }
+
+  lanelet::ConstLanelets lanelets;
+  std::shared_ptr<PlannerData> planner_data_;
+};
+
+TEST_F(TrafficLightTest, getDistanceToNextTrafficLight)
+{
+  using autoware::behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight;
+
+  {
+    const Pose pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    const lanelet::ConstLanelets empty_lanelets;
+    EXPECT_DOUBLE_EQ(
+      getDistanceToNextTrafficLight(pose, empty_lanelets), std::numeric_limits<double>::infinity());
+  }
+  {
+    EXPECT_NEAR(
+      getDistanceToNextTrafficLight(planner_data_->self_odometry->pose.pose, lanelets), 117.1599371,
+      epsilon);
+  }
+}
+
+TEST_F(TrafficLightTest, calcDistanceToRedTrafficLight)
+{
+  using autoware::behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight;
+
+  {
+    const tier4_planning_msgs::msg::PathWithLaneId path;
+    const lanelet::ConstLanelets empty_lanelets;
+    EXPECT_FALSE(calcDistanceToRedTrafficLight(empty_lanelets, path, planner_data_).has_value());
+  }
+  {
+    const auto path = planner_data_->route_handler->getCenterLinePath(lanelets, 0.0, 300.0);
+    const auto distance = calcDistanceToRedTrafficLight(lanelets, path, planner_data_);
+    ASSERT_TRUE(distance.has_value());
+    EXPECT_NEAR(distance.value(), 117.1096960, epsilon);
+  }
+}
+
+TEST_F(TrafficLightTest, isStoppedAtRedTrafficLightWithinDistance)
+{
+  using autoware::behavior_path_planner::utils::traffic_light::
+    isStoppedAtRedTrafficLightWithinDistance;
+  const auto distance_threshold = 10.0;
+  const auto path = planner_data_->route_handler->getCenterLinePath(lanelets, 0.0, 300.0);
+  {
+    EXPECT_FALSE(
+      isStoppedAtRedTrafficLightWithinDistance(lanelets, path, planner_data_, distance_threshold));
+  }
+  {
+    set_zero_velocity();
+    EXPECT_FALSE(
+      isStoppedAtRedTrafficLightWithinDistance(lanelets, path, planner_data_, distance_threshold));
+  }
+  {
+    set_zero_velocity();
+    EXPECT_TRUE(isStoppedAtRedTrafficLightWithinDistance(lanelets, path, planner_data_));
+  }
+}
+
+TEST_F(TrafficLightTest, isTrafficSignalStop)
+{
+  using autoware::behavior_path_planner::utils::traffic_light::isTrafficSignalStop;
+
+  {
+    const lanelet::ConstLanelets empty_lanelets;
+    EXPECT_FALSE(isTrafficSignalStop(empty_lanelets, planner_data_));
+  }
+  {
+    EXPECT_TRUE(isTrafficSignalStop(lanelets, planner_data_));
+  }
+}
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp
index 09c7561955801..4326cfdb8e6be 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc. All rights reserved.
+// Copyright 2024 TIER IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_data/test_traffic_light.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_data/test_traffic_light.yaml
new file mode 100644
index 0000000000000..10d40a15fdb5a
--- /dev/null
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_data/test_traffic_light.yaml
@@ -0,0 +1,285 @@
+#
+# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver
+# format1:
+#
+# format_version: <format-major-version, int>
+# map_path_uri: package://<package-name>/<resource-path>
+# fields(this is array)
+#   - name: <field-name-for-your-yaml-of-this-topic, str>
+#     type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD}
+#     topic: <topic-name, str>
+#
+format_version: 1
+map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm
+dynamic_object:
+  header:
+    stamp:
+      sec: 50
+      nanosec: 334681087
+    frame_id: map
+  objects: []
+traffic_signal:
+  stamp:
+    sec: 1732179380
+    nanosec: 814263853
+  traffic_light_groups:
+    - traffic_light_group_id: 3010757
+      elements:
+        - color: 1
+          shape: 1
+          status: 2
+          confidence: 1.00000
+route:
+  header:
+    stamp:
+      sec: 24
+      nanosec: 685572149
+    frame_id: map
+  start_pose:
+    position:
+      x: 363.790
+      y: 433.627
+      z: 100.000
+    orientation:
+      x: 0.00000
+      y: 0.00000
+      z: 0.695229
+      w: 0.718789
+  goal_pose:
+    position:
+      x: 356.900
+      y: 863.677
+      z: 100.000
+    orientation:
+      x: 0.00000
+      y: 0.00000
+      z: 0.715472
+      w: 0.698641
+  segments:
+    - preferred_primitive:
+        id: 1001
+        primitive_type: ""
+      primitives:
+        - id: 1000
+          primitive_type: lane
+        - id: 1001
+          primitive_type: lane
+        - id: 1002
+          primitive_type: lane
+        - id: 1003
+          primitive_type: lane
+    - preferred_primitive:
+        id: 1011
+        primitive_type: ""
+      primitives:
+        - id: 1010
+          primitive_type: lane
+        - id: 1011
+          primitive_type: lane
+        - id: 1012
+          primitive_type: lane
+    - preferred_primitive:
+        id: 1101
+        primitive_type: ""
+      primitives:
+        - id: 1100
+          primitive_type: lane
+        - id: 1101
+          primitive_type: lane
+        - id: 1102
+          primitive_type: lane
+    - preferred_primitive:
+        id: 3501
+        primitive_type: ""
+      primitives:
+        - id: 3500
+          primitive_type: lane
+        - id: 3501
+          primitive_type: lane
+        - id: 3502
+          primitive_type: lane
+  uuid:
+    uuid:
+      - 6
+      - 70
+      - 102
+      - 94
+      - 252
+      - 4
+      - 42
+      - 203
+      - 16
+      - 204
+      - 158
+      - 233
+      - 14
+      - 119
+      - 47
+      - 217
+  allow_modification: false
+operation_mode:
+  stamp:
+    sec: 37
+    nanosec: 230399363
+  mode: 2
+  is_autoware_control_enabled: true
+  is_in_transition: false
+  is_stop_mode_available: true
+  is_autonomous_mode_available: true
+  is_local_mode_available: true
+  is_remote_mode_available: true
+self_acceleration:
+  header:
+    stamp:
+      sec: 50
+      nanosec: 365979114
+    frame_id: /base_link
+  accel:
+    accel:
+      linear:
+        x: 0.528559
+        y: -0.0304000
+        z: 0.00000
+      angular:
+        x: 0.00000
+        y: 0.00000
+        z: 0.00000
+    covariance:
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00100000
+self_odometry:
+  header:
+    stamp:
+      sec: 50
+      nanosec: 365979114
+    frame_id: map
+  child_frame_id: base_link
+  pose:
+    pose:
+      position:
+        x: 364.031
+        y: 495.469
+        z: 100.000
+      orientation:
+        x: 0.00000
+        y: 0.00000
+        z: 0.707849
+        w: 0.706364
+    covariance:
+      - 0.000100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.000100000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+  twist:
+    twist:
+      linear:
+        x: 10.4530
+        y: 0.00000
+        z: 0.00000
+      angular:
+        x: 0.00000
+        y: 0.00000
+        z: -0.00290825
+    covariance:
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000
+      - 0.00000