Include FoundationPose detections in behavior scene.

calvertdw · calvertdw · commit 787a0c5b5462 · 2025-11-14T20:32:41.000-06:00
diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/scene/RDXBehaviorTreeScene.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/scene/RDXBehaviorTreeScene.java
@@ -19,7 +19,9 @@
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
+import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseInstantDetection;
 import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseObject;
+import us.ihmc.perception.detections.yolo.YOLOv8InstantDetection;
 import us.ihmc.rdx.RDX3DSituatedText;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
@@ -149,10 +151,16 @@ private void renderImGuiWidgets()
 
    private static void renderPersistentDetection(PersistentDetectionStatusMessage message)
    {
-      String text = "%s %.2f Hz Size: %d ID.%s".formatted(message.getObjectClassAsString(),
-                                                           message.getDecayingFrequency(),
-                                                           message.getHistorySize(),
-                                                           message.getIdAsString());
+      String type = "(?)";
+      if (message.getDetectionTypeAsString().equals(IsaacROSFoundationPoseInstantDetection.class.getSimpleName()))
+         type = "(FoundationPose)";
+      if (message.getDetectionTypeAsString().equals(YOLOv8InstantDetection.class.getSimpleName()))
+         type = "(YOLOv8)";
+      String text = "%s %s %.2f Hz Size: %d ID.%s".formatted(type,
+                                                             message.getObjectClassAsString(),
+                                                             message.getDecayingFrequency(),
+                                                             message.getHistorySize(),
+                                                             message.getIdAsString());
       if (message.getIsStable())
          ImGui.text(text);
       else
diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/BehaviorTreeSceneExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/BehaviorTreeSceneExecutor.java
@@ -51,7 +51,7 @@ public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,
       {
          IsaacROSFoundationPoseCommunicator mustardCommunicator = foundationPose.get(IsaacROSFoundationPoseObject.MUSTARD);
          mustardCommunicator.enable(true);
-//         mustardCommunicator.addResultCallback(instantDetectionQueue::add);
+         mustardCommunicator.addResultCallback(detection -> instantDetectionQueue.add(List.of(detection)));
       }
    }
 
diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/PersistentDetectionMessageTool.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/PersistentDetectionMessageTool.java
@@ -19,6 +19,7 @@ public void toMessage(ROS2SyncedRobotModel syncedRobot, Instant now, PersistentD
    {
       String last4Digits = String.format("%04d", Math.abs(persistentDetection.getID().getLeastSignificantBits() % 10000));
       message.setId(last4Digits);
+      message.setDetectionType(persistentDetection.getMostRecentDetection().getClass().getSimpleName());
       message.setObjectClass(persistentDetection.getDetectedObjectClass());
       message.setDecayingFrequency(persistentDetection.getDetectionFrequencyDecaying(now));
       message.setHistorySize(persistentDetection.getHistorySize());
diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/PersistentDetectionStatusMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/PersistentDetectionStatusMessage_.idl
@@ -19,6 +19,10 @@ module behavior_msgs
          * Last 4 digits of UUID as a string
          */
         string id;
+        /**
+         * Instant detection simple class name (e.g. YOLO, FoundationPose)
+         */
+        string detection_type;
         /**
          * Object class name (e.g., "bottle")
          */
diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeSceneObjectStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeSceneObjectStateMessagePubSubType.java
@@ -15,7 +15,7 @@ public class BehaviorTreeSceneObjectStateMessagePubSubType implements us.ihmc.pu
    @Override
    public final java.lang.String getDefinitionChecksum()
    {
-   		return "59f3ee4575c5da2b173882d0133c914881e2de3cd86237a2f6517024c9cf86ee";
+   		return "e940223f873a88665fe841f33f1757063f4481eb711220e7c17b1308e0473324";
    }
    
    @Override
diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeSceneStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeSceneStateMessagePubSubType.java
@@ -15,7 +15,7 @@ public class BehaviorTreeSceneStateMessagePubSubType implements us.ihmc.pubsub.T
    @Override
    public final java.lang.String getDefinitionChecksum()
    {
-   		return "122728bd60171e37f199997f54bd44adc02031acdd2ef58413fd3589f74fc285";
+   		return "1999b399810645f1d7a12a0760852f6eb6ebda0543c9cdce644001de4de3658d";
    }
    
    @Override
diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java
@@ -15,7 +15,7 @@ public class BehaviorTreeStateMessagePubSubType implements us.ihmc.pubsub.TopicD
    @Override
    public final java.lang.String getDefinitionChecksum()
    {
-   		return "838a7377a92ce1ec67ef85e387ffd1f7aaeecbaaa278e7f05cf5a57c918c085e";
+   		return "50bb84905811aae33d6d9e25e6fb7f2a14b1035b419e50086bc01bf3a11e641a";
    }
    
    @Override
diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/PersistentDetectionStatusMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/PersistentDetectionStatusMessage.java
@@ -15,6 +15,10 @@ public class PersistentDetectionStatusMessage extends Packet<PersistentDetection
             * Last 4 digits of UUID as a string
             */
    public java.lang.StringBuilder id_;
+   /**
+            * Instant detection simple class name (e.g. YOLO, FoundationPose)
+            */
+   public java.lang.StringBuilder detection_type_;
    /**
             * Object class name (e.g., "bottle")
             */
@@ -43,6 +47,7 @@ public class PersistentDetectionStatusMessage extends Packet<PersistentDetection
    public PersistentDetectionStatusMessage()
    {
       id_ = new java.lang.StringBuilder(255);
+      detection_type_ = new java.lang.StringBuilder(255);
       object_class_ = new java.lang.StringBuilder(255);
       transform_to_world_ = new controller_msgs.msg.dds.RigidBodyTransformMessage();
       transform_to_camera_ = new controller_msgs.msg.dds.RigidBodyTransformMessage();
@@ -59,6 +64,9 @@ public void set(PersistentDetectionStatusMessage other)
       id_.setLength(0);
       id_.append(other.id_);
 
+      detection_type_.setLength(0);
+      detection_type_.append(other.detection_type_);
+
       object_class_.setLength(0);
       object_class_.append(other.object_class_);
 
@@ -96,6 +104,30 @@ public java.lang.StringBuilder getId()
       return id_;
    }
 
+   /**
+            * Instant detection simple class name (e.g. YOLO, FoundationPose)
+            */
+   public void setDetectionType(java.lang.String detection_type)
+   {
+      detection_type_.setLength(0);
+      detection_type_.append(detection_type);
+   }
+
+   /**
+            * Instant detection simple class name (e.g. YOLO, FoundationPose)
+            */
+   public java.lang.String getDetectionTypeAsString()
+   {
+      return getDetectionType().toString();
+   }
+   /**
+            * Instant detection simple class name (e.g. YOLO, FoundationPose)
+            */
+   public java.lang.StringBuilder getDetectionType()
+   {
+      return detection_type_;
+   }
+
    /**
             * Object class name (e.g., "bottle")
             */
@@ -203,6 +235,8 @@ public boolean epsilonEquals(PersistentDetectionStatusMessage other, double epsi
 
       if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.id_, other.id_, epsilon)) return false;
 
+      if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.detection_type_, other.detection_type_, epsilon)) return false;
+
       if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.object_class_, other.object_class_, epsilon)) return false;
 
       if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.decaying_frequency_, other.decaying_frequency_, epsilon)) return false;
@@ -228,6 +262,8 @@ public boolean equals(Object other)
 
       if (!us.ihmc.idl.IDLTools.equals(this.id_, otherMyClass.id_)) return false;
 
+      if (!us.ihmc.idl.IDLTools.equals(this.detection_type_, otherMyClass.detection_type_)) return false;
+
       if (!us.ihmc.idl.IDLTools.equals(this.object_class_, otherMyClass.object_class_)) return false;
 
       if(this.decaying_frequency_ != otherMyClass.decaying_frequency_) return false;
@@ -250,6 +286,8 @@ public java.lang.String toString()
       builder.append("PersistentDetectionStatusMessage {");
       builder.append("id=");
       builder.append(this.id_);      builder.append(", ");
+      builder.append("detection_type=");
+      builder.append(this.detection_type_);      builder.append(", ");
       builder.append("object_class=");
       builder.append(this.object_class_);      builder.append(", ");
       builder.append("decaying_frequency=");
diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/PersistentDetectionStatusMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/PersistentDetectionStatusMessagePubSubType.java
@@ -15,7 +15,7 @@ public class PersistentDetectionStatusMessagePubSubType implements us.ihmc.pubsu
    @Override
    public final java.lang.String getDefinitionChecksum()
    {
-   		return "8056427588c7403fb501d01e29914d9e9697c32902d4f0d896bcc3c3ac936615";
+   		return "0f320bdc9036f03ba8aaa3d0dbd41d80d15abfc1a1e1ab7d54d2cefbf44908cb";
    }
    
    @Override
@@ -52,6 +52,7 @@ public static int getMaxCdrSerializedSize(int current_alignment)
    {
       int initial_alignment = current_alignment;
 
+      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
       current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
       current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1;
       current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);
@@ -79,6 +80,8 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.PersistentDet
 
       current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getId().length() + 1;
 
+      current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getDetectionType().length() + 1;
+
       current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getObjectClass().length() + 1;
 
       current_alignment += 8 + us.ihmc.idl.CDR.alignment(current_alignment, 8);
@@ -104,6 +107,10 @@ public static void write(behavior_msgs.msg.dds.PersistentDetectionStatusMessage
       cdr.write_type_d(data.getId());else
           throw new RuntimeException("id field exceeds the maximum length: %d > %d".formatted(data.getId().length(), 255));
 
+      if(data.getDetectionType().length() <= 255)
+      cdr.write_type_d(data.getDetectionType());else
+          throw new RuntimeException("detection_type field exceeds the maximum length: %d > %d".formatted(data.getDetectionType().length(), 255));
+
       if(data.getObjectClass().length() <= 255)
       cdr.write_type_d(data.getObjectClass());else
           throw new RuntimeException("object_class field exceeds the maximum length: %d > %d".formatted(data.getObjectClass().length(), 255));
@@ -121,6 +128,7 @@ public static void write(behavior_msgs.msg.dds.PersistentDetectionStatusMessage
    public static void read(behavior_msgs.msg.dds.PersistentDetectionStatusMessage data, us.ihmc.idl.CDR cdr)
    {
       cdr.read_type_d(data.getId());	
+      cdr.read_type_d(data.getDetectionType());	
       cdr.read_type_d(data.getObjectClass());	
       data.setDecayingFrequency(cdr.read_type_6());
       	
@@ -137,6 +145,7 @@ public static void read(behavior_msgs.msg.dds.PersistentDetectionStatusMessage d
    public final void serialize(behavior_msgs.msg.dds.PersistentDetectionStatusMessage data, us.ihmc.idl.InterchangeSerializer ser)
    {
       ser.write_type_d("id", data.getId());
+      ser.write_type_d("detection_type", data.getDetectionType());
       ser.write_type_d("object_class", data.getObjectClass());
       ser.write_type_6("decaying_frequency", data.getDecayingFrequency());
       ser.write_type_2("history_size", data.getHistorySize());
@@ -151,6 +160,7 @@ public final void serialize(behavior_msgs.msg.dds.PersistentDetectionStatusMessa
    public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.PersistentDetectionStatusMessage data)
    {
       ser.read_type_d("id", data.getId());
+      ser.read_type_d("detection_type", data.getDetectionType());
       ser.read_type_d("object_class", data.getObjectClass());
       data.setDecayingFrequency(ser.read_type_6("decaying_frequency"));
       data.setHistorySize(ser.read_type_2("history_size"));
diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/PersistentDetectionStatusMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/PersistentDetectionStatusMessage.msg
@@ -3,6 +3,9 @@
 # Last 4 digits of UUID as a string
 string id
 
+# Instant detection simple class name (e.g. YOLO, FoundationPose)
+string detection_type
+
 # Object class name (e.g., "bottle")
 string object_class
 

Original file line number	Diff line number	Diff line change
`@@ -51,7 +51,7 @@ public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,`
`51`	`51`	`{`
`52`	`52`	`IsaacROSFoundationPoseCommunicator mustardCommunicator = foundationPose.get(IsaacROSFoundationPoseObject.MUSTARD);`
`53`	`53`	`mustardCommunicator.enable(true);`
`54`		`-// mustardCommunicator.addResultCallback(instantDetectionQueue::add);`
	`54`	`+ mustardCommunicator.addResultCallback(detection -> instantDetectionQueue.add(List.of(detection)));`
`55`	`55`	`}`
`56`	`56`	`}`
`57`	`57`
Original file line number	Diff line number	Diff line change
`@@ -19,6 +19,7 @@ public void toMessage(ROS2SyncedRobotModel syncedRobot, Instant now, PersistentD`
`19`	`19`	`{`
`20`	`20`	`String last4Digits = String.format("%04d", Math.abs(persistentDetection.getID().getLeastSignificantBits() % 10000));`
`21`	`21`	`message.setId(last4Digits);`
	`22`	`+ message.setDetectionType(persistentDetection.getMostRecentDetection().getClass().getSimpleName());`
`22`	`23`	`message.setObjectClass(persistentDetection.getDetectedObjectClass());`
`23`	`24`	`message.setDecayingFrequency(persistentDetection.getDetectionFrequencyDecaying(now));`
`24`	`25`	`message.setHistorySize(persistentDetection.getHistorySize());`
Original file line number	Diff line number	Diff line change
`@@ -15,7 +15,7 @@ public class BehaviorTreeSceneObjectStateMessagePubSubType implements us.ihmc.pu`
`15`	`15`	`@Override`
`16`	`16`	`public final java.lang.String getDefinitionChecksum()`
`17`	`17`	`{`
`18`		`- return "59f3ee4575c5da2b173882d0133c914881e2de3cd86237a2f6517024c9cf86ee";`
	`18`	`+ return "e940223f873a88665fe841f33f1757063f4481eb711220e7c17b1308e0473324";`
`19`	`19`	`}`
`20`	`20`
`21`	`21`	`@Override`