diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
index 38501cc059c1e..288c8270aa874 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
@@ -9,6 +9,7 @@
+
@@ -62,7 +63,7 @@
-
+
@@ -112,7 +113,7 @@
-
+
@@ -124,9 +125,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
index 8b3d15f2cff95..a6bcb40e81252 100644
--- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
+++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
@@ -73,7 +73,6 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]")
add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]")
- add_launch_arg("use_crosswalk_traffic_light_estimator", "True")
add_launch_arg("use_intra_process", "False")
add_launch_arg("use_multithread", "False")
@@ -137,46 +136,6 @@ def create_parameter_dict(*args):
output="both",
)
- estimator_loader = LoadComposableNodes(
- composable_node_descriptions=[
- ComposableNode(
- package="crosswalk_traffic_light_estimator",
- plugin="traffic_light::CrosswalkTrafficLightEstimatorNode",
- name="crosswalk_traffic_light_estimator",
- namespace="classification",
- remappings=[
- ("~/input/vector_map", "/map/vector_map"),
- ("~/input/route", "/planning/mission_planning/route"),
- ("~/input/classified/traffic_signals", "classified/traffic_signals"),
- ("~/output/traffic_signals", "estimated/traffic_signals"),
- ],
- extra_arguments=[{"use_intra_process_comms": False}],
- ),
- ],
- target_container=container,
- condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")),
- )
-
- relay_loader = LoadComposableNodes(
- composable_node_descriptions=[
- ComposableNode(
- package="topic_tools",
- plugin="topic_tools::RelayNode",
- name="classified_signals_relay",
- namespace="classification",
- parameters=[
- {"input_topic": "classified/traffic_signals"},
- {"output_topic": "estimated/traffic_signals"},
- ],
- extra_arguments=[
- {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
- ],
- )
- ],
- target_container=container,
- condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")),
- )
-
decompressor_loader = LoadComposableNodes(
composable_node_descriptions=[
ComposableNode(
@@ -251,7 +210,5 @@ def create_parameter_dict(*args):
container,
decompressor_loader,
fine_detector_loader,
- estimator_loader,
- relay_loader,
]
)
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
index 0850436851e6a..3b4855f1f38e9 100644
--- 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
@@ -22,6 +22,7 @@
#include
#include
+#include
#include
#include
#include
@@ -47,9 +48,9 @@ using autoware_planning_msgs::msg::LaneletRoute;
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
using tier4_debug_msgs::msg::Float64Stamped;
-using tier4_perception_msgs::msg::TrafficLightElement;
-using tier4_perception_msgs::msg::TrafficSignal;
-using tier4_perception_msgs::msg::TrafficSignalArray;
+using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
+using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
+using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
using TrafficSignalAndTime = std::pair;
using TrafficLightIdMap = std::unordered_map;
@@ -89,6 +90,9 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
const TrafficLightIdMap & traffic_light_id_map) const;
+ boost::optional getHighestConfidenceTrafficSignal(
+ const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const;
+
// Node param
bool use_last_detect_color_;
double last_detect_color_hold_time_;
diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml
index 435ecf6e6fa3d..f029972f7124f 100644
--- a/perception/crosswalk_traffic_light_estimator/package.xml
+++ b/perception/crosswalk_traffic_light_estimator/package.xml
@@ -12,6 +12,7 @@
autoware_auto_mapping_msgs
autoware_auto_planning_msgs
+ autoware_perception_msgs
autoware_planning_msgs
lanelet2_extension
rclcpp
diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp
index 55d272cb71cfe..dd7a9ea04ae38 100644
--- a/perception/crosswalk_traffic_light_estimator/src/node.cpp
+++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp
@@ -160,7 +160,7 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
TrafficLightIdMap traffic_light_id_map;
for (const auto & traffic_signal : msg->signals) {
- traffic_light_id_map[traffic_signal.traffic_light_id] =
+ traffic_light_id_map[traffic_signal.traffic_signal_id] =
std::pair(traffic_signal, get_clock()->now());
}
@@ -191,11 +191,11 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
continue;
}
- if (elements.front().color == TrafficLightElement::UNKNOWN) {
+ if (elements.front().color == TrafficSignalElement::UNKNOWN) {
continue;
}
- const auto & id = input_traffic_signal.second.first.traffic_light_id;
+ const auto & id = input_traffic_signal.second.first.traffic_signal_id;
if (last_detect_color_.count(id) == 0) {
last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second));
@@ -207,7 +207,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
std::vector erase_id_list;
for (auto & last_traffic_signal : last_detect_color_) {
- const auto & id = last_traffic_signal.second.first.traffic_light_id;
+ const auto & id = last_traffic_signal.second.first.traffic_signal_id;
if (traffic_light_id_map.count(id) == 0) {
// hold signal recognition results for [last_detect_color_hold_time_] seconds.
@@ -227,19 +227,13 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
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;
- TrafficLightElement output_traffic_light;
- output_traffic_light.color = color;
- output_traffic_light.confidence = 1.0;
- output_traffic_signal.elements.push_back(output_traffic_light);
- output_traffic_signal.traffic_light_id = ll_traffic_light.id();
- msg.signals.push_back(output_traffic_signal);
- }
+ TrafficSignal output_traffic_signal;
+ TrafficSignalElement output_traffic_signal_element;
+ output_traffic_signal_element.color = color;
+ output_traffic_signal_element.confidence = 1.0;
+ output_traffic_signal.elements.push_back(output_traffic_signal_element);
+ output_traffic_signal.traffic_signal_id = tl_reg_elem->id();
+ msg.signals.push_back(output_traffic_signal);
}
}
@@ -256,34 +250,32 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets(
}
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);
+ getHighestConfidenceTrafficSignal(tl_reg_elem->id(), traffic_light_id_map);
if (!current_detected_signal && !use_last_detect_color_) {
continue;
}
const auto current_is_not_red =
- current_detected_signal ? current_detected_signal.get() == TrafficLightElement::GREEN ||
- current_detected_signal.get() == TrafficLightElement::AMBER
+ current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::GREEN ||
+ current_detected_signal.get() == TrafficSignalElement::AMBER
: true;
const auto current_is_unknown_or_none =
- current_detected_signal ? current_detected_signal.get() == TrafficLightElement::UNKNOWN
+ current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN
: true;
const auto last_detected_signal =
- getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_);
+ getHighestConfidenceTrafficSignal(tl_reg_elem->id(), last_detect_color_);
if (!last_detected_signal) {
continue;
}
const auto was_not_red = current_is_unknown_or_none &&
- (last_detected_signal.get() == TrafficLightElement::GREEN ||
- last_detected_signal.get() == TrafficLightElement::AMBER) &&
+ (last_detected_signal.get() == TrafficSignalElement::GREEN ||
+ last_detected_signal.get() == TrafficSignalElement::AMBER) &&
use_last_detect_color_;
if (!current_is_not_red && !was_not_red) {
@@ -324,13 +316,13 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
}
if (has_straight_non_red_lane || has_related_non_red_tl) {
- return TrafficLightElement::RED;
+ return TrafficSignalElement::RED;
}
const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_);
return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane
- ? TrafficLightElement::RED
- : TrafficLightElement::UNKNOWN;
+ ? TrafficSignalElement::RED
+ : TrafficSignalElement::UNKNOWN;
}
boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
@@ -367,6 +359,28 @@ boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenc
return ret;
}
+
+boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
+ const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const
+{
+ boost::optional ret{boost::none};
+
+ double highest_confidence = 0.0;
+ if (traffic_light_id_map.count(id) == 0) {
+ return ret;
+ }
+
+ for (const auto & element : traffic_light_id_map.at(id).first.elements) {
+ if (element.confidence < highest_confidence) {
+ continue;
+ }
+
+ highest_confidence = element.confidence;
+ ret = element.color;
+ }
+
+ return ret;
+}
} // namespace traffic_light
#include