New Cluster API

bb_perception_msgs/CMakeLists.txt

@@ -31,12 +31,16 @@ set(message_files
 
   msg/ClusteredCloud.msg
   msg/ClusteredClouds.msg
+  msg/ClusterPosesRequest.msg
+  msg/ClusterPoseResult.msg
+  msg/ClusterPoseResultArray.msg
 
   msg/PointCorrespondencesStamped.msg
 )
 
 set(service_files
   srv/ArucoToggleActivation.srv
+  srv/ClusterPosesSrv.srv
   srv/ClusterTfSrv.srv
   srv/GetObjectCount.srv
   srv/GetPingCount.srv

bb_perception_msgs/action/ClusterPosesAction.action

@@ -1,18 +1,9 @@
 # Request
-string odom_topic
-string pose_stamped_topic
-string clustered_child_frame_id "clustered_object"  # Frame ID for the clustered transform
+ClusterPosesRequest params
 float64 collection_duration 5.0  # Duration in seconds to collect synchronized messages
-float64 sync_tolerance 0.1  # Time synchronization tolerance in seconds
-int32 min_poses 10  # Minimum number of synchronized poses required
-int32 min_cluster_size 2
-int32 min_samples 1
-float64 cluster_selection_epsilon 0.05
 ---
 # Result
-geometry_msgs/PoseStamped clustered_pose
-int32 total_poses_collected
-int32 poses_in_cluster
+ClusterPoseResultArray cluster_results
 ---
 # Feedback
 float64 collection_progress  # 0.0 to 1.0

bb_perception_msgs/msg/ClusterPoseResult.msg

@@ -0,0 +1,5 @@
+geometry_msgs/Pose clustered_pose
+float64 clustered_position_std
+int32 num_cluster_poses
+int32 num_input_poses
+float64 mean_probability

bb_perception_msgs/msg/ClusterPoseResultArray.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+
+# Key used to sort `results`. See ClusterPosesRequest.
+int32 sort_key
+
+ClusterPoseResult[] results

bb_perception_msgs/msg/ClusterPosesRequest.msg

@@ -0,0 +1,38 @@
+# Shared request fields used by both ClusterPosesSrv and ClusterPosesAction.
+
+# --- Subscriber configuration ---
+string odom_topic
+# Every listed PoseStamped topic feeds into the same accumulation buffer.
+# Each gets its own ApproximateTimeSynchronizer paired with `odom_topic`.
+string[] pose_stamped_topics
+int32 sync_queue_size 100
+float64 sync_tolerance 0.05
+
+# --- Output frame IDs ---
+# Base TF frame ID broadcast for each clustered result. Each of the top_k
+# results is broadcast as "<clustered_child_frame_id>_<i>" ordered by 
+# `sort_key`.
+string clustered_child_frame_id "clustered_object"
+
+# --- Clustering parameters ---
+int32 min_poses 10
+int32 min_cluster_size 5
+int32 min_samples 5
+float64 cluster_selection_epsilon 0.0
+# Drop earlier detections when latest_detection_time - detection_time
+# is greater than or equal to this value. Set <= 0.0 to disable age filtering.
+float64 max_detection_age_s 0.0
+
+# --- Result selection ---
+# Number of clusters to return (and TFs to broadcast). The output array is
+# truncated to this many entries after sorting. 0 means "all clusters found".
+int32 top_k 1
+
+# Sort key values for ordering the returned ClusterPoseResultArray. Each key
+# defines its own "best-first" direction.
+int32 SORT_BY_NUM_CLUSTER_POSES = 0       # largest first
+int32 SORT_BY_MEAN_PROBABILITY = 1        # highest first
+int32 SORT_BY_CLUSTERED_POSITION_STD = 2  # smallest first
+
+# How to sort the returned array
+int32 sort_key 0

bb_perception_msgs/srv/ClusterPosesSrv.srv

@@ -0,0 +1,17 @@
+# Service for cluster_poses_service_node.
+#
+# enabled=True  -> start synchronized accumulation using `params`.
+# enabled=False -> stop, cluster the accumulated poses, publish + TF-broadcast.
+#                  `params` is ignored on stop; the configuration baked in at
+#                  the previous enable=True call is used.
+bool enabled
+
+ClusterPosesRequest params
+# When > 0, run clustering on the poses collected so far every
+# `cluster_interval` seconds while enabled and publish a ClusterPoseResultArray.
+float64 cluster_interval 0.0
+
+---
+bool is_enabled
+bool is_cluster_success
+ClusterPoseResultArray cluster_results