Merge branch 'feature/ObjectPoseClientSingleObject' into 'master'

Feature/object pose client extension

See merge request ArmarX/RobotAPI!225

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}")

source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp

 #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
     {

source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h

 #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;
 

source/RobotAPI/libraries/ArmarXObjects/util.cpp

+/**
+ * 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

source/RobotAPI/libraries/ArmarXObjects/util.h

+/**
+ * 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