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 8b6d4e525f4710dde4f6384af5ce89fe99e0e372..90ebecdecbee6d9e84aa86fd1dd60fbd38d76082 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -2,15 +2,16 @@
#include <mutex>
#include <optional>
-#include <Eigen/src/Geometry/Transform.h>
-#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Geometry>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -32,7 +33,8 @@ namespace armarx::armem::articulated_object
{
}
- void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
@@ -49,7 +51,8 @@ namespace armarx::armem::articulated_object
def->optional(properties.providerName, prefix + "read.ProviderName");
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
@@ -65,14 +68,15 @@ namespace armarx::armem::articulated_object
}
armem::MemoryID id = armem::MemoryID();
- id.memoryName = properties.memoryName;
+ id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
}
- void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+ void
+ Reader::updateKnownObject(const armem::MemoryID& snapshotId)
{
// const std::string& nameWithDataset = snapshotId.providerSegmentName;
@@ -82,8 +86,9 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
+ void
+ Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
@@ -106,9 +111,15 @@ namespace armarx::armem::articulated_object
// });
}
- std::optional<ArticulatedObject> Reader::get(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<ArticulatedObject>
+ Reader::get(const std::string& name, const armem::Time& timestamp)
{
+ const auto splits = simox::alg::split(name, "/");
+ ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
+
+ const std::string className = splits.at(0) + "/" + splits.at(1); // `DATASET/NAME`
+ const std::string instanceName = splits.at(2);
+
const auto description = queryDescription(name, timestamp);
if (not description)
@@ -117,23 +128,26 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- return get(*description, timestamp);
+ return get(*description, timestamp, instanceName);
}
- ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp)
+ ArticulatedObject
+ Reader::get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp,
+ const std::string& instanceName)
{
ArticulatedObject obj{.description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp};
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(obj, timestamp);
return obj;
}
- void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
+ void
+ Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
{
auto state = queryState(obj.instance, timestamp);
@@ -146,7 +160,8 @@ namespace armarx::armem::articulated_object
obj.config = std::move(*state);
}
- std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+ std::vector<robot::RobotDescription>
+ Reader::queryDescriptions(const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -171,18 +186,20 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- std::string Reader::getProviderName() const
+ std::string
+ Reader::getProviderName() const
{
return properties.providerName;
}
- void Reader::setProviderName(const std::string& providerName)
+ void
+ Reader::setProviderName(const std::string& providerName)
{
this->properties.providerName = providerName;
}
- std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<robot::RobotDescription>
+ Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -208,8 +225,8 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<robot::RobotState> Reader::queryState(const std::string& instanceName,
- const armem::Time& timestamp)
+ std::optional<robot::RobotState>
+ Reader::queryState(const std::string& instanceName, const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -237,7 +254,8 @@ namespace armarx::armem::articulated_object
}
- std::optional<robot::RobotState> convertToRobotState(const armem::wm::EntityInstance& instance)
+ std::optional<robot::RobotState>
+ convertToRobotState(const armem::wm::EntityInstance& instance)
{
armarx::armem::arondto::ObjectInstance aronObjectInstance;
try
@@ -253,12 +271,9 @@ namespace armarx::armem::articulated_object
objpose::ObjectPose objectPose;
objpose::fromAron(aronObjectInstance.pose, objectPose);
- robot::RobotState robotState
- {
- .timestamp = objectPose.timestamp,
- .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues
- };
+ robot::RobotState robotState{.timestamp = objectPose.timestamp,
+ .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
+ .jointMap = objectPose.objectJointValues};
return robotState;
}
@@ -272,20 +287,16 @@ namespace armarx::armem::articulated_object
// clang-format on
std::optional<wm::EntityInstance> instance;
- coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = i;
- });
-
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; });
+
if (instance.has_value())
{
return convertToRobotState(instance.value());
// return robot::convertRobotState(instance.value());
}
-
+
ARMARX_FATAL << "Failed to obtain robot state";
return std::nullopt;
-
}
@@ -297,10 +308,7 @@ namespace armarx::armem::articulated_object
// clang-format on
std::optional<wm::EntityInstance> instance;
- coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = i;
- });
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; });
if (instance.has_value())
{
@@ -309,7 +317,6 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "No robot description";
return std::nullopt;
-
}
@@ -320,17 +327,19 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(properties.coreClassSegmentName);
std::vector<robot::RobotDescription> descriptions;
- coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
- {
- if (not entity.empty())
+ coreSegment.forEachEntity(
+ [&descriptions](const wm::Entity& entity)
{
- const auto robotDescription = convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
- if (robotDescription)
+ if (not entity.empty())
{
- descriptions.push_back(*robotDescription);
+ const auto robotDescription =
+ convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
+ if (robotDescription)
+ {
+ descriptions.push_back(*robotDescription);
+ }
}
- }
- });
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index c0991c7536e7e82c4ffb401ab6b3497266e76fda..3108b17f9b7054a61290a4647d5d3965f17d2fbd 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -50,7 +50,7 @@ namespace armarx::armem::articulated_object
std::optional<ArticulatedObject> get(const std::string& name,
const armem::Time& timestamp) override;
ArticulatedObject get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp, const std::string& instanceName) override;
std::optional<robot::RobotState> queryState(const std::string &instanceName,
const armem::Time& timestamp);
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index ced0a76fd07fe4575ea4762ea062a804431fff2a..a3061a553cb545605ce37febf9632d6c73d50cda 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -13,7 +13,7 @@ namespace armarx::armem::articulated_object
virtual void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
- virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) = 0;
+ virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName) = 0;
virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) = 0;
};
@@ -26,4 +26,4 @@ namespace armarx::armem::articulated_object
virtual bool store(const ArticulatedObject& obj) = 0;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object