Skip to content
Snippets Groups Projects
Commit 735268e6 authored by Fabian Tërnava's avatar Fabian Tërnava
Browse files

added ObjectInstance BO class

parent 9201e171
No related branches found
No related tags found
1 merge request!321Merge of all branches of Fabian PK (related to skills)
Hide whitespace changes
Inline Side-by-side
Showing
with 84 additions and 58 deletions
source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+ 17
− 1
View file @ 735268e6
......@@ -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 */
......
source/RobotAPI/libraries/armem_objects/aron_conversions.h
+ 2
− 6
View file @ 735268e6
......@@ -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);
......
source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+ 7
− 5
View file @ 735268e6
......@@ -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};
......
source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+ 8
− 9
View file @ 735268e6
......@@ -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;
......
source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+ 37
− 30
View file @ 735268e6
......@@ -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);
}
......
source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+ 2
− 7
View file @ 735268e6
......@@ -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);
......
source/RobotAPI/libraries/armem_objects/types.h
+ 11
− 0
View file @ 735268e6
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment