Refactor code to own class

source/RobotAPI/components/armem/client/RobotAndObjectToIndex/CMakeLists.txt

@@ -14,10 +14,12 @@ set(COMPONENT_LIBS
 
 set(SOURCES
     RobotAndObjectToIndex.cpp
+    ObjectInstanceToIndex.cpp
 )
 
 set(HEADERS
     RobotAndObjectToIndex.h
+    ObjectInstanceToIndex.h
 )
 
 armarx_add_component("${SOURCES}" "${HEADERS}")

source/RobotAPI/components/armem/client/RobotAndObjectToIndex/ObjectInstanceToIndex.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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotAndObjectToIndex
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "ObjectInstanceToIndex.h"
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/armem_objects/utils.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+namespace armarx
+{
+
+    void
+    ObjectInstanceToIndex::fetchAndCommitObjectInstances(
+        const std::vector<armem::MemoryID>& objectPoseSnapshotIDs)
+    {
+        ARMARX_CHECK(client.isConnected());
+
+        // Fetch the latest poses.
+        const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses();
+
+        // ToDo: Filter by snapshotIDs
+
+        // Prepare the commit.
+
+        armem::Commit commit;
+        for (const objpose::ObjectPose& objectPose : objectPoses)
+        {
+            std::optional<simox::OrientedBoxf> oobb = objectPose.oobbGlobal();
+            if (oobb.has_value())
+            {
+                armem::MemoryID id = armem::objects::reconstructObjectInstanceID(objectPose);
+
+                simox::AxisAlignedBoundingBox aabb =
+                    simox::AxisAlignedBoundingBox::from_points(oobb->corners());
+
+                armem::index::arondto::Spatial spatial;
+                armem::toAron(spatial.id, id);
+                toAron(spatial.oobbGlobal, oobb);
+                toAron(spatial.aabbGlobal, aabb);
+
+                armem::EntityUpdate& update = commit.add();
+                update.entityID = indexProviderSegmentID.withEntityName(id.getEntityID().str());
+                update.timeCreated = objectPose.timestamp;
+                update.instancesData = {spatial.toAron()};
+            }
+        }
+
+        // Commit.
+        indexSpatialMemoryWriter.commit(commit);
+    }
+
+} // namespace armarx

source/RobotAPI/components/armem/client/RobotAndObjectToIndex/ObjectInstanceToIndex.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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotAndObjectToIndex
+ * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx
+{
+
+    class ObjectInstanceToIndex
+    {
+    public:
+        void
+        fetchAndCommitObjectInstances(const std::vector<armem::MemoryID>& objectPoseSnapshotIDs);
+
+
+    public:
+        armem::MemoryID indexProviderSegmentID;
+
+        objpose::ObjectPoseClient client;
+        armem::client::Writer indexSpatialMemoryWriter;
+    };
+
+} // namespace armarx

source/RobotAPI/components/armem/client/RobotAndObjectToIndex/RobotAndObjectToIndex.cpp

@@ -35,8 +35,11 @@
 #include <RobotAPI/libraries/armem/core/operations.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/memory_ids.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 
@@ -44,11 +47,6 @@
 namespace armarx
 {
 
-    static const armem::MemoryID indexSpatialSegmentID{"Index", "Spatial"};
-    static const armem::MemoryID robotStateSegmentID{"RobotState", "Spatial"};
-    static const armem::MemoryID objectInstanceSegmentID{"Object", "Instance"};
-
-
     armarx::PropertyDefinitionsPtr
     RobotAndObjectToIndex::createPropertyDefinitions()
     {
@@ -61,7 +59,7 @@ namespace armarx
 
     RobotAndObjectToIndex::RobotAndObjectToIndex()
     {
-        addPlugin(object.clientPlugin);
+        addPlugin(objectClientPlugin);
     }
 
     std::string
@@ -78,8 +76,9 @@ namespace armarx
         // This should not be necessary but seems to be. ToDo: Look into this.
         memoryNameSystem().setComponent(this);
 
-        memoryNameSystem().subscribe(robotStateSegmentID, this, &This::processRobotState);
-        memoryNameSystem().subscribe(objectInstanceSegmentID, this, &This::processObjectInstance);
+        memoryNameSystem().subscribe(armem::robot_state::memoryID, this, &This::processRobotState);
+        memoryNameSystem().subscribe(
+            armem::objects::instaceSegmentID, this, &This::processObjectInstance);
     }
 
 
@@ -88,7 +87,7 @@ namespace armarx
     {
         try
         {
-            indexSpatialMemoryWriter = memoryNameSystem().useWriter(indexSpatialSegmentID);
+            indexSpatialMemoryWriter = memoryNameSystem().useWriter(armem::index::spatialSegmentID);
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
         {
@@ -208,61 +207,22 @@ namespace armarx
 #endif
     }
 
-    armem::MemoryID
-    reconstructObjectInstanceID(const objpose::ObjectPose& objectPose)
-    {
-        armem::MemoryID id =
-            objectInstanceSegmentID.withProviderSegmentName(objectPose.providerName)
-                .withEntityName(objectPose.objectID.str())
-                .withTimestamp(objectPose.timestamp)
-                .withInstanceIndex(0);
-        return id;
-    }
 
     void
     RobotAndObjectToIndex::processObjectInstance(const armem::MemoryID& id,
                                                  const std::vector<armem::MemoryID>& snapshotIDs)
     {
-        ARMARX_INFO << "Fetching object instances.";
-        if (not object.client.has_value())
+        if (not object.has_value())
         {
-            object.client = object.clientPlugin->createClient();
-        }
-        ARMARX_CHECK(object.client.has_value());
-
-        // Fetch the latest poses.
-        const objpose::ObjectPoseSeq objectPoses = object.client->fetchObjectPoses();
-
-        // ToDo: Filter by snapshotIDs
-
-        // Prepare the commit.
-        armem::MemoryID provSegID = indexSpatialSegmentID.withProviderSegmentName(getName());
-
-        armem::Commit commit;
-        for (const objpose::ObjectPose& objectPose : objectPoses)
-        {
-            std::optional<simox::OrientedBoxf> oobb = objectPose.oobbGlobal();
-            if (oobb.has_value())
-            {
-                armem::MemoryID id = reconstructObjectInstanceID(objectPose);
-
-                simox::AxisAlignedBoundingBox aabb =
-                    simox::AxisAlignedBoundingBox::from_points(oobb->corners());
-
-                armem::index::arondto::Spatial spatial;
-                armem::toAron(spatial.id, id);
-                toAron(spatial.oobbGlobal, oobb);
-                toAron(spatial.aabbGlobal, aabb);
-
-                armem::EntityUpdate& update = commit.add();
-                update.entityID = provSegID.withEntityName(id.getEntityID().str());
-                update.timeCreated = objectPose.timestamp;
-                update.instancesData = {spatial.toAron()};
-            }
+            object = ObjectInstanceToIndex{
+                .indexProviderSegmentID =
+                    armem::index::spatialSegmentID.withProviderSegmentName(getName()),
+                .client = objectClientPlugin->createClient(),
+                .indexSpatialMemoryWriter = indexSpatialMemoryWriter};
         }
+        ARMARX_CHECK(object.has_value());
 
-        // Commit.
-        indexSpatialMemoryWriter.commit(commit);
+        object->fetchAndCommitObjectInstances(snapshotIDs);
     }
 
 } // namespace armarx

source/RobotAPI/components/armem/client/RobotAndObjectToIndex/RobotAndObjectToIndex.h

@@ -22,18 +22,31 @@
 
 #pragma once
 
-#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <ArmarXCore/core/Component.h>
 
 #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
-#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
 
 
 namespace armarx
 {
+    class ObjectInstanceToIndex
+    {
+    public:
+        void
+        fetchAndCommitObjectInstances(const std::vector<armem::MemoryID>& objectPoseSnapshotIDs);
+
+
+    public:
+        armem::MemoryID indexProviderSegmentID;
+        objpose::ObjectPoseClient client;
+        armem::client::Writer indexSpatialMemoryWriter;
+    };
+
 
     /**
      * @defgroup Component-RobotAndObjectToIndex RobotAndObjectToIndex
@@ -53,14 +66,12 @@ namespace armarx
         virtual public armarx::LightweightRemoteGuiComponentPluginUser
     {
     public:
-
         RobotAndObjectToIndex();
 
         /// @see armarx::ManagedIceObject::getDefaultName()
         std::string getDefaultName() const override;
 
     protected:
-
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
         void onInitComponent() override;
@@ -78,16 +89,16 @@ namespace armarx
 
 
     private:
-
-        void processRobotState(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
-        void processObjectInstance(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
+        void processRobotState(const armem::MemoryID& id,
+                               const std::vector<armem::MemoryID>& snapshotIDs);
+        void processObjectInstance(const armem::MemoryID& id,
+                                   const std::vector<armem::MemoryID>& snapshotIDs);
 
         // Examples
         void waitForMemory();
 
 
     private:
-
         struct Properties
         {
         };
@@ -96,14 +107,8 @@ namespace armarx
         armem::client::Reader robotMemoryReader;
         armem::client::Writer indexSpatialMemoryWriter;
 
-        struct ObjectInstance
-        {
-            armarx::plugins::ObjectPoseClientPlugin* clientPlugin = nullptr;
-            std::optional<objpose::ObjectPoseClient> client;
-        };
-        ObjectInstance object;
-
-        // armem::client::Reader objectMemoryReader;
+        armarx::plugins::ObjectPoseClientPlugin* objectClientPlugin = nullptr;
+        std::optional<ObjectInstanceToIndex> object;
 
         // armarx::RunningTask<RobotAndObjectToIndex>::pointer_type task;
 
@@ -112,4 +117,4 @@ namespace armarx
         };
         RemoteGuiTab tab;
     };
-}
+} // namespace armarx