diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
index 3432b49f461f373cbf66af66315c27566e20c7b2..553a24c85e79c7353c09893df04212e17c2c1fb8 100644
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
@@ -1,15 +1,19 @@
#include "ArticulatedObjectExampleMemoryWriterClient.h"
-#include "ArmarXCore/core/PackagePath.h"
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include <Eigen/src/Geometry/Transform.h>
+#include <memory>
+
+#include <Eigen/Geometry>
+
#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <memory>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/CycleUtil.h>
@@ -17,19 +21,18 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
namespace armarx::articulated_object
{
ArticulatedObjectExampleMemoryWriterClient::ArticulatedObjectExampleMemoryWriterClient() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)){}
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)) {}
armarx::PropertyDefinitionsPtr ArticulatedObjectExampleMemoryWriterClient::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
- new ComponentPropertyDefinitions(getConfigIdentifier());
+ new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
@@ -55,31 +58,29 @@ namespace armarx::articulated_object
task->start();
}
- void ArticulatedObjectExampleMemoryWriterClient::onDisconnectComponent() { task->stop(); }
+ void ArticulatedObjectExampleMemoryWriterClient::onDisconnectComponent()
+ {
+ task->stop();
+ }
void ArticulatedObjectExampleMemoryWriterClient::onExitComponent() {}
VirtualRobot::RobotPtr createDishwasher()
{
const std::string xml =
- "Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
+ "./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
const std::string name = "dishwasher";
- const PackagePath packagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, xml), xml);
-
- auto robot = VirtualRobot::RobotIO::loadRobot(packagePath.toSystemPath(), VirtualRobot::RobotIO::eStructure);
-
- return robot;
+ return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
}
armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
{
- ARMARX_INFO << "filename is " << obj.getFilename();
-
-
+ ARMARX_INFO << "Filename is " << obj.getFilename();
- return
- armem::articulated_object::ArticulatedObject{
+ return
+ armem::articulated_object::ArticulatedObject
+ {
.description = {
.name = obj.getName(),
.xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
@@ -106,7 +107,6 @@ namespace armarx::articulated_object
while (not task->isStopped())
{
-
ARMARX_INFO << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index cc296ea48e6f06437060c7e1cb41f7cd06678e8b..4b3406074588f20a9b5abd0251b6e2188078bc56 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -23,6 +23,7 @@
#include "ObjectMemory.h"
#include <RobotAPI/interface/core/articulated_object/topic.h>
+#include <mutex>
namespace armarx::armem::server::obj
@@ -120,8 +121,16 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::arviz
);
+
+ {
+ std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
+ server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
+ server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
+ }
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
+
}
void ObjectMemory::onDisconnectComponent()
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 e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..ed69e4c80c5bc975c1acd46ffb7b57e4418915d7 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -0,0 +1,122 @@
+#include "Reader.h"
+
+#include <mutex>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <optional>
+
+namespace armarx::armem::articulated_object
+{
+
+ Reader::Reader(ManagedIceObject& component) : MemoryConnector(component) {}
+
+
+ ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp)
+ {
+ ArticulatedObject obj
+ {
+ .description = description,
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp
+ };
+
+ synchronize(obj, timestamp);
+
+ return obj;
+ }
+
+ void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
+ {
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return;
+ }
+
+ obj.config = std::move(*state);
+ }
+
+ std::optional<RobotState> Reader::queryState(const RobotDescription& description, const armem::Time& timestamp)
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.memoryName)
+ .providerSegments().withName(properties.providerName) // agent
+ .entities().withName(description.name)
+ .snapshots().atTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotState(qResult.memory);
+ }
+
+ std::optional<RobotState> convert(const armem::wm::EntityInstance& instance)
+ {
+ arondto::RobotState aronRobotState;
+ try
+ {
+ aronRobotState.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ return std::nullopt;
+ }
+
+ RobotState robotState;
+ fromAron(aronRobotState, robotState);
+
+ return robotState;
+ }
+
+ std::optional<RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.memoryName)
+ .getProviderSegment(properties.providerName);
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return convert(instance);
+ }
+
+
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
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 556284d2cc8961c96b5545acfdd5a1855c5f2fcf..3fe33b6c6bc69f61a4721a014cb2618a5f59d905 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -1,7 +1,12 @@
#pragma once
-#include "interfaces.h"
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "interfaces.h"
namespace armarx::armem::articulated_object
{
@@ -10,10 +15,31 @@ namespace armarx::armem::articulated_object
virtual public ::armarx::armem::MemoryConnector
{
public:
+ Reader(ManagedIceObject& component);
virtual ~Reader() = default;
void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
+
+ std::optional<RobotState> queryState(const RobotDescription& description, const armem::Time& timestamp);
+
+ protected:
+ std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
+
+ private:
+
+ struct Properties
+ {
+ std::string memoryName = "ObjectMemory";
+ std::string coreSegmentName = "ArticulatedObjectInstance";
+ std::string providerName;
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.articulated.read.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
};
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 c47215d1342f8923a3de7cad5064007636ba9254..ea8545699d4dbc85a3a135ca3efb269d2a0f755c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,4 +1,5 @@
#include "Writer.h"
+
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
#include <mutex>
@@ -49,13 +50,11 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Trying to create core segment + provider segment";
const auto result =
- memoryWriter.addSegment(properties.memoryName, properties.providerName);
+ memoryWriter.addSegment(properties.coreSegmentName, properties.providerName);
if (not result.success)
{
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
return false;
}