From 6e5bb072621fbd41a8682922832ea5995fc25558 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 12 Oct 2020 13:42:33 +0200
Subject: [PATCH] Add attachment to ObjectPose interface

---
 .../ObjectPoseObserver/ObjectPoseObserver.cpp | 26 ++++++++++-
 .../ObjectPoseObserver/ObjectPoseObserver.h   | 13 +++++-
 .../objectpose/ObjectPoseObserver.ice         | 45 +++++++++++++++++++
 .../objectpose/object_pose_types.ice          | 13 ++++++
 4 files changed, 94 insertions(+), 3 deletions(-)

diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 892094431..b392b3efd 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -156,6 +156,7 @@ namespace armarx
         updateProviderInfo(providerName, info);
     }
 
+
     void ObjectPoseObserver::reportObjectPoses(
         const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
     {
@@ -334,7 +335,6 @@ namespace armarx
         return output;
     }
 
-
     objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
     {
         std::scoped_lock lock(dataMutex);
@@ -373,6 +373,30 @@ namespace armarx
     }
 
 
+
+    objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+    {
+        objpose::AttachObjectToRobotNodeOutput output;
+
+        return output;
+    }
+
+    objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+    {
+        objpose::DetachObjectFromRobotNodeOutput output;
+
+        return output;
+    }
+
+    objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
+    {
+        objpose::DetachAllObjectsFromRobotNodesOutput output;
+
+        return output;
+    }
+
+
+
     void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
     {
         // Initialized to 0 on first access.
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 687db8ec0..cdd5d19ec 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -88,17 +88,26 @@ namespace armarx
         // ObjectPoseObserverInterface interface
     public:
 
+        // READING
+
         objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
         objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
 
-        objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
         Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
         objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
         objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
         bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
 
 
+        // MODIFICATION
+
+        objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+        objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
+        objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
+        objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(ICE_CURRENT_ARG) override;
+
+
         //Ice::Int getUpdateCounterByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
         //StringIntDictionary getAllUpdateCounters(ICE_CURRENT_ARG) override;
 
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 0333c21c5..9004c2afc 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -59,6 +59,44 @@ module armarx
             };
         };
 
+        struct AttachObjectToRobotNodeInput
+        {
+            string providerName;
+            armarx::data::ObjectID objectID;
+
+            /// The frame (robot node) to attach to.
+            string frameName;
+            /// The agent's name.
+            string agentName;
+
+            /**
+             * If given, specifies the object's pose in the frame.
+             * If not given, the current object's pose is used.
+             */
+            PoseBase poseInFrame;
+        };
+        struct AttachObjectToRobotNodeOutput
+        {
+            bool success;
+            data::ObjectAttachmentInfo attachment;
+        };
+
+        struct DetachObjectFromRobotNodeInput
+        {
+            string providerName;
+            armarx::data::ObjectID objectID;
+        };
+        struct DetachObjectFromRobotNodeOutput
+        {
+            /// Whether the object was attached before.
+            bool wasAttached;
+        };
+        struct DetachAllObjectsFromRobotNodesOutput
+        {
+            /// Number of objects that have been detached.
+            int numDetached;
+        };
+
         interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
         {
             // Reading
@@ -74,6 +112,13 @@ module armarx
             // Modifying
             observer::RequestObjectsOutput requestObjects(observer::RequestObjectsInput input);
 
+            /// Attach an object to a robot node.
+            AttachObjectToRobotNodeOutput attachObjectToRobotNode(AttachObjectToRobotNodeInput input);
+            /// Detach an attached object from a robot node.
+            DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
+            /// Detach all objects from robot nodes.
+            DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+
         };
     };
 
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index d6b4d7a6e..66618485d 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -83,6 +83,8 @@ module armarx
             sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
 
 
+            class ObjectAttachmentInfo;
+
             /// An object pose as stored by the ObjectPoseObserver.
             struct ObjectPose
             {
@@ -102,6 +104,8 @@ module armarx
                 StringFloatDictionary robotConfig;
                 PoseBase robotPose;
 
+                ObjectAttachmentInfo attachment;
+
                 /// Confidence in [0, 1] (1 = full, 0 = none).
                 float confidence = 0;
                 /// Source timestamp.
@@ -111,6 +115,15 @@ module armarx
                 Box localOOBB;
             };
             sequence<ObjectPose> ObjectPoseSeq;
+
+
+            class ObjectAttachmentInfo
+            {
+                string frameName;
+                string agentName;
+
+                PoseBase poseInFrame;
+            };
         }
 
     };
-- 
GitLab