diff --git a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt
new file mode 100644
index 0000000000000..11a6b232caf90
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(crosswalk_traffic_light_estimator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+###########
+## Build ##
+###########
+
+include_directories()
+
+ament_auto_add_library(crosswalk_traffic_light_estimator SHARED
+ src/node.cpp
+)
+
+rclcpp_components_register_node(crosswalk_traffic_light_estimator
+ PLUGIN "traffic_light::CrosswalkTrafficLightEstimatorNode"
+ EXECUTABLE crosswalk_traffic_light_estimator_node
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+)
diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/crosswalk_traffic_light_estimator/README.md
new file mode 100644
index 0000000000000..3fda780726036
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/README.md
@@ -0,0 +1,83 @@
+# crosswalk_traffic_light_estimator
+
+## Purpose
+
+`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals.
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type | Description |
+| ------------------------------------ | -------------------------------------------------------- | ------------------ |
+| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
+| `~/input/route` | `autoware_auto_planning_msgs::msg::HADMapRoute` | route |
+| `~/input/classified/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | classified signals |
+
+### Output
+
+| Name | Type | Description |
+| -------------------------- | -------------------------------------------------------- | --------------------------------------------------------- |
+| `~/output/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals |
+
+## Parameters
+
+| Name | Type | Description | Default value |
+| :---------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN but also when detection results change GREEN to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN, this module estimates pedestrian's traffic signal as RED.) | `true` |
+
+## Inner-workings / Algorithms
+
+```plantuml
+
+start
+:subscribe detected traffic signals & HDMap;
+:extract crosswalk lanelets from HDMap;
+:extract road lanelets that conflicts crosswalk;
+:initialize green_lanelets(lanelet::ConstLanelets);
+if (Latest detection result is **GREEN**?) then (yes)
+ :push back green_lanelets;
+else (no)
+ if (use_last_detect_color is **true**?) then (yes)
+ if (Latest detection result is **UNKNOWN** and last detection result is **GREEN**?) then (yes)
+ :push back green_lanelets;
+ endif
+ endif
+endif
+if (Is there **STRAIGHT-GREEN** road lanelet in green_lanelets?) then (yes)
+ :estimate related pedestrian's traffic signal as **RED**;
+else if (Is there both **LEFT-GREEN** and **RIGHT-GREEN** road lanelet in green_lanelets?) then (yes)
+ :estimate related pedestrian's traffic signal as **RED**;
+else (no)
+ :estimate related pedestrian's traffic signal as **UNKNOWN**;
+endif
+end
+
+```
+
+If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied.
+
+### Situation1
+
+- crosswalk conflicts **STRAIGHT** lanelet
+- the lanelet refers **GREEN** traffic signal
+
+
+
+
+
+
+
+
+### Situation2
+
+- crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT)
+- the lanelets refer **GREEN** traffic signal
+
+
+
+
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts
diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection1.svg b/perception/crosswalk_traffic_light_estimator/images/intersection1.svg
new file mode 100644
index 0000000000000..7138732472195
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/images/intersection1.svg
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection2.svg b/perception/crosswalk_traffic_light_estimator/images/intersection2.svg
new file mode 100644
index 0000000000000..be91014c95a1b
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/images/intersection2.svg
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg b/perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg
new file mode 100644
index 0000000000000..a459264ddefe6
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
new file mode 100644
index 0000000000000..64e63d2704167
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
@@ -0,0 +1,107 @@
+// Copyright 2022 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.
+
+#ifndef CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
+#define CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+namespace traffic_light
+{
+
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::TrafficLight;
+using autoware_auto_perception_msgs::msg::TrafficSignal;
+using autoware_auto_perception_msgs::msg::TrafficSignalArray;
+using autoware_auto_planning_msgs::msg::HADMapRoute;
+using tier4_autoware_utils::DebugPublisher;
+using tier4_autoware_utils::StopWatch;
+using tier4_debug_msgs::msg::Float64Stamped;
+
+class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
+{
+public:
+ explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options);
+
+private:
+ rclcpp::Subscription::SharedPtr sub_map_;
+ rclcpp::Subscription::SharedPtr sub_route_;
+ rclcpp::Subscription::SharedPtr sub_traffic_light_array_;
+ rclcpp::Publisher::SharedPtr pub_traffic_light_array_;
+
+ lanelet::LaneletMapPtr lanelet_map_ptr_;
+ lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
+ lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
+ std::shared_ptr overall_graphs_ptr_;
+
+ lanelet::ConstLanelets conflicting_crosswalks_;
+
+ void onMap(const HADMapBin::ConstSharedPtr msg);
+ void onRoute(const HADMapRoute::ConstSharedPtr msg);
+ void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);
+
+ void updateLastDetectedSignal(const lanelet::Id & id, const uint8_t color);
+ void setCrosswalkTrafficSignal(
+ const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const;
+
+ lanelet::ConstLanelets getGreenLanelets(
+ const lanelet::ConstLanelets & lanelets,
+ const std::unordered_map & traffic_light_id_map);
+
+ uint8_t estimateCrosswalkTrafficSignal(
+ const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & green_lanelets) const;
+
+ uint8_t getHighestConfidenceTrafficSignal(
+ const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
+ const std::unordered_map & traffic_light_id_map) const;
+
+ uint8_t getLastDetectedTrafficSignal(const lanelet::Id & id) const;
+
+ // Node param
+ bool use_last_detect_color_;
+
+ // Signal history
+ std::unordered_map last_detect_color_;
+
+ // Stop watch
+ StopWatch stop_watch_;
+
+ // Debug
+ std::shared_ptr pub_processing_time_;
+};
+
+} // namespace traffic_light
+
+#endif // CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml
new file mode 100644
index 0000000000000..91bec60c90aee
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml
new file mode 100644
index 0000000000000..3f1bdb6880bd1
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/package.xml
@@ -0,0 +1,28 @@
+
+
+ crosswalk_traffic_light_estimator
+ 0.1.0
+ The crosswalk_traffic_light_estimator package
+
+ Satoshi Ota
+ Apache License 2.0
+
+ ament_cmake_auto
+
+ autoware_cmake
+
+ autoware_auto_mapping_msgs
+ autoware_auto_perception_msgs
+ autoware_auto_planning_msgs
+ lanelet2_extension
+ rclcpp
+ rclcpp_components
+ tier4_autoware_utils
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp
new file mode 100644
index 0000000000000..0eec80143062e
--- /dev/null
+++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp
@@ -0,0 +1,336 @@
+// Copyright 2022 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 "crosswalk_traffic_light_estimator/node.hpp"
+
+#include
+
+#include
+#include
+#include
+#include
+
+namespace traffic_light
+{
+namespace
+{
+
+bool hasMergeLane(
+ const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2,
+ const lanelet::routing::RoutingGraphPtr & routing_graph_ptr)
+{
+ const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1);
+ const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2);
+
+ for (const auto & next_lanelet_1 : next_lanelets_1) {
+ for (const auto & next_lanelet_2 : next_lanelets_2) {
+ if (next_lanelet_1.id() == next_lanelet_2.id()) {
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool hasMergeLane(
+ const lanelet::ConstLanelets & lanelets,
+ const lanelet::routing::RoutingGraphPtr & routing_graph_ptr)
+{
+ for (size_t i = 0; i < lanelets.size(); ++i) {
+ for (size_t j = i + 1; j < lanelets.size(); ++j) {
+ const auto lanelet_1 = lanelets.at(i);
+ const auto lanelet_2 = lanelets.at(j);
+
+ if (lanelet_1.id() == lanelet_2.id()) {
+ continue;
+ }
+
+ const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none");
+ const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none");
+ if (turn_direction_1 == turn_direction_2) {
+ continue;
+ }
+
+ if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) {
+ continue;
+ }
+
+ return true;
+ }
+ }
+
+ return false;
+}
+
+} // namespace
+
+CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
+ const rclcpp::NodeOptions & options)
+: Node("crosswalk_traffic_light_estimator", options)
+{
+ using std::placeholders::_1;
+
+ use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true);
+
+ sub_map_ = create_subscription(
+ "~/input/vector_map", rclcpp::QoS{1}.transient_local(),
+ std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1));
+ sub_route_ = create_subscription(
+ "~/input/route", rclcpp::QoS{1}.transient_local(),
+ std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1));
+ sub_traffic_light_array_ = create_subscription(
+ "~/input/classified/traffic_signals", rclcpp::QoS{1},
+ std::bind(&CrosswalkTrafficLightEstimatorNode::onTrafficLightArray, this, _1));
+
+ pub_traffic_light_array_ =
+ this->create_publisher("~/output/traffic_signals", rclcpp::QoS{1});
+ pub_processing_time_ = std::make_shared(this, "~/debug");
+}
+
+void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg)
+{
+ RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet");
+ lanelet_map_ptr_ = std::make_shared();
+ lanelet::utils::conversion::fromBinMsg(
+ *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
+ const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+ lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+ const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+ lanelet::Locations::Germany, lanelet::Participants::Pedestrian);
+ lanelet::routing::RoutingGraphConstPtr vehicle_graph =
+ lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules);
+ lanelet::routing::RoutingGraphConstPtr pedestrian_graph =
+ lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules);
+ lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
+ overall_graphs_ptr_ =
+ std::make_shared(overall_graphs);
+ RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded");
+}
+
+void CrosswalkTrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
+{
+ if (lanelet_map_ptr_ == nullptr) {
+ RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");
+ return;
+ }
+
+ lanelet::ConstLanelets route_lanelets;
+ for (const auto & segment : msg->segments) {
+ for (const auto & primitive : segment.primitives) {
+ try {
+ route_lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(primitive.id));
+ } catch (const lanelet::NoSuchPrimitiveError & ex) {
+ RCLCPP_ERROR(get_logger(), "%s", ex.what());
+ return;
+ }
+ }
+ }
+
+ conflicting_crosswalks_.clear();
+
+ for (const auto & route_lanelet : route_lanelets) {
+ constexpr int PEDESTRIAN_GRAPH_ID = 1;
+ const auto conflict_lls =
+ overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID);
+ for (const auto & lanelet : conflict_lls) {
+ conflicting_crosswalks_.push_back(lanelet);
+ }
+ }
+}
+
+void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
+ const TrafficSignalArray::ConstSharedPtr msg)
+{
+ StopWatch stop_watch;
+ stop_watch.tic("Total");
+
+ TrafficSignalArray output = *msg;
+
+ std::unordered_map traffic_light_id_map;
+ for (const auto & traffic_signal : msg->signals) {
+ traffic_light_id_map[traffic_signal.map_primitive_id] = traffic_signal;
+ }
+
+ for (const auto & crosswalk : conflicting_crosswalks_) {
+ constexpr int VEHICLE_GRAPH_ID = 0;
+ const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID);
+ const auto green_lanelets = getGreenLanelets(conflict_lls, traffic_light_id_map);
+
+ const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, green_lanelets);
+ setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output);
+ }
+
+ pub_traffic_light_array_->publish(output);
+ pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total"));
+
+ return;
+}
+
+void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
+ const lanelet::Id & id, const uint8_t color)
+{
+ if (color == TrafficLight::UNKNOWN) {
+ return;
+ }
+
+ if (last_detect_color_.count(id) == 0) {
+ last_detect_color_.insert(std::make_pair(id, color));
+ }
+
+ last_detect_color_.at(id) = color;
+}
+
+void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
+ const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const
+{
+ const auto tl_reg_elems = crosswalk.regulatoryElementsAs();
+
+ for (const auto & tl_reg_elem : tl_reg_elems) {
+ const auto crosswalk_traffic_lights = tl_reg_elem->trafficLights();
+
+ for (const auto & traffic_light : crosswalk_traffic_lights) {
+ const auto ll_traffic_light = static_cast(traffic_light);
+
+ TrafficSignal output_traffic_signal;
+ TrafficLight output_traffic_light;
+ output_traffic_light.color = color;
+ output_traffic_light.confidence = 1.0;
+ output_traffic_signal.lights.push_back(output_traffic_light);
+ output_traffic_signal.map_primitive_id = ll_traffic_light.id();
+ msg.signals.push_back(output_traffic_signal);
+ }
+ }
+}
+
+lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getGreenLanelets(
+ const lanelet::ConstLanelets & lanelets,
+ const std::unordered_map & traffic_light_id_map)
+{
+ lanelet::ConstLanelets green_lanelets{};
+
+ for (const auto & lanelet : lanelets) {
+ const auto tl_reg_elems = lanelet.regulatoryElementsAs();
+
+ if (tl_reg_elems.empty()) {
+ continue;
+ }
+
+ const auto tl_reg_elem = tl_reg_elems.front();
+ const auto traffic_lights_for_vehicle = tl_reg_elem->trafficLights();
+
+ const auto current_detected_signal =
+ getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map);
+ const auto is_green = current_detected_signal == TrafficLight::GREEN;
+
+ const auto last_detected_signal = getLastDetectedTrafficSignal(tl_reg_elem->id());
+ const auto was_green = current_detected_signal == TrafficLight::UNKNOWN &&
+ last_detected_signal == TrafficLight::GREEN && use_last_detect_color_;
+
+ updateLastDetectedSignal(tl_reg_elem->id(), current_detected_signal);
+
+ if (!is_green && !was_green) {
+ continue;
+ }
+
+ green_lanelets.push_back(lanelet);
+ }
+
+ return green_lanelets;
+}
+
+uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
+ const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & green_lanelets) const
+{
+ bool has_left_green_lane = false;
+ bool has_right_green_lane = false;
+ bool has_straight_green_lane = false;
+ bool has_related_green_tl = false;
+
+ const std::string related_tl_id = crosswalk.attributeOr("related_traffic_light", "none");
+
+ for (const auto & lanelet : green_lanelets) {
+ const std::string turn_direction = lanelet.attributeOr("turn_direction", "none");
+
+ if (turn_direction == "left") {
+ has_left_green_lane = true;
+ } else if (turn_direction == "right") {
+ has_right_green_lane = true;
+ } else {
+ has_straight_green_lane = true;
+ }
+
+ const auto tl_reg_elems = lanelet.regulatoryElementsAs();
+ if (tl_reg_elems.front()->id() == std::atoi(related_tl_id.c_str())) {
+ has_related_green_tl = true;
+ }
+ }
+
+ if (has_straight_green_lane || has_related_green_tl) {
+ return TrafficLight::RED;
+ }
+
+ const auto has_merge_lane = hasMergeLane(green_lanelets, routing_graph_ptr_);
+ return !has_merge_lane && has_left_green_lane && has_right_green_lane ? TrafficLight::RED
+ : TrafficLight::UNKNOWN;
+}
+
+uint8_t CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
+ const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
+ const std::unordered_map & traffic_light_id_map) const
+{
+ uint8_t ret = TrafficLight::UNKNOWN;
+
+ double highest_confidence = 0.0;
+ for (const auto & traffic_light : traffic_lights) {
+ if (!traffic_light.isLineString()) {
+ continue;
+ }
+
+ const int id = static_cast(traffic_light).id();
+ if (traffic_light_id_map.count(id) == 0) {
+ continue;
+ }
+
+ const auto & lights = traffic_light_id_map.at(id).lights;
+ if (lights.empty()) {
+ continue;
+ }
+
+ const auto & color = lights.front().color;
+ const auto & confidence = lights.front().confidence;
+ if (confidence < highest_confidence) {
+ continue;
+ }
+
+ highest_confidence = confidence;
+ ret = color;
+ }
+
+ return ret;
+}
+
+uint8_t CrosswalkTrafficLightEstimatorNode::getLastDetectedTrafficSignal(
+ const lanelet::Id & id) const
+{
+ if (last_detect_color_.count(id) == 0) {
+ return TrafficLight::UNKNOWN;
+ }
+
+ return last_detect_color_.at(id);
+}
+} // namespace traffic_light
+
+#include
+
+RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::CrosswalkTrafficLightEstimatorNode)
+
+
+