diff --git a/bb_perception_msgs/CMakeLists.txt b/bb_perception_msgs/CMakeLists.txt index 71ac67d..a44263c 100644 --- a/bb_perception_msgs/CMakeLists.txt +++ b/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 diff --git a/bb_perception_msgs/action/ClusterPosesAction.action b/bb_perception_msgs/action/ClusterPosesAction.action index b186adc..6341359 100644 --- a/bb_perception_msgs/action/ClusterPosesAction.action +++ b/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 diff --git a/bb_perception_msgs/msg/ClusterPoseResult.msg b/bb_perception_msgs/msg/ClusterPoseResult.msg new file mode 100644 index 0000000..c362e1b --- /dev/null +++ b/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 diff --git a/bb_perception_msgs/msg/ClusterPoseResultArray.msg b/bb_perception_msgs/msg/ClusterPoseResultArray.msg new file mode 100644 index 0000000..1044ff3 --- /dev/null +++ b/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 diff --git a/bb_perception_msgs/msg/ClusterPosesRequest.msg b/bb_perception_msgs/msg/ClusterPosesRequest.msg new file mode 100644 index 0000000..862ee57 --- /dev/null +++ b/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 "_" 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 diff --git a/bb_perception_msgs/srv/ClusterPosesSrv.srv b/bb_perception_msgs/srv/ClusterPosesSrv.srv new file mode 100644 index 0000000..03c805d --- /dev/null +++ b/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