Make GetSyncedImagePointCloudDepth topics optional

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,21 +69,19 @@ 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):
+    def setup(self, **kwargs) -> None:
         """
         Sets up the ROS node needed for the message filters.
         """
@@ -95,54 +93,44 @@ 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")
+
+        if not self.subscribers:
+            raise RuntimeError(f"[{self.qualified_name}] No topics selected for synchronization!")
 
         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 +142,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)

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()