diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 162f8092446e44e819c8ddd7b97546ef56dbb5d2..23b84e31aef6972059feb8cdf26f75fe3d4271ab 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -8,22 +8,6 @@ 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;
@@ -45,7 +29,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 d7e3b3178cd89fabf7e1bc61622afa8e406b0a99..e27ed16c4cede302e2eecf2c013440d240162550 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -10,8 +10,12 @@
namespace armarx::armem
{
- void fromAron(const arondto::ObjectInstance&, ObjectInstance&);
- void toAron(arondto::ObjectInstance&, const ObjectInstance&);
+ 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);
+
/* 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 ac0be81cf9fa9ff8727f62bcbaa0adcaa4d3f784..9a4417080bb027e4ee8d4a9cf0c2dc080fca3890 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -20,7 +20,6 @@
#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"
@@ -263,13 +262,10 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
- armarx::armem::ObjectInstance aronObjectInstance;
+ armarx::armem::arondto::ObjectInstance aronObjectInstance;
try
{
- armarx::armem::arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
-
- fromAron(dto, aronObjectInstance);
+ aronObjectInstance.fromAron(instance.data());
}
catch (...)
{
@@ -277,7 +273,9 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- objpose::ObjectPose objectPose = aronObjectInstance.pose;
+ objpose::ObjectPose objectPose;
+ objpose::fromAron(aronObjectInstance.pose, objectPose);
+
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 c4251f5b4de0b4b5b9afe5f13c385b297a89535f..fbce08b6323e1a1235bd000aadbd4e73ff6cbf8c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -13,7 +13,6 @@
#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>
@@ -207,7 +206,8 @@ namespace armarx::armem::articulated_object
// arondto::Robot aronArticulatedObject;
// robot::toAron(aronArticulatedObject, obj);
- ObjectInstance objectInstance;
+ arondto::ObjectInstance objectInstance;
+ toAron(objectInstance, obj.config);
const auto classId = storeOrGetClass(obj);
@@ -218,20 +218,21 @@ namespace armarx::armem::articulated_object
}
// install memory link
- objectInstance.classID = *classId;
+ toAron(objectInstance.classID, *classId);
armem::MemoryID id;
id.setEntityID(classId->getEntityID());
armarx::ObjectID objectId(id.entityName);
- armarx::ObjectID cs(objectId.dataset(), objectId.className(), objectId.instanceName());
- objectInstance.pose.objectID = cs;
+ armarx::arondto::ObjectID cs;
+ cs.className = objectId.className();
+ cs.instanceName = objectId.instanceName();
+ cs.dataset = objectId.dataset();
- arondto::ObjectInstance dto;
- toAron(dto, objectInstance);
+ objectInstance.pose.objectID = cs;
- update.instancesData = {dto.toAron()};
+ update.instancesData = {objectInstance.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 33282bb4eeb7ec9d594a80e810608431adebab41..47f055c977c63801354ef83870fcc7fd3680a52c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -158,7 +158,7 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseSeq newObjectPoses;
stats.numUpdated = 0;
- // timestamp used to reduce the rpc calls for robot sync
+ // timestamp used to reduce the rpc calls for robot sync
Time robotSyncTimestamp = Time::Invalid();
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
@@ -170,8 +170,10 @@ namespace armarx::armem::server::obj::instance
const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
- const ObjectInstance data = getLatestInstanceData(*entity);
- previousPose = data.pose;
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+
+ previousPose = objpose::ObjectPose();
+ fromAron(data, *previousPose);
}
bool discard = false;
@@ -272,7 +274,7 @@ namespace armarx::armem::server::obj::instance
update.confidence = pose.confidence;
arondto::ObjectInstance dto;
- toAron(dto.pose, pose);
+ toAron(dto, pose);
// Search for object class.
if (auto instance = findClassInstance(pose.objectID))
{
@@ -502,17 +504,15 @@ 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, bo);
- out = bo.pose;
+ fromAron(dto, out);
});
}
- ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
+ arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
@@ -520,13 +520,10 @@ namespace armarx::armem::server::obj::instance
ARMARX_CHECK_EQUAL(snapshot.size(), 1);
const wm::EntityInstance& instance = snapshot.getInstance(0);
- ObjectInstance bo;
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
-
- fromAron(dto, bo);
+ arondto::ObjectInstance data;
+ data.fromAron(instance.data());
- return bo;
+ return data;
}
@@ -605,7 +602,7 @@ namespace armarx::armem::server::obj::instance
dto.fromAron(instance.data());
ObjectPose pose;
- fromAron(dto.pose, pose);
+ fromAron(dto, pose);
result.emplace(snapshot.time(), pose);
});
@@ -684,7 +681,7 @@ namespace armarx::armem::server::obj::instance
<< "' of agent '" << agent->getName() << "', but object is currently not provided.";
return output;
}
- ObjectInstance data = getLatestInstanceData(*objectEntity);
+ arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
objpose::ObjectAttachmentInfo info;
info.agentName = agent->getName();
@@ -720,12 +717,10 @@ namespace armarx::armem::server::obj::instance
update.entityID = objectEntity->id();
update.timeCreated = now;
{
- ObjectInstance updated = data;
- data.pose.attachment = info;
-
- arondto::ObjectInstance dto;
- toAron(dto, data);
- update.instancesData = { dto.toAron() };
+ arondto::ObjectInstance updated = data;
+ toAron(updated.pose.attachment, info);
+ updated.pose.attachmentValid = true;
+ update.instancesData = { updated.toAron() };
}
iceMemory.commit(commit);
}
@@ -753,15 +748,15 @@ namespace armarx::armem::server::obj::instance
ObjectID objectID = armarx::fromIce(input.objectID);
std::string providerName = input.providerName;
- std::optional<objpose::ObjectAttachmentInfo> attachment;
+ std::optional<objpose::arondto::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 ObjectInstance data = getLatestInstanceData(*entity);
- if (data.pose.attachment)
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+ if (data.pose.attachmentValid)
{
attachment = data.pose.attachment;
@@ -804,8 +799,8 @@ namespace armarx::armem::server::obj::instance
output.numDetached = 0;
segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity)
{
- const ObjectInstance data = this->getLatestInstanceData(entity);
- if (data.pose.attachment)
+ const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
{
++output.numDetached;
// Store non-attached pose in new snapshot.
@@ -821,7 +816,7 @@ namespace armarx::armem::server::obj::instance
void Segment::storeDetachedSnapshot(
wm::Entity& entity,
- const ObjectInstance& data,
+ const arondto::ObjectInstance& data,
Time now,
bool commitAttachedPose)
{
@@ -830,29 +825,27 @@ namespace armarx::armem::server::obj::instance
update.entityID = entity.id();
update.timeCreated = now;
{
- ObjectInstance updated;
- if (commitAttachedPose and data.pose.attachment)
+ arondto::ObjectInstance updated;
+ if (commitAttachedPose and data.pose.attachmentValid)
{
- ObjectPose objectPose = data.pose;
+ ObjectPose objectPose;
+ fromAron(data, objectPose);
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.attachment = std::nullopt;
- //updated.pose.attachmentValid = false;
- //toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
+ updated.pose.attachmentValid = false;
+ toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
}
- arondto::ObjectInstance i;
- toAron(i, updated);
- update.instancesData = { i.toAron() };
+ update.instancesData = { updated.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 aee71528b8a93fda088e98a9ac461470135720aa..08163aee236405e1da5c3f99ef748f6e4cdc3e48 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -27,6 +27,11 @@
#include "Decay.h"
+namespace armarx::armem::arondto
+{
+ class ObjectInstance;
+}
+
namespace armarx::armem::server::obj::instance
{
@@ -97,7 +102,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 ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+ static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
@@ -130,7 +135,7 @@ namespace armarx::armem::server::obj::instance
void storeDetachedSnapshot(
wm::Entity& entity,
- const ObjectInstance& data,
+ const arondto::ObjectInstance& data,
Time now,
bool commitAttachedPose);