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 b80a0a07c82ac6ee452b7cab489cee1f12e11b3b..d11b7421313e4586e4347b31c9f8c79c09f169f5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -6,14 +6,13 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-
namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
@@ -38,7 +37,6 @@ namespace armarx::armem::articulated_object
def->optional(properties.providerName, prefix + "ProviderName");
}
-
void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
@@ -55,7 +53,7 @@ namespace armarx::armem::articulated_object
memoryReader.setReadingMemory(result.proxy);
armem::MemoryID id = armem::MemoryID();
- id.memoryName = properties.memoryName;
+ id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
@@ -72,7 +70,8 @@ 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!";
@@ -89,15 +88,14 @@ namespace armarx::armem::articulated_object
// const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
// std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
// {
// updateKnownObject(snapshotID);
// });
}
-
- 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 description = queryDescription(name, timestamp);
@@ -110,17 +108,13 @@ namespace armarx::armem::articulated_object
return get(*description, timestamp);
}
-
ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp)
{
- ArticulatedObject obj
- {
- .description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp
- };
+ ArticulatedObject obj{.description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(obj, timestamp);
@@ -165,7 +159,8 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- 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;
@@ -174,8 +169,8 @@ namespace armarx::armem::articulated_object
qb
.coreSegments().withName(properties.coreClassSegmentName)
.providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
- .entities().withName(name)
- .snapshots().atTime(timestamp);
+ .entities().all() // withName(name)
+ .snapshots().latest();
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -184,13 +179,15 @@ namespace armarx::armem::articulated_object
if (not qResult.success) /* c++20 [[unlikely]] */
{
+ ARMARX_WARNING << qResult.errorMessage;
return std::nullopt;
}
return getRobotDescription(qResult.memory);
}
- std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -200,9 +197,9 @@ namespace armarx::armem::articulated_object
// clang-format off
qb
.coreSegments().withName(properties.coreInstanceSegmentName)
- .providerSegments().withName(properties.providerName) // agent
+ .providerSegments().all() // withName(properties.providerName) // agent
.entities().withName(description.name)
- .snapshots().atTime(timestamp);
+ .snapshots().latest();
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -217,76 +214,91 @@ namespace armarx::armem::articulated_object
return getRobotState(qResult.memory);
}
-
- std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotState>
+ Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
- const armem::wm::ProviderSegment& providerSegment = memory
- .getCoreSegment(properties.coreInstanceSegmentName)
- .getProviderSegment(properties.providerName);
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
{
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- return std::nullopt;
- }
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
- return robot::convertRobotState(instance);
- }
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ continue;
+ }
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ return robot::convertRobotState(instance);
+ }
+
+ return std::nullopt;
+ }
- std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotDescription>
+ Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
- const armem::wm::ProviderSegment& providerSegment = memory
- .getCoreSegment(properties.coreClassSegmentName)
- .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.coreClassSegmentName);
+ // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
- const auto entities = simox::alg::get_values(providerSegment.entities());
- if (entities.empty())
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
{
- ARMARX_WARNING << "No entity found";
- return std::nullopt;
- }
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ // return std::nullopt;
+ }
- const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
- if (entitySnapshots.empty())
- {
- ARMARX_WARNING << "No entity snapshots found";
- return std::nullopt;
- }
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ continue;
+ // return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ return robot::convertRobotDescription(instance);
+ }
- return robot::convertRobotDescription(instance);
+ return std::nullopt;
}
- std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ std::vector<robot::RobotDescription>
+ Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
std::vector<robot::RobotDescription> descriptions;
- const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
{
- for (const auto& [name, entity] : providerSegment.entities())
+ for (const auto &[name, entity] : providerSegment.entities())
{
if (entity.empty())
{
@@ -309,5 +321,4 @@ namespace armarx::armem::articulated_object
return descriptions;
}
-
} // namespace armarx::armem::articulated_object
\ No newline at end of file