From 60cbd733b355ff10c04c556de0ddb12663b29e1c Mon Sep 17 00:00:00 2001
From: Leroy Ruegemer <lruegeme@techfak.uni-bielefeld.de>
Date: Sat, 6 Apr 2024 20:15:28 +0200
Subject: [PATCH] adapter_ros: add debugging for bb3d serializer

---
 .../vision/BoundingBox3DSerializer.java       |  7 +++++
 .../vision/Detection3DSerializer.java         | 28 +++++++++----------
 2 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/BoundingBox3DSerializer.java b/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/BoundingBox3DSerializer.java
index 032dc848..68655e06 100644
--- a/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/BoundingBox3DSerializer.java
+++ b/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/BoundingBox3DSerializer.java
@@ -6,6 +6,7 @@
 import de.unibi.citec.clf.btl.ros.MsgTypeFactory;
 import de.unibi.citec.clf.btl.ros.RosSerializer;
 import de.unibi.citec.clf.btl.units.LengthUnit;
+import org.apache.log4j.Logger;
 import org.ros.message.MessageFactory;
 
 /**
@@ -13,6 +14,8 @@
  */
 public class BoundingBox3DSerializer extends RosSerializer<BoundingBox3D, vision_msgs.BoundingBox3D> {
 
+    private Logger logger = Logger.getLogger(getClass());
+
     @Override
     public vision_msgs.BoundingBox3D serialize(BoundingBox3D data, MessageFactory fact) throws SerializationException {
         vision_msgs.BoundingBox3D msg = fact.newFromType(vision_msgs.BoundingBox3D._TYPE);
@@ -29,9 +32,13 @@ public vision_msgs.BoundingBox3D serialize(BoundingBox3D data, MessageFactory fa
     public BoundingBox3D deserialize(vision_msgs.BoundingBox3D msg) throws DeserializationException {
         BoundingBox3D data = new BoundingBox3D();
 
+        logger.debug("deserialize BoundingBox3D center.pose.x=" + msg.getCenter().getPosition().getX());
+
         data.setPose(MsgTypeFactory.getInstance().createType(msg.getCenter(),Pose3D.class));
         data.setSize(new Point3D(msg.getSize().getX(),msg.getSize().getY(),msg.getSize().getZ(),LengthUnit.METER));
 
+        logger.debug("deserialized BoundingBox3D bb.pose.x=" + data.getPose().getTranslation().getX(LengthUnit.METER));
+
         return data;
     }
 
diff --git a/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/Detection3DSerializer.java b/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/Detection3DSerializer.java
index f60e703f..86e56368 100644
--- a/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/Detection3DSerializer.java
+++ b/bonsai_adapter_ros/src/main/java/de/unibi/citec/clf/btl/ros/serializers/vision/Detection3DSerializer.java
@@ -6,6 +6,7 @@
 import de.unibi.citec.clf.btl.ros.MsgTypeFactory;
 import de.unibi.citec.clf.btl.ros.RosSerializer;
 import de.unibi.citec.clf.btl.units.LengthUnit;
+import org.apache.log4j.Logger;
 import org.ros.message.MessageFactory;
 import vision_msgs.Detection3D;
 import vision_msgs.ObjectHypothesisWithPose;
@@ -18,6 +19,8 @@
  */
 public class Detection3DSerializer extends RosSerializer<ObjectShapeData, vision_msgs.Detection3D> {
 
+    private Logger logger = Logger.getLogger(getClass());
+
     final LengthUnit lum = LengthUnit.METER;
     @Override
     public vision_msgs.Detection3D serialize(ObjectShapeData data, MessageFactory fact) throws SerializationException {
@@ -41,26 +44,23 @@ public vision_msgs.Detection3D serialize(ObjectShapeData data, MessageFactory fa
 
     @Override
     public ObjectShapeData deserialize(vision_msgs.Detection3D msg) throws DeserializationException {
-        MsgTypeFactory fac = MsgTypeFactory.getInstance();
+        ObjectShapeData data = new ObjectShapeData();
 
+        // OSD header
+        MsgTypeFactory.setHeader(data,msg.getHeader());
 
-        ObjectShapeData data = new ObjectShapeData();
-        fac.setHeader(data,msg.getHeader());
+        // Bounding box
+        logger.debug("deserialize Detection3D bb.c.pose.x=" + msg.getBbox().getCenter().getPosition().getX());
+        data.setBoundingBox(MsgTypeFactory.getInstance().createType(msg.getBbox(),BoundingBox3D.class));
+        MsgTypeFactory.setHeader(data.getBoundingBox(),msg.getHeader());
+        MsgTypeFactory.setHeader(data.getBoundingBox().getPose(),msg.getHeader());
+        logger.debug("deserialized Detection3D osd.bb.c.pose.x=" + data.getCenter().getX(lum));
 
+        // Hypotheses
         for(ObjectHypothesisWithPose hyp : msg.getResults()) {
-            data.addHypothesis(fac.createType(hyp,ObjectData.Hypothesis.class));
+            data.addHypothesis(MsgTypeFactory.getInstance().createType(hyp,ObjectData.Hypothesis.class));
         }
 
-        data.setBoundingBox(fac.createType(msg.getBbox(),BoundingBox3D.class));
-        fac.setHeader(data.getBoundingBox(),msg.getHeader());
-        fac.setHeader(data.getBoundingBox().getPose(),msg.getHeader());
-
-        data.getCenter().setX(data.getBoundingBox().getPose().getTranslation().getX(lum),lum);
-        data.getCenter().setY(data.getBoundingBox().getPose().getTranslation().getY(lum),lum);
-        data.getCenter().setZ(data.getBoundingBox().getPose().getTranslation().getZ(lum),lum);
-        data.getCenter().setFrameId(data.getFrameId());
-
-
         //TODO
         //data.setId();