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
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 703ef91aa2eea9d00ea09b88255b68ffacba68bc..9062747de124ac3db5ba37e3f14f2b8cdfc7b692 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -34,7 +34,7 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
- def->optional(properties.providerName, prefix + "ProviderName");
+ def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
}
void Writer::connect()
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 7e450b0464d1b571791392beb4622afc77444f5b..f2031dd402e546a21a80a17c28f8b2e4107f9059 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -70,7 +70,7 @@ namespace armarx::armem::articulated_object
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName = "ArmarXObjects";
+ std::string providerName;
bool allowClassCreation = false;
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 6c05e89e038c6f80f61e3e9c47f25bf8b2f1335f..3bf4171486e35c33a1cca23a666baac2233a8356 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -43,10 +43,10 @@ namespace armarx::armem::robot
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
{
- arondto::RobotState aronRobotState;
+ arondto::Robot aronRobot;
try
{
- aronRobotState.fromAron(instance.data());
+ aronRobot.fromAron(instance.data());
}
catch (...)
{
@@ -55,7 +55,7 @@ namespace armarx::armem::robot
}
RobotState robotState;
- fromAron(aronRobotState, robotState);
+ fromAron(aronRobot.state, robotState);
return robotState;
}