feat: stream clustered poses based on child_frame_ids

Author: wes-w-z-h

bb_filters/nodes/cluster/cluster_poses_action_node.py

@@ -9,6 +9,7 @@
     ClusterPosesNode,
     fill_cluster_result_array,
     seconds_to_duration,
+    validate_stream_frame_ids,
 )
 from rclpy.action import ActionServer, GoalResponse
 from rclpy.action.server import ServerGoalHandle
@@ -117,6 +118,10 @@ def _start_goal(self, goal_handle: ServerGoalHandle) -> None:
         feedback.poses_collected_so_far = 0
         goal_handle.publish_feedback(feedback)
 
+        validate_stream_frame_ids(
+            list(goal.params.clustered_child_frame_ids),
+            len(goal.params.pose_stamped_topics),
+        )
         self._start_subscribers(
             odom_topic=goal.params.odom_topic,
             pose_topics=list(goal.params.pose_stamped_topics),
@@ -154,7 +159,7 @@ def _step_collection(self, goal_handle: ServerGoalHandle) -> None:
         feedback = ClusterPosesAction.Feedback()
         feedback.current_status = "Collecting synchronized messages"
         feedback.collection_progress = min(elapsed.nanoseconds / duration_ns, 1.0)
-        feedback.poses_collected_so_far = len(self._synchronized_data)
+        feedback.poses_collected_so_far = self._num_collected()
         goal_handle.publish_feedback(feedback)
 
     def _finalize_goal(self, goal_handle: ServerGoalHandle) -> None:
@@ -166,21 +171,18 @@ def _finalize_goal(self, goal_handle: ServerGoalHandle) -> None:
         feedback = ClusterPosesAction.Feedback()
         feedback.current_status = "Finalizing clustering"
         feedback.collection_progress = 1.0
-        feedback.poses_collected_so_far = len(self._synchronized_data)
+        feedback.poses_collected_so_far = self._num_collected()
         goal_handle.publish_feedback(feedback)
 
-        clustered, transformed_poses, total_collected = self._run_clustering(
-            self._cluster_params(goal)
+        clustered, total_collected, last_header = self._cluster_streams(
+            list(params.clustered_child_frame_ids),
+            self._cluster_params(goal),
+            publish_tfs=True,
         )
         if not clustered:
             self._finish_action("aborted", self._empty_result(int(params.sort_key)))
             return
 
-        last_header = transformed_poses[-1].header
-        self._publish_results(
-            clustered, transformed_poses, params.clustered_child_frame_id
-        )
-
         result = ClusterPosesAction.Result()
         fill_cluster_result_array(
             result.cluster_results,

bb_filters/nodes/cluster/cluster_poses_node.py

@@ -69,6 +69,19 @@ def build_cluster_pose_result(
     return msg
 
 
+def validate_stream_frame_ids(frame_ids: list[str], num_pose_topics: int) -> None:
+    """Reject `clustered_child_frame_ids` lengths the stream layout can't honour.
+
+    Empty (single default stream) and 1 (single merged stream) are always
+    valid; otherwise there must be exactly one frame per pose topic.
+    """
+    if frame_ids and len(frame_ids) not in (1, num_pose_topics):
+        raise ValueError(
+            "clustered_child_frame_ids must be empty, have 1 entry, or one per "
+            f"pose topic ({num_pose_topics}); got {len(frame_ids)}"
+        )
+
+
 def fill_cluster_result_array(
     msg: ClusterPoseResultArray,
     clustered: list[tuple[Pose, ClusterResult]],
@@ -106,7 +119,10 @@ def __init__(self, node_name: str) -> None:
             qos_profile=static_qos,
         )
 
-        self._synchronized_data: list[tuple[Odometry, PoseStamped]] = []
+        # One accumulation buffer per pose topic, indexed by topic position.
+        # Populated by `_start_subscribers`; stream layout (merge vs. split) is
+        # decided later by the output frame IDs, not here.
+        self._stream_data: list[list[tuple[Odometry, PoseStamped]]] = []
         self._camera_to_odom_transforms: dict[str, TransformStamped] = {}
         self._odom_subscriber: Subscriber | None = None
         self._pose_subscribers: list[Subscriber] = []
@@ -138,8 +154,13 @@ def _handle_tf_static(self, msg: TFMessage) -> None:
         for transform in msg.transforms:
             self._tf_buffer.set_transform_static(transform, "default_authority")
 
-    def _synchronized_callback(self, odom_msg: Odometry, pose_msg: PoseStamped) -> None:
-        self._synchronized_data.append((odom_msg, pose_msg))
+    def _collect_pose(
+        self, odom_msg: Odometry, pose_msg: PoseStamped, stream_index: int
+    ) -> None:
+        self._stream_data[stream_index].append((odom_msg, pose_msg))
+
+    def _num_collected(self) -> int:
+        return sum(len(stream) for stream in self._stream_data)
 
     def _is_detection_too_old(
         self, clustering_time: Time, pose_msg: PoseStamped
@@ -153,7 +174,7 @@ def _is_detection_too_old(
         return detection_age_s >= self._max_detection_age_s
 
     def _reset_collection(self) -> None:
-        self._synchronized_data = []
+        self._stream_data = []
         self._camera_to_odom_transforms = {}
 
     def _ensure_camera_to_odom(
@@ -201,18 +222,25 @@ def _start_subscribers(
         sync_queue_size: int | None = None,
         max_detection_age_s: float = 0.0,
     ) -> None:
-        """Subscribe to one odom topic and N pose topics, all feeding one buffer."""
+        """Subscribe to one odom topic and N pose topics.
+
+        Each pose topic accumulates into its own `_stream_data` buffer (indexed
+        by topic position). Whether those buffers are later merged into one
+        clustering stream or clustered independently is decided at cluster time
+        by the output frame IDs, not here.
+        """
         if not pose_topics:
             raise ValueError("pose_topics must contain at least one topic")
         self._max_detection_age_s = float(max_detection_age_s)
+        self._stream_data = [[] for _ in pose_topics]
         qsize = int(sync_queue_size or self._sync_queue_size)
         self._odom_subscriber = Subscriber(
             self,
             Odometry,
             odom_topic,
             qos_profile=qos_profile_sensor_data,
         )
-        for topic in pose_topics:
+        for stream_index, topic in enumerate(pose_topics):
             pose_sub = Subscriber(
                 self,
                 PoseStamped,
@@ -225,7 +253,7 @@ def _start_subscribers(
                 queue_size=qsize,
                 slop=float(sync_tolerance),
             )
-            time_synchronizer.registerCallback(self._synchronized_callback)
+            time_synchronizer.registerCallback(self._collect_pose, stream_index)
             self._time_synchronizers.append(time_synchronizer)
 
     def _cleanup_subscribers(self) -> None:
@@ -242,35 +270,80 @@ def _cleanup_subscribers(self) -> None:
         self._pose_subscribers = []
         self._time_synchronizers = []
 
+    def _resolve_stream_frames(
+        self, frame_ids: list[str]
+    ) -> list[tuple[list[tuple[Odometry, PoseStamped]], str]]:
+        """Pair collected poses with the output frame for each clustering stream.
+
+        The number of `frame_ids` selects the layout (see
+        `validate_stream_frame_ids`):
+          - empty or 1 entry: every pose topic is merged into a single stream.
+          - one per pose topic: each topic is its own independent stream.
+        """
+        frame_ids = list(frame_ids) or ["clustered_object"]
+        validate_stream_frame_ids(frame_ids, len(self._stream_data))
+        if len(frame_ids) == 1:
+            merged = [pair for stream in self._stream_data for pair in stream]
+            return [(merged, frame_ids[0])]
+        return list(zip(self._stream_data, frame_ids))
+
+    def _cluster_streams(
+        self,
+        frame_ids: list[str],
+        params: ClusterParams,
+        publish_tfs: bool,
+    ) -> tuple[list[tuple[Pose, ClusterResult]], int, object]:
+        """Cluster each output stream independently and aggregate the results.
+
+        `params.top_k` applies per stream, so N streams can yield up to
+        N * top_k results. Returns `(clustered, total_collected, last_header)`;
+        when `publish_tfs`, each stream's results are TF-broadcast under its
+        own frame.
+        """
+        aggregated: list[tuple[Pose, ClusterResult]] = []
+        total_collected = 0
+        last_header = None
+        for synchronized_data, frame_id in self._resolve_stream_frames(frame_ids):
+            clustered, transformed_poses, collected = self._run_clustering(
+                synchronized_data, params
+            )
+            total_collected += collected
+            if not clustered:
+                continue
+            last_header = transformed_poses[-1].header
+            if publish_tfs:
+                self._publish_results(clustered, transformed_poses, frame_id)
+            aggregated.extend(clustered)
+        return aggregated, total_collected, last_header
+
     def _run_clustering(
         self,
+        synchronized_data: list[tuple[Odometry, PoseStamped]],
         params: ClusterParams,
     ) -> tuple[
         list[tuple[Pose, ClusterResult]],
         list[PoseStamped],
         int,
     ]:
-        """Run the full clustering pipeline against the current buffer.
+        """Run the full clustering pipeline against `synchronized_data`.
 
         Returns `(clustered, transformed_poses, total_collected)`
         """
-        if not self._synchronized_data:
-            total_collected = 0
+        if not synchronized_data:
             self.get_logger().error(
                 "Not enough synchronized poses collected. "
-                f"Got {total_collected}, need {int(params.min_poses)}"
+                f"Got 0, need {int(params.min_poses)}"
             )
-            return [], [], total_collected
+            return [], [], 0
 
         # Match the result PoseArray/ClusterPoseResultArray timestamp, which is
         # taken from the last transformed pose after this age filter.
-        clustering_time = Time.from_msg(self._synchronized_data[-1][1].header.stamp)
-        self._synchronized_data = [
+        clustering_time = Time.from_msg(synchronized_data[-1][1].header.stamp)
+        synchronized_data = [
             (odom_msg, pose_msg)
-            for odom_msg, pose_msg in self._synchronized_data
+            for odom_msg, pose_msg in synchronized_data
             if not self._is_detection_too_old(clustering_time, pose_msg)
         ]
-        synchronized_data = self._synchronized_data
         total_collected = len(synchronized_data)
 
         if total_collected < int(params.min_poses):

bb_filters/nodes/cluster/cluster_poses_service_node.py

@@ -9,6 +9,7 @@
     ClusterParams,
     ClusterPosesNode,
     fill_cluster_result_array,
+    validate_stream_frame_ids,
 )
 from geometry_msgs.msg import Pose
 from rclpy.timer import Timer
@@ -54,7 +55,7 @@ def __init__(self) -> None:
         self.top_k = 1
         self.sort_key = int(ClusterSortKey.NUM_CLUSTER_POSES)
         self.cluster_interval = 0.0
-        self._clustered_child_frame_id = ""
+        self._clustered_child_frame_ids: list[str] = []
         self._cluster_timer: Timer | None = None
 
         self.get_logger().info("Cluster Poses Service Node initialized")
@@ -78,8 +79,11 @@ def _apply_request_config(self, request: ClusterPosesSrv.Request) -> None:
         self.top_k = int(params.top_k)
         self.sort_key = int(params.sort_key)
         self.cluster_interval = float(request.cluster_interval)
-        self._clustered_child_frame_id = (
-            params.clustered_child_frame_id or "unknown/clustered"
+        # Stream layout (merge vs. independent per topic) is derived from the
+        # number of frame IDs at cluster time; validate the length up front.
+        self._clustered_child_frame_ids = list(params.clustered_child_frame_ids)
+        validate_stream_frame_ids(
+            self._clustered_child_frame_ids, len(self.pose_stamped_topics)
         )
 
     def _cluster_params(self) -> ClusterParams:
@@ -152,17 +156,13 @@ def _stop_and_cluster(
             response.cluster_results.sort_key = int(self.sort_key)
             response.is_enabled = False
 
-            clustered, transformed_poses, total_collected = self._run_clustering(
-                self._cluster_params()
+            clustered, total_collected, last_header = self._cluster_streams(
+                self._clustered_child_frame_ids, self._cluster_params(), publish_tfs=True
             )
             if not clustered:
                 response.is_cluster_success = False
                 return response
 
-            last_header = transformed_poses[-1].header
-            self._publish_results(
-                clustered, transformed_poses, self._clustered_child_frame_id
-            )
             fill_cluster_result_array(
                 response.cluster_results,
                 clustered,
@@ -178,20 +178,18 @@ def _stop_and_cluster(
         finally:
             self._cleanup_cluster_pose_publishers()
             self._reset_collection()
-            self._clustered_child_frame_id = ""
+            self._clustered_child_frame_ids = []
 
     def _periodic_cluster_tick(self) -> None:
         """Cluster the poses collected so far and publish a ClusterPoseResultArray."""
         if not self.enabled:
             return
-        clustered, transformed_poses, total_collected = self._run_clustering(
-            self._cluster_params()
+        clustered, total_collected, last_header = self._cluster_streams(
+            self._clustered_child_frame_ids, self._cluster_params(), publish_tfs=False
         )
         if not clustered:
             return
-        self._publish_cluster_pose_result_array(
-            clustered, total_collected, transformed_poses[-1].header
-        )
+        self._publish_cluster_pose_result_array(clustered, total_collected, last_header)
 
     def _publish_cluster_pose_result_array(
         self,

test/test_cluster_poses_action.py

@@ -57,7 +57,7 @@ def test_action_succeeds(rig):
     goal = ClusterPosesAction.Goal()
     goal.params.odom_topic = ODOM_TOPIC
     goal.params.pose_stamped_topics = [POSE_TOPIC]
-    goal.params.clustered_child_frame_id = "test/clustered_action"
+    goal.params.clustered_child_frame_ids = ["test/clustered_action"]
     goal.params.sync_tolerance = 0.05
     goal.params.min_poses = 5
     goal.params.min_cluster_size = 3

test/test_cluster_poses_service.py

@@ -92,7 +92,9 @@ def _base_enable_request(pose_topics, child_frame_id):
     req.enabled = True
     req.params.odom_topic = ODOM_TOPIC
     req.params.pose_stamped_topics = pose_topics
-    req.params.clustered_child_frame_id = child_frame_id
+    req.params.clustered_child_frame_ids = (
+        [child_frame_id] if isinstance(child_frame_id, str) else list(child_frame_id)
+    )
     req.params.sync_queue_size = 100
     req.params.sync_tolerance = 0.05
     req.params.min_poses = 5
@@ -219,3 +221,43 @@ def test_multiple_pose_topics_feed_same_buffer(multi_rig):
     for cluster in final.results:
         assert cluster.num_cluster_poses >= 3
         assert cluster.num_cluster_poses < total_input
+
+
+def test_independent_streams_one_frame_per_topic(multi_rig):
+    """One frame id per pose topic clusters each topic independently. With
+    top_k=1 we still get one cluster per topic, but each result's
+    num_input_poses reflects only its own topic, not the merged total."""
+    multi_rig["tf_pub"].publish(identity_tf_static())
+    spin_for(multi_rig["executor"], 0.2)
+
+    req = _base_enable_request(
+        [POSE_TOPIC_A, POSE_TOPIC_B],
+        ["test/clustered_a", "test/clustered_b"],
+    )
+    req.params.top_k = 1
+
+    enable_response = multi_rig["call_service"](req)
+    assert enable_response.is_enabled is True
+
+    multi_rig["publish_timer"].reset()
+    spin_for(multi_rig["executor"], 1.5)
+    multi_rig["publish_timer"].cancel()
+
+    disable_req = ClusterPosesSrv.Request()
+    disable_req.enabled = False
+    response = multi_rig["call_service"](disable_req)
+
+    assert response.is_cluster_success is True
+    final = response.cluster_results
+    assert len(final.results) == 2, (
+        f"expected one cluster per independent stream, got {len(final.results)}"
+    )
+
+    cluster_xs = sorted(r.clustered_pose.position.x for r in final.results)
+    assert abs(cluster_xs[0] - EXPECTED_CLUSTER_X) < 0.05
+    assert abs(cluster_xs[1] - SECONDARY_CLUSTER_X) < 0.05
+
+    # Each stream sees only its own topic's poses, so a single result's
+    # num_input_poses is well under the combined two-topic total.
+    for cluster in final.results:
+        assert cluster.num_cluster_poses >= 3