diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 07f3089ca3acaf5370ca5ff38f3d2f2e3187294e..d6966d179fa408ca925f63d28b96dca76ea669f7 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/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);
+ });
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 4eb71acf3b9374eff330ae463acc208872a918f2..869beea33f6878cc7dde20db9a03bd352e870fd7 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/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;
};
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index ac67c9ad6cc34d051144df93def583c94ce63187..7265e46d24e5e046e55a7e01365018d725c167c7 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/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
)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Marker.xml b/source/RobotAPI/libraries/armem_objects/aron/Marker.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0fa6542098e8d8d6542845927564a5026e192de7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/Marker.xml
@@ -0,0 +1,32 @@
+<!--
+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>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 8602178224b696fdcf026f50184628ee7a870dae..23b84e31aef6972059feb8cdf26f75fe3d4271ab 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -1,50 +1,55 @@
#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);
}
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 61e89c37ec00aaabed94e1b20a2ba1b50be976a8..e27ed16c4cede302e2eecf2c013440d240162550 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
index 9416a033d713f5c8dab28506538c1a56534dbb53..6a57255f1019c433f87d7f1f66faa3919a9087c8 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
@@ -5,4 +5,6 @@ namespace armarx::armem::arondto
{
class ObjectClass;
class ObjectInstance;
+ class Marker;
}
+
diff --git a/source/RobotAPI/libraries/armem_objects/types.cpp b/source/RobotAPI/libraries/armem_objects/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5969c49aecafbf88a2481a3e1cae1f2ff6737f77
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/types.cpp
@@ -0,0 +1,63 @@
+/*
+ * 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/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(),
+ "global",
+ "marker"};
+ }
+
+} // namespace armarx::armem::marker
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 5ca91ff285e5fc92d1d3523a504379d582dda8b5..06fb59cee6480f1dc6aaa91144261d9bb1f9a397 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/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,40 @@ 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
+ {
+ using FramedPose = armarx::FramedPose;
+
+ 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;
+ armarx::FramedPose robotGlobal;
+ armarx::FramedPose rgbCamera;
+ armarx::FramedPose depthCamera;
+ armarx::FramedPose markerPose;
+ armarx::FramedPose markerGlobal;
+
+ armarx::FramedPose getGlobalMarkerPose() const;
+ };
+} // namespace armarx::armem::marker