From 46ff2d66e39f5602b798daa2a2e4148fb61ae291 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Wed, 30 Aug 2023 13:47:48 +0900 Subject: [PATCH] fix(crosswalk_traffic_light_estimator): move crosswalk after fusion (#4734) * fix: move crosswalk after fusion Signed-off-by: tzhong518 * Update launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> * Rename TrafficLight to TrafficSignal Signed-off-by: Shunsuke Miura * change input to be considered as the regulatory-element Signed-off-by: Shunsuke Miura --------- Signed-off-by: tzhong518 Signed-off-by: Shunsuke Miura Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shunsuke Miura --- .../traffic_light.launch.xml | 23 +++++- .../traffic_light_node_container.launch.py | 43 ----------- .../node.hpp | 10 ++- .../package.xml | 1 + .../src/node.cpp | 72 +++++++++++-------- 5 files changed, 72 insertions(+), 77 deletions(-) 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