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