From 735268e675687ffb636c8c018eb143e8cfdbb42c Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Thu, 15 Dec 2022 18:03:32 +0100
Subject: [PATCH] added ObjectInstance BO class
---
.../armem_objects/aron_conversions.cpp | 18 ++++-
.../armem_objects/aron_conversions.h | 8 +--
.../client/articulated_object/Reader.cpp | 12 ++--
.../client/articulated_object/Writer.cpp | 17 +++--
.../armem_objects/server/instance/Segment.cpp | 67 ++++++++++---------
.../armem_objects/server/instance/Segment.h | 9 +--
.../RobotAPI/libraries/armem_objects/types.h | 11 +++
7 files changed, 84 insertions(+), 58 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 23b84e31a..162f80924 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -8,6 +8,22 @@ namespace armarx::armem
{
void
+ fromAron(const arondto::ObjectInstance& dto, ObjectInstance& bo)
+ {
+ fromAron(dto.pose, bo.pose);
+ fromAron(dto.classID, bo.classID);
+ fromAron(dto.sourceID, bo.sourceID);
+ }
+
+ void
+ toAron(arondto::ObjectInstance& dto, const ObjectInstance& bo)
+ {
+ toAron(dto.pose, bo.pose);
+ toAron(dto.classID, bo.classID);
+ toAron(dto.sourceID, bo.sourceID);
+ }
+
+ /*void
fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
{
bo = dto.pose;
@@ -29,7 +45,7 @@ namespace armarx::armem
toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
{
objpose::toAron(dto.pose, bo);
- }
+ }*/
/* Attachments */
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index e27ed16c4..d7e3b3178 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -10,12 +10,8 @@
namespace armarx::armem
{
- void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
- void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo);
-
- void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
- void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
-
+ void fromAron(const arondto::ObjectInstance&, ObjectInstance&);
+ void toAron(arondto::ObjectInstance&, const ObjectInstance&);
/* Attachments */
void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 9a4417080..ac0be81cf 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -20,6 +20,7 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include "utils.h"
@@ -262,10 +263,13 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
- armarx::armem::arondto::ObjectInstance aronObjectInstance;
+ armarx::armem::ObjectInstance aronObjectInstance;
try
{
- aronObjectInstance.fromAron(instance.data());
+ armarx::armem::arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ fromAron(dto, aronObjectInstance);
}
catch (...)
{
@@ -273,9 +277,7 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- objpose::ObjectPose objectPose;
- objpose::fromAron(aronObjectInstance.pose, objectPose);
-
+ objpose::ObjectPose objectPose = aronObjectInstance.pose;
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
.jointMap = objectPose.objectJointValues};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index fbce08b63..c4251f5b4 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -13,6 +13,7 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -206,8 +207,7 @@ namespace armarx::armem::articulated_object
// arondto::Robot aronArticulatedObject;
// robot::toAron(aronArticulatedObject, obj);
- arondto::ObjectInstance objectInstance;
- toAron(objectInstance, obj.config);
+ ObjectInstance objectInstance;
const auto classId = storeOrGetClass(obj);
@@ -218,21 +218,20 @@ namespace armarx::armem::articulated_object
}
// install memory link
- toAron(objectInstance.classID, *classId);
+ objectInstance.classID = *classId;
armem::MemoryID id;
id.setEntityID(classId->getEntityID());
armarx::ObjectID objectId(id.entityName);
-
- armarx::arondto::ObjectID cs;
- cs.className = objectId.className();
- cs.instanceName = objectId.instanceName();
- cs.dataset = objectId.dataset();
+ armarx::ObjectID cs(objectId.dataset(), objectId.className(), objectId.instanceName());
objectInstance.pose.objectID = cs;
- update.instancesData = {objectInstance.toAron()};
+ arondto::ObjectInstance dto;
+ toAron(dto, objectInstance);
+
+ update.instancesData = {dto.toAron()};
update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 7f3aee34f..ff7f10803 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -170,10 +170,8 @@ namespace armarx::armem::server::obj::instance
const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
- const arondto::ObjectInstance data = getLatestInstanceData(*entity);
-
- previousPose = objpose::ObjectPose();
- fromAron(data, *previousPose);
+ const ObjectInstance data = getLatestInstanceData(*entity);
+ previousPose = data.pose;
}
bool discard = false;
@@ -274,7 +272,7 @@ namespace armarx::armem::server::obj::instance
update.confidence = pose.confidence;
arondto::ObjectInstance dto;
- toAron(dto, pose);
+ toAron(dto.pose, pose);
// Search for object class.
if (auto instance = findClassInstance(pose.objectID))
{
@@ -504,15 +502,17 @@ namespace armarx::armem::server::obj::instance
{
entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
{
+ ObjectInstance bo;
arondto::ObjectInstance dto;
dto.fromAron(instance.data());
- fromAron(dto, out);
+ fromAron(dto, bo);
+ out = bo.pose;
});
}
- arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
+ ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
@@ -520,10 +520,13 @@ namespace armarx::armem::server::obj::instance
ARMARX_CHECK_EQUAL(snapshot.size(), 1);
const wm::EntityInstance& instance = snapshot.getInstance(0);
- arondto::ObjectInstance data;
- data.fromAron(instance.data());
+ ObjectInstance bo;
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ fromAron(dto, bo);
- return data;
+ return bo;
}
@@ -602,7 +605,7 @@ namespace armarx::armem::server::obj::instance
dto.fromAron(instance.data());
ObjectPose pose;
- fromAron(dto, pose);
+ fromAron(dto.pose, pose);
result.emplace(snapshot.time(), pose);
});
@@ -681,7 +684,7 @@ namespace armarx::armem::server::obj::instance
<< "' of agent '" << agent->getName() << "', but object is currently not provided.";
return output;
}
- arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
+ ObjectInstance data = getLatestInstanceData(*objectEntity);
objpose::ObjectAttachmentInfo info;
info.agentName = agent->getName();
@@ -717,10 +720,12 @@ namespace armarx::armem::server::obj::instance
update.entityID = objectEntity->id();
update.timeCreated = now;
{
- arondto::ObjectInstance updated = data;
- toAron(updated.pose.attachment, info);
- updated.pose.attachmentValid = true;
- update.instancesData = { updated.toAron() };
+ ObjectInstance updated = data;
+ data.pose.attachment = info;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, data);
+ update.instancesData = { dto.toAron() };
}
iceMemory.commit(commit);
}
@@ -748,15 +753,15 @@ namespace armarx::armem::server::obj::instance
ObjectID objectID = armarx::fromIce(input.objectID);
std::string providerName = input.providerName;
- std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
+ std::optional<objpose::ObjectAttachmentInfo> attachment;
{
// Remove from latest pose (if it was cached).
// Find object pose (provider name can be empty).
wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
if (entity)
{
- const arondto::ObjectInstance data = getLatestInstanceData(*entity);
- if (data.pose.attachmentValid)
+ const ObjectInstance data = getLatestInstanceData(*entity);
+ if (data.pose.attachment)
{
attachment = data.pose.attachment;
@@ -799,8 +804,8 @@ namespace armarx::armem::server::obj::instance
output.numDetached = 0;
segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity)
{
- const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
- if (data.pose.attachmentValid)
+ const ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachment)
{
++output.numDetached;
// Store non-attached pose in new snapshot.
@@ -816,7 +821,7 @@ namespace armarx::armem::server::obj::instance
void Segment::storeDetachedSnapshot(
wm::Entity& entity,
- const arondto::ObjectInstance& data,
+ const ObjectInstance& data,
Time now,
bool commitAttachedPose)
{
@@ -825,27 +830,29 @@ namespace armarx::armem::server::obj::instance
update.entityID = entity.id();
update.timeCreated = now;
{
- arondto::ObjectInstance updated;
- if (commitAttachedPose and data.pose.attachmentValid)
+ ObjectInstance updated;
+ if (commitAttachedPose and data.pose.attachment)
{
- ObjectPose objectPose;
- fromAron(data, objectPose);
+ ObjectPose objectPose = data.pose;
VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
bool agentSynchronized = false;
updateAttachement(objectPose, robot, agentSynchronized);
objectPose.attachment = std::nullopt;
- toAron(updated, objectPose);
+ //toAron(updated, objectPose);
}
else
{
updated = data;
- updated.pose.attachmentValid = false;
- toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
+ updated.pose.attachment = std::nullopt;
+ //updated.pose.attachmentValid = false;
+ //toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
}
- update.instancesData = { updated.toAron() };
+ arondto::ObjectInstance i;
+ toAron(i, updated);
+ update.instancesData = { i.toAron() };
}
iceMemory.commit(commit);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 08163aee2..aee71528b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -27,11 +27,6 @@
#include "Decay.h"
-namespace armarx::armem::arondto
-{
- class ObjectInstance;
-}
-
namespace armarx::armem::server::obj::instance
{
@@ -102,7 +97,7 @@ namespace armarx::armem::server::obj::instance
static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
- static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+ static ObjectInstance getLatestInstanceData(const wm::Entity& entity);
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
@@ -135,7 +130,7 @@ namespace armarx::armem::server::obj::instance
void storeDetachedSnapshot(
wm::Entity& entity,
- const arondto::ObjectInstance& data,
+ const ObjectInstance& data,
Time now,
bool commitAttachedPose);
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index c7e4fe489..79fe9ac95 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -23,6 +23,7 @@
#include <Eigen/Geometry>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot/types.h>
@@ -31,6 +32,16 @@
#include "aron_forward_declarations.h"
+namespace armarx::armem
+{
+ struct ObjectInstance
+ {
+ objpose::ObjectPose pose;
+ MemoryID classID;
+ MemoryID sourceID;
+ };
+}
+
namespace armarx::armem::attachment
{
using AgentID = armem::MemoryID;
--
GitLab