From 73acc160934d911c238dcd0d7d6ab7fa9739c079 Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sat, 13 Jan 2024 16:16:00 +0100 Subject: [PATCH 1/5] feat: timeout traffic light status after 3s if no new info available --- code/perception/src/traffic_light_node.py | 22 +++++++++++++++++++++- code/perception/src/vision_node.py | 3 +++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py index 6f67b5b1..b55b0508 100755 --- a/code/perception/src/traffic_light_node.py +++ b/code/perception/src/traffic_light_node.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 +from datetime import datetime +import threading +from time import sleep from ros_compatibility.node import CompatibleNode import ros_compatibility as roscomp from rospy.numpy_msg import numpy_msg @@ -18,6 +21,8 @@ def __init__(self, name, **kwargs): self.role_name = self.get_param("role_name", "hero") self.side = self.get_param("side", "Center") self.classifier = TrafficLightInference(self.get_param("model", "")) + self.last_info: datetime = None + threading.Thread(target=self.auto_invalidate_state).start() # publish / subscribe setup self.setup_camera_subscriptions() @@ -38,15 +43,30 @@ def setup_traffic_light_publishers(self): qos_profile=1 ) + def auto_invalidate_state(self): + while True: + sleep(1) + + if self.last_info is None: + continue + + if (datetime.now() - self.last_info).total_seconds() >= 3: + msg = TrafficLightState() + msg.state = 0 + self.traffic_light_publisher.publish(msg) + self.last_info = None + def handle_camera_image(self, image): result = self.classifier(self.bridge.imgmsg_to_cv2(image)) # 1: Green, 2: Red, 4: Yellow, 0: Unknown msg = TrafficLightState() msg.state = result if result in [1, 2, 4] else 0 - self.traffic_light_publisher.publish(msg) + # invalidates state (state=0) after 3s in auto_invalidate_state() + self.last_info = datetime.now() + def run(self): self.spin() diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index e9681a4c..2366fcbf 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -198,6 +198,9 @@ def process_traffic_lights(self, prediction, cv_image, image_header): for index in indices: box = prediction.boxes.cpu().data.numpy()[index] + if (box[2] - box[0]) * 2 > box[3] - box[1]: + continue # ignore horizontal boxes + if box[0] < min_x or box[2] > max_x or box[4] < min_prob: continue From fd4c32c969932ff562e8316edba365a195a6aaf4 Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sat, 13 Jan 2024 17:56:22 +0100 Subject: [PATCH 2/5] Support more intersection types. More robust detection. --- .../traffic_light_inference.py | 2 +- code/perception/src/traffic_light_node.py | 30 ++++++++++++------- code/perception/src/vision_node.py | 10 ++++--- 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py index ada59c5f..cb3d9e5c 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py @@ -64,7 +64,7 @@ def __call__(self, img): else: out = self.model(img) _, prediction = torch.max(out.data, 1) - return prediction.item() + return (prediction.item(), out.data.cpu().numpy()) # main function for testing purposes diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py index b55b0508..3be9199c 100755 --- a/code/perception/src/traffic_light_node.py +++ b/code/perception/src/traffic_light_node.py @@ -21,7 +21,8 @@ def __init__(self, name, **kwargs): self.role_name = self.get_param("role_name", "hero") self.side = self.get_param("side", "Center") self.classifier = TrafficLightInference(self.get_param("model", "")) - self.last_info: datetime = None + self.last_info_time: datetime = None + self.last_state = None threading.Thread(target=self.auto_invalidate_state).start() # publish / subscribe setup @@ -47,25 +48,34 @@ def auto_invalidate_state(self): while True: sleep(1) - if self.last_info is None: + if self.last_info_time is None: continue - if (datetime.now() - self.last_info).total_seconds() >= 3: + if (datetime.now() - self.last_info_time).total_seconds() >= 2: msg = TrafficLightState() msg.state = 0 self.traffic_light_publisher.publish(msg) - self.last_info = None + self.last_info_time = None def handle_camera_image(self, image): - result = self.classifier(self.bridge.imgmsg_to_cv2(image)) + result, data = self.classifier(self.bridge.imgmsg_to_cv2(image)) - # 1: Green, 2: Red, 4: Yellow, 0: Unknown - msg = TrafficLightState() - msg.state = result if result in [1, 2, 4] else 0 - self.traffic_light_publisher.publish(msg) + if data[0][0] > 1e-15 and data[0][3] > 1e-15 or \ + data[0][0] > 1e-10 or data[0][3] > 1e-10: + return # too uncertain, may not be a traffic light + + state = result if result in [1, 2, 4] else 0 + if self.last_state == state: + # 1: Green, 2: Red, 4: Yellow, 0: Unknown + msg = TrafficLightState() + msg.state = state + self.traffic_light_publisher.publish(msg) + else: + self.last_state = state # invalidates state (state=0) after 3s in auto_invalidate_state() - self.last_info = datetime.now() + if state != 0: + self.last_info_time = datetime.now() def run(self): self.spin() diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 2366fcbf..21a42157 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -191,17 +191,19 @@ def process_traffic_lights(self, prediction, cv_image, image_header): indices = (prediction.boxes.cls == 9).nonzero().squeeze().cpu().numpy() indices = np.asarray([indices]) if indices.size == 1 else indices - min_x = 550 - max_x = 700 - min_prob = 0.35 + max_y = 360 # middle of image + min_prob = 0.30 for index in indices: box = prediction.boxes.cpu().data.numpy()[index] + if box[4] < min_prob: + continue + if (box[2] - box[0]) * 2 > box[3] - box[1]: continue # ignore horizontal boxes - if box[0] < min_x or box[2] > max_x or box[4] < min_prob: + if box[1] > max_y: continue box = box[0:4].astype(int) From 9922dd3973fe99ed65bf492f4c2cefa472bee320 Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sat, 13 Jan 2024 18:42:50 +0100 Subject: [PATCH 3/5] Update docs about filtering --- .../13_traffic_light_detection.md | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/doc/06_perception/13_traffic_light_detection.md b/doc/06_perception/13_traffic_light_detection.md index c627e3cf..de3eb66c 100644 --- a/doc/06_perception/13_traffic_light_detection.md +++ b/doc/06_perception/13_traffic_light_detection.md @@ -35,3 +35,22 @@ This method sets up a publisher for the traffic light state. It publishes to the This method is called whenever a new image message is received. It performs traffic light detection by using `traffic_light_inference.py` on the image and publishes the result. The result is a `TrafficLightState` message where the state is set to the detected traffic light state (1 for green, 2 for red, 4 for yellow, 0 for unknown). + +## Filtering of images + +### Vision Node + +Objects, which are detected as traffic light by the RTDETR-X model (or others), must fulfill the following criterias to be published: + +- At least a 30% (0.30) certainty/probablity of the classification model +- More than twice as tall (height) as it is wide (width) +- Above 360px (upper half of the 1280x720 image) + +### Traffic Light Node + +Objects, which are published by the Vision Node, are further filtered by the following criterias: + +- Classification probabilities of "Unknown" and "Side" are either both below 1e-10 or one of both are below 1e-15 +- "Side" is treated as "Unknown" +- Valid states (Red, Green, Yellow) must be present at least twice in a row to be actually published +- A state decays (state=0; "Unknown") after 2 seconds if there is no new info in the meantime From e91144287c324bc9670d3a171d83133d1a72e3d7 Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sat, 13 Jan 2024 18:47:06 +0100 Subject: [PATCH 4/5] Update code comment --- code/perception/src/traffic_light_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py index 3be9199c..00034bab 100755 --- a/code/perception/src/traffic_light_node.py +++ b/code/perception/src/traffic_light_node.py @@ -73,7 +73,7 @@ def handle_camera_image(self, image): else: self.last_state = state - # invalidates state (state=0) after 3s in auto_invalidate_state() + # Automatically invalidates state (state=0) in auto_invalidate_state() if state != 0: self.last_info_time = datetime.now() From a3288dbf920d97e264e26c987ac5985d7b9cbd56 Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sun, 14 Jan 2024 12:02:25 +0100 Subject: [PATCH 5/5] Improve detection range --- code/perception/src/vision_node.py | 2 +- doc/06_perception/13_traffic_light_detection.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 21a42157..938ee669 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -200,7 +200,7 @@ def process_traffic_lights(self, prediction, cv_image, image_header): if box[4] < min_prob: continue - if (box[2] - box[0]) * 2 > box[3] - box[1]: + if (box[2] - box[0]) * 1.5 > box[3] - box[1]: continue # ignore horizontal boxes if box[1] > max_y: diff --git a/doc/06_perception/13_traffic_light_detection.md b/doc/06_perception/13_traffic_light_detection.md index de3eb66c..68a061c6 100644 --- a/doc/06_perception/13_traffic_light_detection.md +++ b/doc/06_perception/13_traffic_light_detection.md @@ -43,7 +43,7 @@ The result is a `TrafficLightState` message where the state is set to the detect Objects, which are detected as traffic light by the RTDETR-X model (or others), must fulfill the following criterias to be published: - At least a 30% (0.30) certainty/probablity of the classification model -- More than twice as tall (height) as it is wide (width) +- More than 1.5x as tall (height) as it is wide (width) - Above 360px (upper half of the 1280x720 image) ### Traffic Light Node