From 5efcc2764e2b6570e5d867e7ea782423b4ee1c99 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Thu, 17 Sep 2026 13:15:14 -0400 Subject: [PATCH 1/2] Make GetSyncedImagePointCloudDepth topics optional --- .../ros_behaviors/perception.py | 94 ++++++++----------- .../tests/test_ros_behaviors.py | 78 ++++++++++----- 2 files changed, 92 insertions(+), 80 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py b/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py index 35e64b5..66183f0 100644 --- a/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py +++ b/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py @@ -34,10 +34,10 @@ def __init__( self, name: str, *, - camera_info_topic: str, - rgb_image_topic: str, - depth_image_topic: str, - point_cloud_topic: str, + camera_info_topic: str | None = None, + rgb_image_topic: str | None = None, + depth_image_topic: str | None = None, + point_cloud_topic: str | None = None, queue_size: int = 10, time_slop: float = 0.1, sync_timeout: float | None = None, @@ -48,10 +48,10 @@ def __init__( Args: name: The name of the behavior (required by PyTrees). - camera_info_topic: The name of the camera info topic. - rgb_image_topic: The name of the RGB image topic. - depth_image_topic: The name of the depth image topic. - point_cloud_topic: The name of the point cloud topic. + camera_info_topic: The name of the camera info topic (optional). + rgb_image_topic: The name of the RGB image topic (optional). + depth_image_topic: The name of the depth image topic (optional). + point_cloud_topic: The name of the point cloud topic (optional). queue_size: The size of the queue in the message synchronizer. time_stop: The time slop, in seconds, in the message synchronizer. sync_timeout: Timeout, in seconds, to wait for the synchronized data. @@ -69,18 +69,16 @@ def __init__( self.node: Node self.synchronizer = None - self.camera_info_sub = None - self.rgb_image_sub = None - self.depth_image_sub = None - self.point_cloud_sub = None + self.subscribers: list[message_filters.Subscriber] = [] + self.port_order: list[str] = [] INPUT_PORTS = {} OUTPUT_PORTS = { - "camera_info": PortInformation(data_type=CameraInfo, required=True), - "rgb_image": PortInformation(data_type=Image, required=True), - "depth_image": PortInformation(data_type=Image, required=True), - "point_cloud": PortInformation(data_type=PointCloud2, required=True), + "camera_info": PortInformation(data_type=CameraInfo, required=False), + "rgb_image": PortInformation(data_type=Image, required=False), + "depth_image": PortInformation(data_type=Image, required=False), + "point_cloud": PortInformation(data_type=PointCloud2, required=False), } def setup(self, **kwargs): @@ -95,54 +93,41 @@ def initialise(self) -> None: """ self.latest_data = None - self.camera_info_sub = message_filters.Subscriber(self.node, CameraInfo, self.camera_info_topic) - self.rgb_image_sub = message_filters.Subscriber(self.node, Image, self.rgb_image_topic) - self.depth_image_sub = message_filters.Subscriber(self.node, Image, self.depth_image_topic) - self.point_cloud_sub = message_filters.Subscriber(self.node, PointCloud2, self.point_cloud_topic) + self.subscribers = [] + self.port_order = [] + if self.camera_info_topic: + self.subscribers.append(message_filters.Subscriber(self.node, CameraInfo, self.camera_info_topic)) + self.port_order.append("camera_info") + if self.rgb_image_topic: + self.subscribers.append(message_filters.Subscriber(self.node, Image, self.rgb_image_topic)) + self.port_order.append("rgb_image") + if self.depth_image_topic: + self.subscribers.append(message_filters.Subscriber(self.node, Image, self.depth_image_topic)) + self.port_order.append("depth_image") + if self.point_cloud_topic: + self.subscribers.append(message_filters.Subscriber(self.node, PointCloud2, self.point_cloud_topic)) + self.port_order.append("point_cloud") self.synchronizer = message_filters.ApproximateTimeSynchronizer( - [ - self.camera_info_sub, - self.rgb_image_sub, - self.depth_image_sub, - self.point_cloud_sub, - ], + self.subscribers, queue_size=self.queue_size, slop=self.time_slop, ) self.synchronizer.registerCallback(self._synchronize_callback) self.start_time = self.node.get_clock().now() - def _synchronize_callback( - self, - camera_info_msg: CameraInfo, - rgb_image_msg: Image, - depth_image_msg: Image, - point_cloud_msg: PointCloud2, - ) -> None: + def _synchronize_callback(self, *data) -> None: """Callback triggered only when all topics have synchronized headers.""" - self.latest_data = ( - camera_info_msg, - rgb_image_msg, - depth_image_msg, - point_cloud_msg, - ) + self.latest_data = data def update(self) -> Status: """ Executes every time the behavior tree ticks this node. """ if self.latest_data is not None: - self.logger.info(f"[{self.qualified_name}] Got synchronized images and point clouds!") - camera_info_msg, rgb_image_msg, depth_image_msg, point_cloud_msg = self.latest_data - self._set_output("camera_info", camera_info_msg) - self._set_output("rgb_image", rgb_image_msg) - self._set_output("depth_image", depth_image_msg) - self._set_output("point_cloud", point_cloud_msg) - - # Clear cache so we don't process the exact same frame on the next tick. - self.latest_data = None - + self.logger.info(f"[{self.qualified_name}] Got synchronized perception data!") + for message, port_name in zip(self.latest_data, self.port_order, strict=True): + self._set_output(port_name, message) return py_trees.common.Status.SUCCESS # If no synchronized frame has arrived yet, keep waiting until timeout. @@ -154,17 +139,12 @@ def update(self) -> Status: def terminate(self, new_status: Status) -> None: """Cleanup if the behavior is interrupted or completes.""" + self.latest_data = None if self.node is None: return if self.synchronizer is not None: self.synchronizer.callbacks.clear() - for subscription in ( - self.camera_info_sub, - self.rgb_image_sub, - self.depth_image_sub, - self.point_cloud_sub, - ): - if subscription is not None: - self.node.destroy_subscription(subscription.sub) + for subscription in self.subscribers: + self.node.destroy_subscription(subscription.sub) diff --git a/src/imetro_behavior/tests/test_ros_behaviors.py b/src/imetro_behavior/tests/test_ros_behaviors.py index 4c54bc4..629ba6f 100644 --- a/src/imetro_behavior/tests/test_ros_behaviors.py +++ b/src/imetro_behavior/tests/test_ros_behaviors.py @@ -23,6 +23,7 @@ from imetro_behavior.ros_behaviors.service_client import CallTriggerService from imetro_behavior.ros_behaviors.subscriber_base import GetStringTopic from py_trees.common import Status +from py_trees.ports import NoDataAvailable from rclpy.duration import Duration from rclpy.node import Node from rclpy.task import Future @@ -99,8 +100,7 @@ def test_call_trigger_service(ros_node: Node) -> None: assert behavior.process_response(Trigger.Response(success=False, message="oops")) == Status.FAILURE -@pytest.fixture() -def sync_behavior(ros_node: Node) -> GetSyncedImagePointCloudDepth: +def test_get_synced_data_full_topics(ros_node: Node) -> None: behavior = GetSyncedImagePointCloudDepth( name="get_synced_data", camera_info_topic="/camera/camera_info", @@ -111,34 +111,66 @@ def sync_behavior(ros_node: Node) -> GetSyncedImagePointCloudDepth: ) behavior.setup(node=ros_node) behavior.setup_ports() - return behavior + behavior.initialise() + # Inject a synchronized frame directly instead of waiting for topics. + behavior.latest_data = (CameraInfo(), Image(), Image(), PointCloud2()) + + assert behavior.update() == Status.SUCCESS + assert isinstance(behavior.get_last_output("camera_info"), CameraInfo) + assert isinstance(behavior.get_last_output("rgb_image"), Image) + assert isinstance(behavior.get_last_output("depth_image"), Image) + assert isinstance(behavior.get_last_output("point_cloud"), PointCloud2) + # The latest data should clear on termination. + behavior.terminate(Status.INVALID) + assert behavior.latest_data is None -def test_get_synced_data_success(sync_behavior: GetSyncedImagePointCloudDepth) -> None: - # Inject a synchronized frame directly instead of calling initialise(), - # which would create real topic subscriptions. - sync_behavior.latest_data = (CameraInfo(), Image(), Image(), PointCloud2()) - assert sync_behavior.update() == Status.SUCCESS - assert isinstance(sync_behavior.get_last_output("camera_info"), CameraInfo) - assert isinstance(sync_behavior.get_last_output("rgb_image"), Image) - assert isinstance(sync_behavior.get_last_output("depth_image"), Image) - assert isinstance(sync_behavior.get_last_output("point_cloud"), PointCloud2) - # The cached frame should be cleared so the same data is not processed twice. - assert sync_behavior.latest_data is None +def test_get_synced_data_partial_topics(ros_node: Node) -> None: + behavior = GetSyncedImagePointCloudDepth( + name="get_synced_data", + rgb_image_topic="/camera/color", + depth_image_topic="/camera/depth", + sync_timeout=1.0, + ) + behavior.setup(node=ros_node) + behavior.setup_ports() + behavior.initialise() + # Inject a synchronized frame directly instead of waiting for topics. + behavior.latest_data = (Image(), Image()) + assert behavior.update() == Status.SUCCESS + assert isinstance(behavior.get_last_output("rgb_image"), Image) + assert isinstance(behavior.get_last_output("depth_image"), Image) + with pytest.raises(NoDataAvailable): + behavior.get_last_output("camera_info") + with pytest.raises(NoDataAvailable): + behavior.get_last_output("point_cloud") -def test_get_synced_data_waiting_and_timeout( - sync_behavior: GetSyncedImagePointCloudDepth, -) -> None: - node = sync_behavior.node - sync_behavior.latest_data = None + # The latest data should clear on termination. + behavior.terminate(Status.INVALID) + assert behavior.latest_data is None + + +def test_get_synced_data_waiting_and_timeout(ros_node: Node) -> None: + behavior = GetSyncedImagePointCloudDepth( + name="get_synced_data", + camera_info_topic="/camera/camera_info", + rgb_image_topic="/camera/color", + depth_image_topic="/camera/depth", + point_cloud_topic="/camera/points", + sync_timeout=1.0, + ) + behavior.setup(node=ros_node) + behavior.setup_ports() + behavior.initialise() + node = behavior.node - sync_behavior.start_time = node.get_clock().now() - assert sync_behavior.update() == Status.RUNNING + behavior.start_time = node.get_clock().now() + assert behavior.update() == Status.RUNNING - sync_behavior.start_time = node.get_clock().now() - Duration(seconds=2.0) # ty: ignore [invalid-assignment] - assert sync_behavior.update() == Status.FAILURE + behavior.start_time = node.get_clock().now() - Duration(seconds=2.0) # ty: ignore [invalid-assignment] + assert behavior.update() == Status.FAILURE @pytest.fixture() From 2bb54e518e66048a25262846cdadc7da500839b1 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Thu, 17 Sep 2026 15:37:38 -0400 Subject: [PATCH 2/2] Raise on no subscribers --- .../imetro_behavior/ros_behaviors/perception.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py b/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py index 66183f0..2be7346 100644 --- a/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py +++ b/src/imetro_behavior/imetro_behavior/ros_behaviors/perception.py @@ -81,7 +81,7 @@ def __init__( "point_cloud": PortInformation(data_type=PointCloud2, required=False), } - def setup(self, **kwargs): + def setup(self, **kwargs) -> None: """ Sets up the ROS node needed for the message filters. """ @@ -108,6 +108,9 @@ def initialise(self) -> None: self.subscribers.append(message_filters.Subscriber(self.node, PointCloud2, self.point_cloud_topic)) self.port_order.append("point_cloud") + if not self.subscribers: + raise RuntimeError(f"[{self.qualified_name}] No topics selected for synchronization!") + self.synchronizer = message_filters.ApproximateTimeSynchronizer( self.subscribers, queue_size=self.queue_size,