diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 20f30486fd3249168f6f80a9353d6dd78cc7b2ea..2b2fb5f9d4ea899930351b1234873b2354cb50bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -30,6 +30,8 @@ set(LIB_FILES
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
plugins/RequestedObjects.cpp
+
+ util.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -54,6 +56,8 @@ set(LIB_HEADERS
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
plugins/RequestedObjects.h
+
+ util.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index 8710af33c07522713bd833e53c7f1e982754e3ec..13de0bb80cccb9d9e7a63819907d3b7d81688fbd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -1,4 +1,6 @@
#include "ObjectPoseClient.h"
+#include <optional>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
namespace armarx::objpose
@@ -45,7 +47,8 @@ namespace armarx::objpose
}
- ObjectPoseMap ObjectPoseClient::fetchObjectPosesAsMap()
+ ObjectPoseMap
+ ObjectPoseClient::fetchObjectPosesAsMap()
{
ObjectPoseMap map;
for (auto& pose : fetchObjectPoses())
@@ -54,8 +57,24 @@ namespace armarx::objpose
}
return map;
}
+
- ObjectPoseSeq ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
+ std::optional<ObjectPose>
+ ObjectPoseClient::fetchObjectPose(const ObjectID& objectID)
+ {
+ const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+
+ if(object != nullptr)
+ {
+ return *object;
+ }
+
+ return std::nullopt;
+ }
+
+
+ ObjectPoseSeq
+ ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
{
if (!objectPoseStorage)
{
@@ -66,7 +85,6 @@ namespace armarx::objpose
}
-
const ObjectPoseStorageInterfacePrx&
ObjectPoseClient::getObjectPoseStorage() const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 33f9a6dd7b40fd933a451d50d093afc937a08635..a5b5fd18d7686e0cc46cd7e0154588ca3b13b40f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -1,6 +1,9 @@
#pragma once
+#include <optional>
+
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
@@ -16,28 +19,83 @@ namespace armarx::objpose
{
public:
+ /// Construct a disconnected client.
ObjectPoseClient();
- ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
- const ObjectFinder& finder = {});
-
- void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
-
- bool isConnected() const;
-
-
+ /// Construct a client and connect it to the object pose storage.
+ ObjectPoseClient(
+ const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder = {}
+ );
+
+ /**
+ * @brief Connect to the given object pose storage.
+ *
+ * This function can be used after default-constructing the client.
+ *
+ * @param objectPoseStorage The object pose storage.
+ */
+ void
+ connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+
+ /**
+ * @brief Indicate whether this client is connected to an object pose
+ * storage.
+ *
+ * That is, whether its proxy has been set via the constructor or
+ * `connect()`.
+ *
+ * If false, all `fetch*()` functions will return empty results.
+ *
+ * @return True if connected
+ */
+ bool
+ isConnected() const;
+
+
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses.
+ */
ObjectPoseSeq
fetchObjectPoses();
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses, with object ID as key.
+ */
ObjectPoseMap
fetchObjectPosesAsMap();
+ /**
+ * @brief Fetch the pose of a single object.
+ *
+ * This is a network call. If you need multiple object poses, use
+ * `fetchObjectPoses()` instead.
+ *
+ * @param objectID The object's ID.
+ * @return The object's pose, if known.
+ */
+ std::optional<ObjectPose>
+ fetchObjectPose(const ObjectID& objectID);
+
+ /**
+ * @brief Fetch object poses from a specific provider.
+ * @param providerName The provider's name.
+ * @return The object poses from that provider.
+ */
ObjectPoseSeq
fetchObjectPosesFromProvider(const std::string& providerName);
+ /**
+ * @brief Get the object pose storage's proxy.
+ */
const ObjectPoseStorageInterfacePrx&
getObjectPoseStorage() const;
+ /**
+ * @brief Get the internal object finder.
+ */
const ObjectFinder&
getObjectFinder() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dd90c141d81b77fcce1e0b91f8a5963d84a2726
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
@@ -0,0 +1,88 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+namespace armarx::objpose
+{
+
+ objpose::ObjectPoseSeq
+ filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlocklist)
+ {
+ const auto isBlacklisted = [&datasetBlocklist](const objpose::ObjectPose& objectPose)
+ {
+ const auto dataset = objectPose.objectID.dataset();
+
+ return std::find(datasetBlocklist.begin(), datasetBlocklist.end(), dataset) !=
+ datasetBlocklist.end();
+ };
+
+ objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+ return objects;
+ }
+
+
+ VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+
+ VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd8ebf0c8397ae1a26b899a8d48c1f9a303377f9
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.h
@@ -0,0 +1,36 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::objpose
+{
+ objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
+ const std::vector<std::string>& datasetBlocklist);
+
+ VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+
+} // namespace armarx::objpose