From 4a7f7738531d7ec500590dbc022ba8782e2aea64 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 3 Dec 2021 18:11:30 +0100
Subject: [PATCH] Add ObjectPoseClient
---
.../libraries/ArmarXObjects/CMakeLists.txt | 2 +
.../ArmarXObjects/ObjectPoseClient.cpp | 73 +++++++++++++++++++
.../ArmarXObjects/ObjectPoseClient.h | 49 +++++++++++++
.../plugins/ObjectPoseClientPlugin.cpp | 7 ++
.../plugins/ObjectPoseClientPlugin.h | 5 ++
5 files changed, 136 insertions(+)
create mode 100644 source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
create mode 100644 source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 31fd00c7e..b2e71660a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -16,6 +16,7 @@ set(LIB_FILES
ObjectInfo.cpp
ObjectFinder.cpp
ObjectPose.cpp
+ ObjectPoseClient.cpp
json_conversions.cpp
ice_conversions.cpp
@@ -33,6 +34,7 @@ set(LIB_HEADERS
ObjectInfo.h
ObjectFinder.h
ObjectPose.h
+ ObjectPoseClient.h
json_conversions.h
ice_conversions.h
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
new file mode 100644
index 000000000..de740d244
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -0,0 +1,73 @@
+#include "ObjectPoseClient.h"
+
+
+namespace armarx::objpose
+{
+
+ ObjectPoseClient::ObjectPoseClient()
+ {
+ }
+
+
+ ObjectPoseClient::ObjectPoseClient(
+ const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder) :
+ objectFinder(finder)
+ {
+ this->connect(objectPoseStorage);
+ }
+
+
+ void
+ ObjectPoseClient::connect(
+ const ObjectPoseStorageInterfacePrx& objectPoseStorage)
+ {
+ this->objectPoseStorage = objectPoseStorage;
+ }
+
+
+ bool
+ ObjectPoseClient::isConnected() const
+ {
+ return bool(objectPoseStorage);
+ }
+
+
+ ObjectPoseSeq
+ ObjectPoseClient::fetchObjectPoses()
+ {
+ if (not objectPoseStorage)
+ {
+ ARMARX_WARNING << "No object pose observer.";
+ return {};
+ }
+ return fromIce(objectPoseStorage->getObjectPoses());
+ }
+
+
+ ObjectPoseMap ObjectPoseClient::fetchObjectPosesAsMap()
+ {
+ ObjectPoseMap map;
+ for (auto& pose : fetchObjectPoses())
+ {
+ map.emplace(pose.objectID, std::move(pose));
+ }
+ return map;
+ }
+
+
+ const ObjectPoseStorageInterfacePrx&
+ ObjectPoseClient::getObjectPoseStorage() const
+ {
+ return objectPoseStorage;
+ }
+
+
+ const ObjectFinder&
+ ObjectPoseClient::getObjectFinder() const
+ {
+ return objectFinder;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
new file mode 100644
index 000000000..2e262cc15
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+
+namespace armarx::objpose
+{
+
+ /**
+ * @brief Provides access to the `armarx::objpose::ObjectPoseStorageInterface`
+ * (aka the object memory).
+ */
+ class ObjectPoseClient
+ {
+ public:
+
+ ObjectPoseClient();
+ ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder = {});
+
+ void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+
+ bool isConnected() const;
+
+
+ ObjectPoseSeq
+ fetchObjectPoses();
+ ObjectPoseMap
+ fetchObjectPosesAsMap();
+
+
+ const ObjectPoseStorageInterfacePrx&
+ getObjectPoseStorage() const;
+
+ const ObjectFinder&
+ getObjectFinder() const;
+
+
+ public:
+
+ ObjectPoseStorageInterfacePrx objectPoseStorage;
+
+ ObjectFinder objectFinder;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 9bf87346d..a2b3ef473 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -55,6 +55,13 @@ namespace armarx
return plugin->createObjectPoseStorage();
}
+
+ objpose::ObjectPoseClient ObjectPoseClientPluginUser::getClient() const
+ {
+ return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder());
+ }
+
+
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
{
if (!objectPoseStorage)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 769d1176d..9f72ab1eb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -5,6 +5,7 @@
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
namespace armarx::plugins
@@ -73,6 +74,10 @@ namespace armarx
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseStorageInterfacePrx objectPoseStorage;
+
+ objpose::ObjectPoseClient getClient() const;
+
+
objpose::ObjectPoseSeq getObjectPoses();
plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
--
GitLab