From 0ff0d4f676c3a20c82b56a26573f5c79cfb1f3f1 Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Mon, 22 Jun 2026 14:26:07 -0400 Subject: [PATCH 01/11] Create pass_on_left_ego_only.py --- pass_on_left_ego_only.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 pass_on_left_ego_only.py diff --git a/pass_on_left_ego_only.py b/pass_on_left_ego_only.py new file mode 100644 index 0000000..9217215 --- /dev/null +++ b/pass_on_left_ego_only.py @@ -0,0 +1,28 @@ +import SG_Primitives as P +from SymbolicEntity import SymbolicEntity +from SymbolicProperty import SymbolicProperty +from functools import partial + +EGO = partial(P.filterByAttr, "G", "name", "ego") +HUMAN = SymbolicEntity('person_1', ['person']) + +def person_in_direction(robot, person, direction_label): + """ + Checks if the person is located in a specific direction relative to the robot. + """ + direction_set = partial(P.relSet, robot, direction_label) + overlapping_entities = partial(P.intersection, direction_set, person) + return partial(P.gt, partial(P.size, overlapping_entities), 0) + + +passing_human_on_left = SymbolicProperty( + "robot_must_pass_human_on_the_left", + "((is_front & !is_behind) -> (!is_left U is_behind))", + [ + ("is_front", person_in_direction(EGO, HUMAN, "FRONT")), + ("is_left", person_in_direction(EGO, HUMAN, "LEFT")), + ("is_behind", person_in_direction(EGO, HUMAN, "BACK")) + ], + [HUMAN] +) +all_human_properties = [passing_human_on_left] From 55ddfaf2c88fe07b9cdb2eb8e4341b53eff803a1 Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Mon, 22 Jun 2026 15:27:16 -0400 Subject: [PATCH 02/11] Create ros2_async_tracker.py --- ros2_async_tracker.py | 243 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 243 insertions(+) create mode 100644 ros2_async_tracker.py diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py new file mode 100644 index 0000000..b435ede --- /dev/null +++ b/ros2_async_tracker.py @@ -0,0 +1,243 @@ +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from sensor_msgs.msg import CompressedImage +from geometry_msgs.msg import PoseArray +import torch +from torchvision.models.detection import fasterrcnn_resnet50_fpn, FasterRCNN_ResNet50_FPN_Weights +from PIL import Image as PILImage +import torchvision.transforms as T +import cv2 +import numpy as np +import math +import threading # Needed to handle safe multi-threaded memory access +import networkx as nx + +class SceneNode: + """A custom node object that perfectly matches the SceneFlowLang expectations.""" + def __init__(self, node_id, name, base_class): + self.id = node_id + self.name = name + self.base_class = base_class + + def get_id(self): + return self.id + def is_phantom(self): + # The repo uses this to ignore certain nodes; we want our human to be real! + return False + + def __hash__(self): + # NetworkX requires custom objects to be hashable + return hash(self.id) + + def __eq__(self, other): + return self.id == other.id +class AsyncTrackingNode(Node): + def __init__(self): + super().__init__('async_tracking_node') + + # --- 1. THREAD SAFETY LOCK --- + self.lock = threading.Lock() + + # --- 2. MODEL SETUP --- + self.get_logger().info('Loading PyTorch model...') + weights = FasterRCNN_ResNet50_FPN_Weights.DEFAULT + self.model = fasterrcnn_resnet50_fpn(weights=weights) + self.model.eval() + self.transform = T.Compose([T.ToTensor()]) + self.threshold = 0.80 + self.categories = weights.meta["categories"] + # --- 3. STATE VARIABLES --- + self.graph = SceneGraph() + self.camera_hfov = math.radians(73.0) + self.latest_lidar_msg = None + + self.human_last_x = None + self.human_last_y = None + self.last_seen_time = 0.0 + self.human_angle = 0.0 + + self.max_jump_distance = 0.38 # Increased slightly to account for faster tracking steps + self.memory_timeout = 3.5 # Keeps lock stable through the camera's processing lag + + # --- 4. ROS2 MULTI-THREADING SETUP --- + # Create two separate lane assignments + self.lidar_cb_group = MutuallyExclusiveCallbackGroup() + self.camera_cb_group = MutuallyExclusiveCallbackGroup() + + # Assign the subscriptions to their respective lanes + self.lidar_sub = self.create_subscription( + PoseArray, '/detected_objects', self.lidar_callback, 10, + callback_group=self.lidar_cb_group) + + self.camera_sub = self.create_subscription( + CompressedImage, '/oak/rgb/image_raw/compressed', self.camera_callback, 1, + callback_group=self.camera_cb_group) + + self.get_logger().info('Asynchronous Object Permanence Tracker Operational!') + + def camera_callback(self, msg): + # Heavy computing happens here in Thread B + np_arr = np.frombuffer(msg.data, np.uint8) + img_bgr = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if img_bgr is None: return + + img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) + img_tensor = self.transform(PILImage.fromarray(img_rgb)) + img_w = img_bgr.shape[1] + + with torch.no_grad(): + prediction = self.model([img_tensor]) + + for box, score, label in zip(prediction[0]['boxes'], prediction[0]['scores'], prediction[0]['labels']): + if score > self.threshold: + if self.categories[label.item()] == 'person': + x1, y1, x2, y2 = box.numpy().astype(int) + box_center_x = (x1 + x2) / 2.0 + normalized_x = (box_center_x / img_w) - 0.5 + camera_angle = -normalized_x * self.camera_hfov + self.set_ground_truth_from_camera(camera_angle) + + def set_ground_truth_from_camera(self, camera_angle): + with self.lock: + if self.latest_lidar_msg is None: return + poses = self.latest_lidar_msg.poses + + best_match = None + min_angle_diff = 100.0 + + for pose in poses: + x_robot = -pose.position.x + y_robot = -pose.position.y + lidar_angle = math.atan2(y_robot, x_robot) + + angle_diff = abs(camera_angle - lidar_angle) + if angle_diff < math.radians(20.0) and angle_diff < min_angle_diff: + min_angle_diff = angle_diff + best_match = (x_robot, y_robot) + if best_match is not None: + # Safely lock the variables while updating memory map values + with self.lock: + self.human_last_x, self.human_last_y = best_match + self.last_seen_time = self.get_clock().now().nanoseconds / 1e9 + def lidar_callback(self, msg): + # Fast computing happens here in Thread A + with self.lock: + self.latest_lidar_msg = msg + hx, hy = self.human_last_x, self.human_last_y + last_time = self.last_seen_time + + if hx is None: + return + + current_time = self.get_clock().now().nanoseconds / 1e9 + + if current_time - last_time > self.memory_timeout: + self.get_logger().warn("Track lost! Human out of range. Resetting graph.") + with self.lock: + self.human_last_x = None + self.human_last_y = None + if self.graph.connection is not None: + self.graph.connection = None + self.get_logger().info(f"GRAPH UPDATE: {self.graph.get_graph_string()}") + return + + closest_distance = 999.0 + best_x = None + best_y = None + for pose in msg.poses: + x_robot = -pose.position.x + y_robot = -pose.position.y + + jump_dist = math.hypot(x_robot - hx, y_robot - hy) + if jump_dist < closest_distance: + closest_distance = jump_dist + best_x = x_robot + best_y = y_robot + + if closest_distance <= self.max_jump_distance and best_x is not None: + with self.lock: + self.human_last_x = best_x + self.human_last_y = best_y + self.last_seen_time = current_time + self.human_angle = math.degrees(math.atan2(best_y, best_x)) + self.get_logger().info("Human Angle: "+ str(self.human_angle)) + #self.get_logger().info("Human is at:\nx="+str(best_x)+"\ny="+str(best_y)+"\nDistance: "+str(closest_distan> + if self.graph.update_connection(best_x, best_y): + self.get_logger().info(f"GRAPH UPDATE: {self.graph.get_graph_string()}") + def build_networkx_graph(self, human_x, human_y): + """Constructs a fresh NetworkX DiGraph for the current physical frame.""" + # 1. Initialize an empty Directed Graph + sg = nx.DiGraph() + # 2. Create the Ego (Robot) Node and Human Node + # Notice how the names match the EGO and HUMAN definitions we discussed earlier! + ego_node = SceneNode(node_id=0, name="ego", base_class="vehicle") + human_node = SceneNode(node_id=1, name="person_1", base_class="person") + + # 3. Add the nodes to the graph + sg.add_node(ego_node) + sg.add_node(human_node) + + # 4. Calculate the spatial relationship + angle_rad = math.atan2(human_y, human_x) + angle_deg = math.degrees(angle_rad) + + if -45.0 <= angle_deg <= 45.0: + direction = "FRONT" + elif 45.0 < angle_deg <= 135.0: + direction = "LEFT" + elif -135.0 <= angle_deg < -45.0: + direction = "RIGHT" + else: + direction = "BACK" + + # 5. Add the Directed Edge (Arrow) from the Robot to the Human + # The repository looks for relations in the edge data attributes. + # Check step 4 below regarding the "label" key! # 2. Create the Ego (Robot) Node and Human Node + # Notice how the names match the EGO and HUMAN definitions we discussed earlier! + ego_node = SceneNode(node_id=0, name="ego", base_class="vehicle") + human_node = SceneNode(node_id=1, name="person_1", base_class="person") + + # 3. Add the nodes to the graph + sg.add_node(ego_node) + sg.add_node(human_node) + + # 4. Calculate the spatial relationship + angle_rad = math.atan2(human_y, human_x) + angle_deg = math.degrees(angle_rad) + + if -45.0 <= angle_deg <= 45.0: + direction = "FRONT" + elif 45.0 < angle_deg <= 135.0: + direction = "LEFT" + elif -135.0 <= angle_deg < -45.0: + direction = "RIGHT" + else: + direction = "BACK" + + # 5. Add the Directed Edge (Arrow) from the Robot to the Human + # The repository looks for relations in the edge data attributes. + # Check step 4 below regarding the "label" key! + sg.add_edge(ego_node, human_node, label=direction) + + return sg +def main(args=None): + rclpy.init(args=args) + node = AsyncTrackingNode() + + # NEW FOR PHASE 4.5: Explicitly use a MultiThreadedExecutor instead of the default single thread + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + executor.spin() # Spin handles tracking across available threads asynchronously + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() From 8ee2b942d9f62fb4189cbf5845a5a6a6dd17f720 Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Mon, 22 Jun 2026 15:33:29 -0400 Subject: [PATCH 03/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index b435ede..10835f3 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -164,8 +164,12 @@ def lidar_callback(self, msg): self.human_angle = math.degrees(math.atan2(best_y, best_x)) self.get_logger().info("Human Angle: "+ str(self.human_angle)) #self.get_logger().info("Human is at:\nx="+str(best_x)+"\ny="+str(best_y)+"\nDistance: "+str(closest_distan> - if self.graph.update_connection(best_x, best_y): - self.get_logger().info(f"GRAPH UPDATE: {self.graph.get_graph_string()}") + # Generate the NetworkX graph for this exact microsecond in time + current_sg = self.build_networkx_graph(best_x, best_y) + + # TODO: We will pass `current_sg` to the DFA evaluator engine here! + + def build_networkx_graph(self, human_x, human_y): """Constructs a fresh NetworkX DiGraph for the current physical frame.""" # 1. Initialize an empty Directed Graph From 9f6f5e0b886406d7f61001521a4c67bded4f6dfe Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Mon, 22 Jun 2026 16:05:27 -0400 Subject: [PATCH 04/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 61 ++++++++++++++----------------------------- 1 file changed, 20 insertions(+), 41 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index 10835f3..f654247 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -13,6 +13,7 @@ import math import threading # Needed to handle safe multi-threaded memory access import networkx as nx +from pass_on_left_ego_only.py import all_human_properties class SceneNode: """A custom node object that perfectly matches the SceneFlowLang expectations.""" @@ -20,19 +21,20 @@ def __init__(self, node_id, name, base_class): self.id = node_id self.name = name self.base_class = base_class - + def get_id(self): return self.id + def is_phantom(self): # The repo uses this to ignore certain nodes; we want our human to be real! return False - + def __hash__(self): - # NetworkX requires custom objects to be hashable + # NetworkX requires custom objects to be hashable so it can use them as dictionary keys return hash(self.id) - + def __eq__(self, other): - return self.id == other.id + return getattr(other, 'id', None) == self.id class AsyncTrackingNode(Node): def __init__(self): super().__init__('async_tracking_node') @@ -75,7 +77,10 @@ def __init__(self): CompressedImage, '/oak/rgb/image_raw/compressed', self.camera_callback, 1, callback_group=self.camera_cb_group) + self.active_properties = + self.get_logger().info('Asynchronous Object Permanence Tracker Operational!') + def camera_callback(self, msg): # Heavy computing happens here in Thread B @@ -167,46 +172,22 @@ def lidar_callback(self, msg): # Generate the NetworkX graph for this exact microsecond in time current_sg = self.build_networkx_graph(best_x, best_y) - # TODO: We will pass `current_sg` to the DFA evaluator engine here! - + # TODO: Loop through self.active_properties and call prop.evaluate(current_sg) + self.get_logger().debug("NetworkX Graph successfully built for this frame!") - def build_networkx_graph(self, human_x, human_y): +def build_networkx_graph(self, human_x, human_y): """Constructs a fresh NetworkX DiGraph for the current physical frame.""" # 1. Initialize an empty Directed Graph sg = nx.DiGraph() + # 2. Create the Ego (Robot) Node and Human Node - # Notice how the names match the EGO and HUMAN definitions we discussed earlier! - ego_node = SceneNode(node_id=0, name="ego", base_class="vehicle") - human_node = SceneNode(node_id=1, name="person_1", base_class="person") - - # 3. Add the nodes to the graph - sg.add_node(ego_node) - sg.add_node(human_node) - - # 4. Calculate the spatial relationship - angle_rad = math.atan2(human_y, human_x) - angle_deg = math.degrees(angle_rad) - - if -45.0 <= angle_deg <= 45.0: - direction = "FRONT" - elif 45.0 < angle_deg <= 135.0: - direction = "LEFT" - elif -135.0 <= angle_deg < -45.0: - direction = "RIGHT" - else: - direction = "BACK" - - # 5. Add the Directed Edge (Arrow) from the Robot to the Human - # The repository looks for relations in the edge data attributes. - # Check step 4 below regarding the "label" key! # 2. Create the Ego (Robot) Node and Human Node - # Notice how the names match the EGO and HUMAN definitions we discussed earlier! ego_node = SceneNode(node_id=0, name="ego", base_class="vehicle") human_node = SceneNode(node_id=1, name="person_1", base_class="person") - + # 3. Add the nodes to the graph sg.add_node(ego_node) sg.add_node(human_node) - + # 4. Calculate the spatial relationship angle_rad = math.atan2(human_y, human_x) angle_deg = math.degrees(angle_rad) @@ -219,12 +200,10 @@ def build_networkx_graph(self, human_x, human_y): direction = "RIGHT" else: direction = "BACK" - - # 5. Add the Directed Edge (Arrow) from the Robot to the Human - # The repository looks for relations in the edge data attributes. - # Check step 4 below regarding the "label" key! - sg.add_edge(ego_node, human_node, label=direction) - + + # 5. SAFE FALLBACK: Add the directed edge with multiple attribute keys + sg.add_edge(ego_node, human_node, label=direction, type=direction, relation=direction) + return sg def main(args=None): rclpy.init(args=args) From 24d87d277ad8d78e0f6d72a1ec7e50dd61702464 Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 10:06:55 -0400 Subject: [PATCH 05/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index f654247..1573296 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -77,8 +77,8 @@ def __init__(self): CompressedImage, '/oak/rgb/image_raw/compressed', self.camera_callback, 1, callback_group=self.camera_cb_group) - self.active_properties = - + self.active_properties = all_human_properties + self.frame_counter =0 self.get_logger().info('Asynchronous Object Permanence Tracker Operational!') @@ -124,6 +124,7 @@ def set_ground_truth_from_camera(self, camera_angle): if best_match is not None: # Safely lock the variables while updating memory map values with self.lock: + self.active_trackers = symbolic_prop.make_concrete(current_sg) self.human_last_x, self.human_last_y = best_match self.last_seen_time = self.get_clock().now().nanoseconds / 1e9 def lidar_callback(self, msg): @@ -140,6 +141,7 @@ def lidar_callback(self, msg): if current_time - last_time > self.memory_timeout: self.get_logger().warn("Track lost! Human out of range. Resetting graph.") + self.frame_counter = 0 with self.lock: self.human_last_x = None self.human_last_y = None @@ -148,6 +150,7 @@ def lidar_callback(self, msg): self.get_logger().info(f"GRAPH UPDATE: {self.graph.get_graph_string()}") return + self.frame_counter += 1 closest_distance = 999.0 best_x = None best_y = None @@ -173,6 +176,30 @@ def lidar_callback(self, msg): current_sg = self.build_networkx_graph(best_x, best_y) # TODO: Loop through self.active_properties and call prop.evaluate(current_sg) + ''' +SymbolicProperty( + "robot_must_pass_human_on_the_left", + "((is_front & !is_behind) -> (!is_left U is_behind))", + [ + ("is_front", person_in_direction(EGO, HUMAN, "FRONT")), + ("is_left", person_in_direction(EGO, HUMAN, "LEFT")), + ("is_behind", person_in_direction(EGO, HUMAN, "BACK")) + ], + [HUMAN] +) + ''' + temp = False + try: + temp = self.active_trackers[0] + temp = True + except NameError: + pass + for tracker in self.active_trackers: + tracker.step(current_sg) + if tracker.is_trap(): + self.get_logger().error("Trap State") + else: + self.get_logger().error("Accepting State") self.get_logger().debug("NetworkX Graph successfully built for this frame!") def build_networkx_graph(self, human_x, human_y): @@ -203,7 +230,9 @@ def build_networkx_graph(self, human_x, human_y): # 5. SAFE FALLBACK: Add the directed edge with multiple attribute keys sg.add_edge(ego_node, human_node, label=direction, type=direction, relation=direction) - + + sg.graph['frame'] = self.frame_counter # Track an incremental counter or timestamp + sg.graph['cache'] = {} # The engine uses this to prevent double-calculations return sg def main(args=None): rclpy.init(args=args) From 533fb5012fe28709cebb7a1bcf0f7325da54d45a Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 10:25:19 -0400 Subject: [PATCH 06/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 34 +++++++++++++--------------------- 1 file changed, 13 insertions(+), 21 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index 1573296..cbd71eb 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -13,7 +13,7 @@ import math import threading # Needed to handle safe multi-threaded memory access import networkx as nx -from pass_on_left_ego_only.py import all_human_properties +from pass_on_left_ego_only import all_human_properties class SceneNode: """A custom node object that perfectly matches the SceneFlowLang expectations.""" @@ -50,8 +50,6 @@ def __init__(self): self.transform = T.Compose([T.ToTensor()]) self.threshold = 0.80 self.categories = weights.meta["categories"] - # --- 3. STATE VARIABLES --- - self.graph = SceneGraph() self.camera_hfov = math.radians(73.0) self.latest_lidar_msg = None @@ -78,6 +76,7 @@ def __init__(self): callback_group=self.camera_cb_group) self.active_properties = all_human_properties + self.active_trackers = None self.frame_counter =0 self.get_logger().info('Asynchronous Object Permanence Tracker Operational!') @@ -124,7 +123,6 @@ def set_ground_truth_from_camera(self, camera_angle): if best_match is not None: # Safely lock the variables while updating memory map values with self.lock: - self.active_trackers = symbolic_prop.make_concrete(current_sg) self.human_last_x, self.human_last_y = best_match self.last_seen_time = self.get_clock().now().nanoseconds / 1e9 def lidar_callback(self, msg): @@ -145,9 +143,6 @@ def lidar_callback(self, msg): with self.lock: self.human_last_x = None self.human_last_y = None - if self.graph.connection is not None: - self.graph.connection = None - self.get_logger().info(f"GRAPH UPDATE: {self.graph.get_graph_string()}") return self.frame_counter += 1 @@ -174,7 +169,9 @@ def lidar_callback(self, msg): #self.get_logger().info("Human is at:\nx="+str(best_x)+"\ny="+str(best_y)+"\nDistance: "+str(closest_distan> # Generate the NetworkX graph for this exact microsecond in time current_sg = self.build_networkx_graph(best_x, best_y) - + if self.active_trackers = None: + for prop in self.active_properties: + prop.make_concrete(current_sg) # TODO: Loop through self.active_properties and call prop.evaluate(current_sg) ''' SymbolicProperty( @@ -188,21 +185,16 @@ def lidar_callback(self, msg): [HUMAN] ) ''' - temp = False - try: - temp = self.active_trackers[0] - temp = True - except NameError: - pass - for tracker in self.active_trackers: - tracker.step(current_sg) - if tracker.is_trap(): - self.get_logger().error("Trap State") - else: - self.get_logger().error("Accepting State") + if self.active_trackers is not None: + for tracker in self.active_trackers: + tracker.step(current_sg) + if tracker.is_trap(): + self.get_logger().error("Trap State") + else: + self.get_logger().info("Accepting State") self.get_logger().debug("NetworkX Graph successfully built for this frame!") -def build_networkx_graph(self, human_x, human_y): + def build_networkx_graph(self, human_x, human_y): """Constructs a fresh NetworkX DiGraph for the current physical frame.""" # 1. Initialize an empty Directed Graph sg = nx.DiGraph() From e87bab9596df14ca74fc9d4d5f70ad67c20c5edd Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 10:36:51 -0400 Subject: [PATCH 07/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index cbd71eb..7bb97e5 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -169,10 +169,10 @@ def lidar_callback(self, msg): #self.get_logger().info("Human is at:\nx="+str(best_x)+"\ny="+str(best_y)+"\nDistance: "+str(closest_distan> # Generate the NetworkX graph for this exact microsecond in time current_sg = self.build_networkx_graph(best_x, best_y) - if self.active_trackers = None: + if self.active_trackers is None: + self.active_trackers = [] for prop in self.active_properties: - prop.make_concrete(current_sg) - # TODO: Loop through self.active_properties and call prop.evaluate(current_sg) + self.active_trackers.extend(prop.make_concrete(current_sg)) ''' SymbolicProperty( "robot_must_pass_human_on_the_left", From 2ff2561283e92bb93890e70970089abdef17959e Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 11:12:22 -0400 Subject: [PATCH 08/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index 7bb97e5..d26f1dd 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -173,25 +173,15 @@ def lidar_callback(self, msg): self.active_trackers = [] for prop in self.active_properties: self.active_trackers.extend(prop.make_concrete(current_sg)) - ''' -SymbolicProperty( - "robot_must_pass_human_on_the_left", - "((is_front & !is_behind) -> (!is_left U is_behind))", - [ - ("is_front", person_in_direction(EGO, HUMAN, "FRONT")), - ("is_left", person_in_direction(EGO, HUMAN, "LEFT")), - ("is_behind", person_in_direction(EGO, HUMAN, "BACK")) - ], - [HUMAN] -) - ''' if self.active_trackers is not None: for tracker in self.active_trackers: tracker.step(current_sg) - if tracker.is_trap(): - self.get_logger().error("Trap State") - else: - self.get_logger().info("Accepting State") + if tracker.is_trap() and not tracker.is_accepting(): + self.get_logger().error("Robot Made Error (went Right)") + elif tracker.is_trap(): + self.get_logger().info("Robot successfully passe on Left!") + else: + self.get_logger().info("Robot has not yet made error") self.get_logger().debug("NetworkX Graph successfully built for this frame!") def build_networkx_graph(self, human_x, human_y): From 817386c5d7264a9759a329fcc9aa5eba735c1ce7 Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 11:45:12 -0400 Subject: [PATCH 09/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index d26f1dd..114d9c1 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -58,7 +58,7 @@ def __init__(self): self.last_seen_time = 0.0 self.human_angle = 0.0 - self.max_jump_distance = 0.38 # Increased slightly to account for faster tracking steps + self.max_jump_distance = 0.25 # Increased slightly to account for faster tracking steps self.memory_timeout = 3.5 # Keeps lock stable through the camera's processing lag # --- 4. ROS2 MULTI-THREADING SETUP --- @@ -143,6 +143,7 @@ def lidar_callback(self, msg): with self.lock: self.human_last_x = None self.human_last_y = None + self.active_trackers = None return self.frame_counter += 1 From 9f1687220f20ddccf3b4f88bf36b5bca0f24b9ec Mon Sep 17 00:00:00 2001 From: Linwood Bailey Date: Tue, 23 Jun 2026 11:55:16 -0400 Subject: [PATCH 10/11] Update ros2_async_tracker.py --- ros2_async_tracker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index 114d9c1..a403b2a 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -58,8 +58,8 @@ def __init__(self): self.last_seen_time = 0.0 self.human_angle = 0.0 - self.max_jump_distance = 0.25 # Increased slightly to account for faster tracking steps - self.memory_timeout = 3.5 # Keeps lock stable through the camera's processing lag + self.max_jump_distance = 0.32 + self.memory_timeout = 3.5 # --- 4. ROS2 MULTI-THREADING SETUP --- # Create two separate lane assignments From 5342142e27d4a821ba88c03750890329f5f83f90 Mon Sep 17 00:00:00 2001 From: LW-College Date: Wed, 24 Jun 2026 11:14:23 -0400 Subject: [PATCH 11/11] Enhance tracking with global odometry and debug features Refactor AsyncTrackingNode to include global odometry tracking for the robot and human, enhancing tracking accuracy and adding debug capabilities. --- ros2_async_tracker.py | 276 +++++++++++++++++++++++++++--------------- 1 file changed, 178 insertions(+), 98 deletions(-) diff --git a/ros2_async_tracker.py b/ros2_async_tracker.py index a403b2a..7753699 100644 --- a/ros2_async_tracker.py +++ b/ros2_async_tracker.py @@ -4,6 +4,7 @@ from rclpy.executors import MultiThreadedExecutor from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import PoseArray +from nav_msgs.msg import Odometry # NEW: We need to import the Odometry message type import torch from torchvision.models.detection import fasterrcnn_resnet50_fpn, FasterRCNN_ResNet50_FPN_Weights from PIL import Image as PILImage @@ -11,38 +12,29 @@ import cv2 import numpy as np import math -import threading # Needed to handle safe multi-threaded memory access +import threading import networkx as nx from pass_on_left_ego_only import all_human_properties +from geometry_msgs.msg import PoseArray +from visualization_msgs.msg import Marker class SceneNode: - """A custom node object that perfectly matches the SceneFlowLang expectations.""" def __init__(self, node_id, name, base_class): self.id = node_id self.name = name self.base_class = base_class - def get_id(self): - return self.id - - def is_phantom(self): - # The repo uses this to ignore certain nodes; we want our human to be real! - return False - - def __hash__(self): - # NetworkX requires custom objects to be hashable so it can use them as dictionary keys - return hash(self.id) - - def __eq__(self, other): - return getattr(other, 'id', None) == self.id + def get_id(self): return self.id + def is_phantom(self): return False + def __hash__(self): return hash(self.id) + def __eq__(self, other): return getattr(other, 'id', None) == self.id + class AsyncTrackingNode(Node): def __init__(self): super().__init__('async_tracking_node') - # --- 1. THREAD SAFETY LOCK --- self.lock = threading.Lock() - # --- 2. MODEL SETUP --- self.get_logger().info('Loading PyTorch model...') weights = FasterRCNN_ResNet50_FPN_Weights.DEFAULT self.model = fasterrcnn_resnet50_fpn(weights=weights) @@ -53,36 +45,54 @@ def __init__(self): self.camera_hfov = math.radians(73.0) self.latest_lidar_msg = None - self.human_last_x = None - self.human_last_y = None + # NEW: Now our memory tracks GLOBAL Map Coordinates, not Local Robot Coordinates! + self.human_last_global_x = None + self.human_last_global_y = None self.last_seen_time = 0.0 - self.human_angle = 0.0 - - self.max_jump_distance = 0.32 - self.memory_timeout = 3.5 + + # NEW: Store the Robot's current position and rotation in the world + self.robot_global_x = 0.0 + self.robot_global_y = 0.0 + self.robot_global_yaw = 0.0 + self.human_vx = 0.0 + self.human_vy = 0.0 + self.max_jump_distance = 0.32 + self.memory_timeout = 3.5 - # --- 4. ROS2 MULTI-THREADING SETUP --- - # Create two separate lane assignments self.lidar_cb_group = MutuallyExclusiveCallbackGroup() self.camera_cb_group = MutuallyExclusiveCallbackGroup() - - # Assign the subscriptions to their respective lanes - self.lidar_sub = self.create_subscription( - PoseArray, '/detected_objects', self.lidar_callback, 10, - callback_group=self.lidar_cb_group) - - self.camera_sub = self.create_subscription( - CompressedImage, '/oak/rgb/image_raw/compressed', self.camera_callback, 1, - callback_group=self.camera_cb_group) + self.odom_cb_group = MutuallyExclusiveCallbackGroup() self.active_properties = all_human_properties self.active_trackers = None - self.frame_counter =0 - self.get_logger().info('Asynchronous Object Permanence Tracker Operational!') + self.frame_counter = 0 + self.get_logger().info('Object Permanence Tracker with Global Odometry is Operational!') + # Subscriptions + self.lidar_sub = self.create_subscription( + PoseArray, '/detected_objects', self.lidar_callback, 10, callback_group=self.lidar_cb_group) + self.camera_sub = self.create_subscription( + CompressedImage, '/oak/rgb/image_raw/compressed', self.camera_callback, 1, callback_group=self.camera_cb_group) + self.odom_sub = self.create_subscription( + Odometry, '/odometry/filtered', self.odom_callback, 10, callback_group=self.odom_cb_group) + # NEW: Debug Publisher to broadcast where the robot thinks the human is + self.debug_pub = self.create_publisher(Marker, '/debug/human_tracked_position', 10) + + # NEW: Helper function to convert Robot Quaternion data into simple Yaw (rotation in radians) + def euler_from_quaternion(self, x, y, z, w): + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + return math.atan2(t3, t4) + + # NEW: Keep track of exactly where the robot is in the world at all times + def odom_callback(self, msg): + with self.lock: + self.robot_global_x = msg.pose.pose.position.x + self.robot_global_y = msg.pose.pose.position.y + q = msg.pose.pose.orientation + self.robot_global_yaw = self.euler_from_quaternion(q.x, q.y, q.z, q.w) def camera_callback(self, msg): - # Heavy computing happens here in Thread B np_arr = np.frombuffer(msg.data, np.uint8) img_bgr = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) if img_bgr is None: return @@ -107,132 +117,202 @@ def set_ground_truth_from_camera(self, camera_angle): with self.lock: if self.latest_lidar_msg is None: return poses = self.latest_lidar_msg.poses + rob_x = self.robot_global_x + rob_y = self.robot_global_y + rob_yaw = self.robot_global_yaw + + # NEW: Pull the current human memory so the camera can check its math + hg_x = self.human_last_global_x + hg_y = self.human_last_global_y best_match = None - min_angle_diff = 100.0 + min_physical_distance = 999.0 for pose in poses: - x_robot = -pose.position.x - y_robot = -pose.position.y - lidar_angle = math.atan2(y_robot, x_robot) + x_local = -pose.position.x + y_local = -pose.position.y + lidar_angle = math.atan2(y_local, x_local) angle_diff = abs(camera_angle - lidar_angle) - if angle_diff < math.radians(20.0) and angle_diff < min_angle_diff: - min_angle_diff = angle_diff - best_match = (x_robot, y_robot) + if angle_diff < math.radians(15.0): + distance_to_clump = math.hypot(x_local, y_local) + if distance_to_clump < min_physical_distance: + min_physical_distance = distance_to_clump + + global_x = rob_x + (x_local * math.cos(rob_yaw)) - (y_local * math.sin(rob_yaw)) + global_y = rob_y + (x_local * math.sin(rob_yaw)) + (y_local * math.cos(rob_yaw)) + best_match = (global_x, global_y) + if best_match is not None: - # Safely lock the variables while updating memory map values + # --- NEW: DIAGNOSTIC AND JUMP LIMIT FOR THE CAMERA --- + if hg_x is not None and hg_y is not None: + jump_dist = math.hypot(best_match[0] - hg_x, best_match[1] - hg_y) + + # If the camera tries to teleport the human further than your limit, block it! + if jump_dist > self.max_jump_distance: + self.get_logger().warn(f"CAMERA FALSE POSITIVE BLOCKED: Tried to jump {jump_dist:.2f}m") + return # Exit the function immediately without updating memory + + # If it passes the test (or if we have no current memory), update the coordinates with self.lock: - self.human_last_x, self.human_last_y = best_match + self.human_last_global_x, self.human_last_global_y = best_match self.last_seen_time = self.get_clock().now().nanoseconds / 1e9 + # ----------------------------------------------------- + def lidar_callback(self, msg): - # Fast computing happens here in Thread A with self.lock: self.latest_lidar_msg = msg - hx, hy = self.human_last_x, self.human_last_y + hg_x, hg_y = self.human_last_global_x, self.human_last_global_y + rob_x, rob_y, rob_yaw = self.robot_global_x, self.robot_global_y, self.robot_global_yaw last_time = self.last_seen_time - if hx is None: - return - + if hg_x is None: return current_time = self.get_clock().now().nanoseconds / 1e9 + dt = current_time - last_time # Calculate time passed since last scan + # NEW: Predict where the human is based on their current walking speed + if dt > 0 and dt < 1.0: + pred_x = hg_x + (self.human_vx * dt) + pred_y = hg_y + (self.human_vy * dt) + else: + # If it's been too long, reset momentum + pred_x = hg_x + pred_y = hg_y + self.human_vx = 0.0 + self.human_vy = 0.0 if current_time - last_time > self.memory_timeout: self.get_logger().warn("Track lost! Human out of range. Resetting graph.") self.frame_counter = 0 with self.lock: - self.human_last_x = None - self.human_last_y = None + self.human_last_global_x = None + self.human_last_global_y = None self.active_trackers = None return self.frame_counter += 1 closest_distance = 999.0 - best_x = None - best_y = None + best_global_x = None + best_global_y = None + best_local_x = None + best_local_y = None + for pose in msg.poses: - x_robot = -pose.position.x - y_robot = -pose.position.y + x_local = -pose.position.x + y_local = -pose.position.y + + # NEW: Convert EVERY Lidar clump into a Global coordinate before measuring distance + clump_global_x = rob_x + (x_local * math.cos(rob_yaw)) - (y_local * math.sin(rob_yaw)) + clump_global_y = rob_y + (x_local * math.sin(rob_yaw)) + (y_local * math.cos(rob_yaw)) - jump_dist = math.hypot(x_robot - hx, y_robot - hy) + # NEW: The jump distance is now based purely on Global map coordinates, immune to robot movement! + jump_dist = math.hypot(clump_global_x - pred_x, clump_global_y - pred_y) + if jump_dist < closest_distance: closest_distance = jump_dist - best_x = x_robot - best_y = y_robot + best_global_x = clump_global_x + best_global_y = clump_global_y + + # We still save the local coordinates because the DFA rulebook (FRONT/LEFT/RIGHT) needs Ego-centric values! + best_local_x = x_local + best_local_y = y_local - if closest_distance <= self.max_jump_distance and best_x is not None: + if closest_distance <= self.max_jump_distance and best_global_x is not None: with self.lock: - self.human_last_x = best_x - self.human_last_y = best_y + # NEW: Calculate how fast the human is moving to update their momentum + if dt > 0: + inst_vx = (best_global_x - hg_x) / dt + inst_vy = (best_global_y - hg_y) / dt + + # Smooth the velocity (50% old, 50% new) so it doesn't get jerky + self.human_vx = (0.5 * inst_vx) + (0.5 * self.human_vx) + self.human_vy = (0.5 * inst_vy) + (0.5 * self.human_vy) + + # Update memory with the new Global position + self.human_last_global_x = best_global_x + self.human_last_global_y = best_global_y self.last_seen_time = current_time - self.human_angle = math.degrees(math.atan2(best_y, best_x)) - self.get_logger().info("Human Angle: "+ str(self.human_angle)) - #self.get_logger().info("Human is at:\nx="+str(best_x)+"\ny="+str(best_y)+"\nDistance: "+str(closest_distan> - # Generate the NetworkX graph for this exact microsecond in time - current_sg = self.build_networkx_graph(best_x, best_y) + + #debug publisher: + debug_msg = Marker() + + # 1. Header info + debug_msg.header.stamp = self.get_clock().now().to_msg() + debug_msg.header.frame_id = 'odom' + + # 2. Marker Configuration + debug_msg.ns = "human_tracker" + debug_msg.id = 0 + debug_msg.type = Marker.CYLINDER # Draw a cylinder + debug_msg.action = Marker.ADD + + # 3. Position (Using your global math) + debug_msg.pose.position.x = best_global_x + debug_msg.pose.position.y = best_global_y + debug_msg.pose.position.z = 0.5 # Lift it half a meter off the floor + + # 4. Size (Human sized: 0.4m wide, 1.0m tall) + debug_msg.scale.x = 0.4 + debug_msg.scale.y = 0.4 + debug_msg.scale.z = 1.0 + + # 5. Color (Bright Green, fully opaque) + debug_msg.color.r = 0.0 + debug_msg.color.g = 1.0 + debug_msg.color.b = 0.0 + debug_msg.color.a = 1.0 + + self.debug_pub.publish(debug_msg) + + # The NetworkX graph still gets the local coordinates, so FRONT/LEFT/RIGHT is always relative to the camera + current_sg = self.build_networkx_graph(best_local_x, best_local_y) + if self.active_trackers is None: self.active_trackers = [] for prop in self.active_properties: self.active_trackers.extend(prop.make_concrete(current_sg)) + if self.active_trackers is not None: for tracker in self.active_trackers: tracker.step(current_sg) if tracker.is_trap() and not tracker.is_accepting(): - self.get_logger().error("Robot Made Error (went Right)") + self.get_logger().error("Trap State: Maneuver Violated!") elif tracker.is_trap(): - self.get_logger().info("Robot successfully passe on Left!") - else: - self.get_logger().info("Robot has not yet made error") - self.get_logger().debug("NetworkX Graph successfully built for this frame!") + self.get_logger().info("Robot Successfully Passed on Left! !]") + else: + self.get_logger().info("Accepting State: Human is at ("+ str(round(self.human_last_global_x*100)/100)+", "+str(round(self.human_last_global_y*100)/100)+")") def build_networkx_graph(self, human_x, human_y): - """Constructs a fresh NetworkX DiGraph for the current physical frame.""" - # 1. Initialize an empty Directed Graph sg = nx.DiGraph() - - # 2. Create the Ego (Robot) Node and Human Node ego_node = SceneNode(node_id=0, name="ego", base_class="vehicle") human_node = SceneNode(node_id=1, name="person_1", base_class="person") - - # 3. Add the nodes to the graph sg.add_node(ego_node) sg.add_node(human_node) - # 4. Calculate the spatial relationship angle_rad = math.atan2(human_y, human_x) angle_deg = math.degrees(angle_rad) - if -45.0 <= angle_deg <= 45.0: - direction = "FRONT" - elif 45.0 < angle_deg <= 135.0: - direction = "LEFT" - elif -135.0 <= angle_deg < -45.0: - direction = "RIGHT" - else: - direction = "BACK" + if -45.0 <= angle_deg <= 45.0: direction = "FRONT" + elif 45.0 < angle_deg <= 135.0: direction = "LEFT" + elif -135.0 <= angle_deg < -45.0: direction = "RIGHT" + else: direction = "BACK" - # 5. SAFE FALLBACK: Add the directed edge with multiple attribute keys sg.add_edge(ego_node, human_node, label=direction, type=direction, relation=direction) - - sg.graph['frame'] = self.frame_counter # Track an incremental counter or timestamp - sg.graph['cache'] = {} # The engine uses this to prevent double-calculations + sg.graph['frame'] = self.frame_counter + sg.graph['cache'] = {} return sg + def main(args=None): rclpy.init(args=args) node = AsyncTrackingNode() - - # NEW FOR PHASE 4.5: Explicitly use a MultiThreadedExecutor instead of the default single thread executor = MultiThreadedExecutor() executor.add_node(node) - try: - executor.spin() # Spin handles tracking across available threads asynchronously + executor.spin() except KeyboardInterrupt: pass finally: node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() + if rclpy.ok(): rclpy.shutdown() if __name__ == '__main__': main()