Merge branch 'aruco_object_memory' into 'master'

Aruco object memory

See merge request !289

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp

@@ -32,6 +32,7 @@
 #include <RobotAPI/libraries/armem/client/query.h>
 #include <RobotAPI/libraries/armem/core/Prediction.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
 
 
 namespace armarx::armem::server::obj
@@ -67,6 +68,9 @@ namespace armarx::armem::server::obj
                        "Duration of time window into the past to use for predictions"
                        " when requested via the PredictingMemoryInterface (in seconds).");
 
+        defs->optional(p.markerMemoryName, prefix + ".marker.Name", "Marker Memory Name");
+        defs->optional(p.maxMarkerHistorySize, prefix + ".marker.maxHistorySize", "Maximum marker memory history size");
+
         return defs;
     }
 
@@ -126,6 +130,13 @@ namespace armarx::armem::server::obj
         {
             attachmentSegment.init();
         });
+
+        initSegmentWithCatch(p.markerMemoryName, [&]()
+        {
+            workingMemory()
+              .addCoreSegment(p.markerMemoryName, arondto::Marker::ToAronType())
+              .setMaxHistorySize(p.maxMarkerHistorySize);
+        });
     }
 
 

source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h

@@ -134,6 +134,12 @@ namespace armarx::armem::server::obj
         };
         std::unique_ptr<RemoteGuiTab> tab;
 
+        struct Properties {
+            int64_t maxMarkerHistorySize = -1;
+            std::string markerMemoryName = "Marker";
+        };
+        Properties p;
+
         armem::client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>* virtualRobotReaderPlugin;
     };
 

source/RobotAPI/libraries/armem_objects/CMakeLists.txt

@@ -48,6 +48,7 @@ armarx_add_library(
 
     SOURCES
         aron_conversions.cpp
+        types.cpp
 
         client/articulated_object/Reader.cpp
         client/articulated_object/Writer.cpp
@@ -67,6 +68,8 @@ armarx_add_library(
 
         aron/Attachment.xml
         # aron/Constraint.xml
+
+        aron/Marker.xml
 )
 
 

source/RobotAPI/libraries/armem_objects/aron/Marker.xml

+<!--
+Core segment type of Object/Marker
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <AronIncludes>
+      <Include include="<RobotAPI/libraries/aron/common/aron/framed.xml>" autoinclude="true" />
+    </AronIncludes>
+    <GenerateTypes>
+        <Object name="armarx::armem::arondto::Marker">
+            <ObjectChild key="name">
+                <String />
+            </ObjectChild>
+            <ObjectChild key="markerPose">
+                <armarx::arondto::FramedPose />
+            </ObjectChild>
+            <ObjectChild key="markerGlobal">
+                <armarx::arondto::FramedPose />
+            </ObjectChild>
+            <ObjectChild key="robotGlobal">
+                <armarx::arondto::FramedPose />
+            </ObjectChild>
+            <ObjectChild key="rgbCamera">
+                <armarx::arondto::FramedPose />
+            </ObjectChild>
+            <ObjectChild key="depthCamera">
+                <armarx::arondto::FramedPose />
+            </ObjectChild>
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>
+

source/RobotAPI/libraries/armem_objects/aron_conversions.cpp

 #include "aron_conversions.h"
 
 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
 namespace armarx::armem
 {
 
-    void fromAron(const arondto::ObjectInstance& dto,
-                  objpose::arondto::ObjectPose& bo)
+    void
+    fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
     {
         bo = dto.pose;
     }
 
-    void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+    void
+    toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
     {
         dto.pose = bo;
     }
 
-    void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+    void
+    fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
     {
         objpose::fromAron(dto.pose, bo);
     }
 
-    void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+    void
+    toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
     {
         objpose::toAron(dto.pose, bo);
     }
 
 
     /* Attachments */
-    void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+    void
+    fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
     {
         fromAron(dto.id, bo.id);
         aron::fromAron(dto.frame, bo.frame);
     }
 
-    void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
+    void
+    toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
     {
         toAron(dto.id, bo.id);
         aron::toAron(dto.frame, bo.frame);
     }
 
 
-    void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
+    void
+    fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
     {
         fromAron(dto.agent, bo.agent);
         aron::fromAron(dto.transformation, bo.transformation);
@@ -53,7 +58,8 @@ namespace armarx::armem
         // TODO aron::fromAron(dto.timestamp, bo.timestamp);
     }
 
-    void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+    void
+    toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
     {
         toAron(dto.agent, bo.agent);
         aron::toAron(dto.transformation, bo.transformation);
@@ -63,7 +69,9 @@ namespace armarx::armem
     }
 
 
-    void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo)
+    void
+    fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+             attachment::ArticulatedObjectAttachment& bo)
     {
         fromAron(dto.agent, bo.agent);
         aron::fromAron(dto.transformation, bo.transformation);
@@ -72,7 +80,9 @@ namespace armarx::armem
         // TODO aron::fromAron(dto.timestamp, bo.timestamp);
     }
 
-    void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo)
+    void
+    toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+           const attachment::ArticulatedObjectAttachment& bo)
     {
         toAron(dto.agent, bo.agent);
         aron::toAron(dto.transformation, bo.transformation);
@@ -81,16 +91,34 @@ namespace armarx::armem
         // TODO aron::toAron(dto.timestamp, bo.timestamp);
     }
 
+    void
+    toAron(arondto::Marker& dto, const marker::Marker& bo)
+    {
+        dto.name = bo.name;
+        dto.robotGlobal = bo.robotGlobal;
+        dto.rgbCamera = bo.rgbCamera;
+        dto.depthCamera = bo.depthCamera;
+        dto.markerPose = bo.markerPose;
+        dto.markerGlobal = bo.getGlobalMarkerPose();
+    }
 
+    void
+    fromAron(const arondto::Marker& dto, marker::Marker& bo)
+    {
+        bo.name = dto.name;
+        bo.robotGlobal = dto.robotGlobal;
+        bo.rgbCamera = dto.rgbCamera;
+        bo.depthCamera = dto.depthCamera;
+        bo.markerPose = dto.markerPose;
+    }
 
-
-}  // namespace armarx::armem
+} // namespace armarx::armem
 
 armarx::armem::MemoryID
 armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
 {
     return MemoryID("Object/Instance")
-           .withProviderSegmentName(objectPose.providerName)
-           .withEntityName(objectPose.objectID.str())
-           .withTimestamp(objectPose.timestamp);
+        .withProviderSegmentName(objectPose.providerName)
+        .withEntityName(objectPose.objectID.str())
+        .withTimestamp(objectPose.timestamp);
 }

source/RobotAPI/libraries/armem_objects/aron_conversions.h

@@ -4,6 +4,7 @@
 
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
 #include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
 
 #include <RobotAPI/libraries/armem_objects/types.h>
 
@@ -26,6 +27,8 @@ namespace armarx::armem
     void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
     void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
 
+    void fromAron(const arondto::Marker& dto, marker::Marker&bo);
+    void toAron(arondto::Marker& dto, const marker::Marker&bo);
 }  // namespace armarx::armem
 
 

source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h

@@ -5,4 +5,6 @@ namespace armarx::armem::arondto
 {
     class ObjectClass;
     class ObjectInstance;
+    class Marker;
 }
+

source/RobotAPI/libraries/armem_objects/types.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::marker_pose_data
+ * @author     Hawo Höfer ( uuujt at student dot kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "types.h"
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
+
+#include "aron_forward_declarations.h"
+
+
+namespace armarx::armem::marker
+{
+    Marker::Marker(const std::string& name,
+                   const FramedPose& framedMarkerPose,
+                   const FramedPose& framedRobotPose,
+                   const FramedPose& framedRGBCameraPose,
+                   const FramedPose& framedDepthCameraPose) :
+        name(name),
+        robotGlobal(framedRobotPose),
+        rgbCamera(framedRGBCameraPose),
+        depthCamera(framedDepthCameraPose),
+        markerPose(framedMarkerPose)
+    {
+        markerGlobal = getGlobalMarkerPose();
+    }
+
+    Marker::Marker(const armarx::armem::arondto::Marker& dto) :
+        name(dto.name),
+        robotGlobal(dto.robotGlobal),
+        rgbCamera(dto.rgbCamera),
+        depthCamera(dto.depthCamera),
+        markerPose(dto.markerPose)
+    {
+        markerGlobal = getGlobalMarkerPose();
+    }
+    armarx::FramedPose
+    Marker::getGlobalMarkerPose() const
+    {
+        return {robotGlobal.toEigen() * depthCamera.toEigen() * markerPose.toEigen(),
+                GlobalFrame,
+                "marker"};
+    }
+
+} // namespace armarx::armem::marker

source/RobotAPI/libraries/armem_objects/types.h

@@ -26,6 +26,9 @@
 #include <RobotAPI/libraries/armem/core/MemoryID.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "aron_forward_declarations.h"
 
 
 namespace armarx::armem::attachment
@@ -84,15 +87,39 @@ namespace armarx::armem::attachment
         armem::Time timestamp;
 
         bool active;
-
     };
 
-}  // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
 
 namespace armarx::armem::articulated_object
 {
     using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
 
-    using ArticulatedObject  = armarx::armem::robot::Robot;
+    using ArticulatedObject = armarx::armem::robot::Robot;
     using ArticulatedObjects = armarx::armem::robot::Robots;
 } // namespace armarx::armem::articulated_object
+
+
+namespace armarx::armem::marker
+{
+    class Marker
+    {
+
+    public:
+        Marker(const armarx::armem::arondto::Marker& dto);
+        Marker(const std::string& name,
+               const FramedPose& markerPose,
+               const FramedPose& robotPose,
+               const FramedPose& rgbCameraPose,
+               const FramedPose& depthCameraPose);
+
+        std::string name;
+        FramedPose robotGlobal;
+        FramedPose rgbCamera;
+        FramedPose depthCamera;
+        FramedPose markerPose;
+        FramedPose markerGlobal;
+
+        FramedPose getGlobalMarkerPose() const;
+    };
+} // namespace armarx::armem::marker