diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index a7594d23e1668080fc0006afc44b1a09abfa8587..16f823e37de7efd21d6ac9f9633976a989ca0294 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,2 +1,3 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(ArticulatedObjectExampleMemoryWriterClient)
+add_subdirectory(VirtualRobotReaderExampleClient)
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cd132bb87336bf793e86e79b7e048ea044921491
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("VirtualRobotReaderExampleClient")
+
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore
+ RobotAPIInterfaces
+ armem_robot_state
+)
+
+set(SOURCES
+ VirtualRobotReaderExampleClient.cpp
+)
+
+set(HEADERS
+ VirtualRobotReaderExampleClient.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::robot_state")
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85ca85f48ca62c7bc686bd9286d52fae5480ebc7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -0,0 +1,105 @@
+
+
+#include "VirtualRobotReaderExampleClient.h"
+
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#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>
+
+
+namespace armarx::robot_state
+{
+ VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
+ virtualRobotReader(*this) {}
+
+ armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(p.robotName, "robotName");
+ defs->optional(p.updateFrequency, "updateFrequency");
+
+ virtualRobotReader.registerPropertyDefinitions(defs);
+
+ return defs;
+ }
+
+ std::string VirtualRobotReaderExampleClient::getDefaultName() const
+ {
+ return "VirtualRobotReaderExampleClient";
+ }
+
+ void VirtualRobotReaderExampleClient::onInitComponent() {}
+
+ void VirtualRobotReaderExampleClient::onConnectComponent()
+ {
+ virtualRobotReader.connect();
+
+ ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
+
+ task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task->start();
+ }
+
+ void VirtualRobotReaderExampleClient::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void VirtualRobotReaderExampleClient::onExitComponent() {}
+
+ void VirtualRobotReaderExampleClient::run()
+ {
+
+ // initialize if needed
+ if (virtualRobot == nullptr)
+ {
+ TIMING_START(getRobot);
+
+ virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
+
+ if (virtualRobot == nullptr)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
+ return;
+ }
+ // only print timing once the robot is loadable & loaded
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+
+ TIMING_START(synchronizeRobot);
+ virtualRobotReader.synchronizeRobot(*virtualRobot, now);
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+
+ }
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e1d01f5564e96c7e46ad2db33081835873dd7a6
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -0,0 +1,96 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// ArmarX
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// RobotAPI
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+namespace armarx::robot_state
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class VirtualRobotReaderExampleClient :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
+ {
+ public:
+ VirtualRobotReaderExampleClient();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void run();
+
+
+ private:
+
+ struct Properties
+ {
+ std::string robotName{"Armar6"};
+ float updateFrequency{10.F};
+ } p;
+
+ armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+
+ armem::robot_state::VirtualRobotReader virtualRobotReader;
+
+ std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+
+ };
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 444c6fdc39e109de28afa911f74b19e01b4cd36c..5aba1210686f0cc0a821d83f1053e65a41c348e6 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -337,7 +337,7 @@ namespace armarx::armem::server::robot_state
auto it = convertedAndGroupedData.groups.find("sens.Platform");
if (it != convertedAndGroupedData.groups.end())
{
- ARMARX_DEBUG << " found odometry data.";
+ ARMARX_DEBUG << "Found odometry data.";
const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index f08ae23bc6b8b6bc7fb4ed99f9ac51ed570f556c..35203a014fa423cec4f8ef0f4d09759bc54385a9 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/source/RobotAPI/libraries/armem/client/query/query_fns.h
@@ -156,7 +156,7 @@ namespace armarx::armem::client::query_fns
inline
std::function<void(query::SnapshotSelector&)>
- beforeTime(Time time, long nElements)
+ beforeTime(Time time, long nElements = 1)
{
return [ = ](query::SnapshotSelector & selector)
{
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index a1c314232099438f9498fbabb763e52c00abe862..df642ded9a70ea2688eaca5865f730e8e99ee6de 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.h
@@ -28,7 +28,7 @@ namespace armarx::armem::client::query
SnapshotSelector& atTime(Time timestamp);
SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
- SnapshotSelector& beforeTime(Time timestamp, long maxEntries);
+ SnapshotSelector& beforeTime(Time timestamp, long maxEntries = 1);
SnapshotSelector& beforeOrAtTime(Time timestamp);
SnapshotSelector& timeRange(Time min, Time max);
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..d127fbf51eee8d842ee259211fd63a467d0a2f9b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -0,0 +1,29 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+namespace armarx::armem::robot
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ };
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ virtual bool store(const Robot& obj) = 0;
+ };
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 03c8ae1bfed67fb900f50905caeee18ca668a835..4fed4d1cb277ab316a318cbc13b4f32222fd8939 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -22,10 +22,13 @@ namespace armarx::armem::robot
struct RobotState
{
+ using JointMap = std::map<std::string, float>;
+ using Pose = Eigen::Affine3f;
+
IceUtil::Time timestamp;
- Eigen::Affine3f globalPose;
- std::map<std::string, float> jointMap;
+ Pose globalPose;
+ JointMap jointMap;
};
struct Robot
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index e6a42e3a28b9db59d4dc24bf4dd34e5ee2074450..2eead70ac6845aedad5d4f1e5c69e9c0d8522347 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -21,10 +21,12 @@ armarx_add_library(
Eigen3::Eigen
aroncommon
HEADERS
-
./common/localization/types.h
./common/localization/TransformHelper.h
+ ./client/common/RobotReader.h
+ ./client/common/VirtualRobotReader.h
+
./client/localization/interfaces.h
./client/localization/TransformReader.h
./client/localization/TransformWriter.h
@@ -44,6 +46,9 @@ armarx_add_library(
SOURCES
./common/localization/TransformHelper.cpp
+ ./client/common/RobotReader.cpp
+ ./client/common/VirtualRobotReader.cpp
+
./client/localization/TransformReader.cpp
./client/localization/TransformWriter.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a635e51628f9b468e404eca10c4411b92171ade
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -0,0 +1,300 @@
+#include "RobotReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#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_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::robot_state
+{
+
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : memoryClient(component), transformReader(component) {}
+
+ void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ transformReader.registerPropertyDefinitions(def);
+
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
+ def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
+ def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
+ def->optional(properties.proprioceptionCoreSegment, propertyPrefix + "proprioceptionSegment");
+ }
+
+ void RobotReader::connect()
+ {
+ transformReader.connect();
+
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ std::optional<robot::Robot> RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ ARMARX_ERROR << "Unknown object " << name;
+ return std::nullopt;
+ }
+
+ return get(*description, timestamp);
+ }
+
+
+ robot::Robot RobotReader::get(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
+ {
+ robot::Robot robot
+ {
+ .description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp
+ };
+
+ synchronize(robot, timestamp);
+
+ return robot;
+ }
+
+ bool RobotReader::synchronize(robot::Robot& 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 false;
+ }
+
+ obj.config = std::move(*state);
+ return true;
+ }
+
+ std::optional<robot::RobotDescription> RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().all() // withName(properties.descriptionCoreSegment)
+ .providerSegments().all() // .withName(name)
+ .entities().all()
+ .snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
+ // clang-format on
+
+ ARMARX_INFO << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null";
+ return std::nullopt;
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_INFO << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotDescription(qResult.memory, name);
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
+ }
+
+ return std::nullopt;
+
+ }
+
+ std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ {
+ const auto jointMap = queryJointState(description, timestamp);
+ if (not jointMap)
+ {
+ return std::nullopt;
+ }
+
+ const auto globalPose = queryGlobalPose(description, timestamp);
+ if (not globalPose)
+ {
+ return std::nullopt;
+ }
+
+ return robot::RobotState
+ {
+ .timestamp = timestamp,
+ .globalPose = *globalPose,
+ .jointMap = *jointMap
+ };
+ }
+
+ std::optional<robot::RobotState::JointMap> RobotReader::queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ {
+ // 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.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeTime(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 getRobotJointState(qResult.memory, description.name);
+ }
+
+ std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ {
+ const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
+ if (not result)
+ {
+ return std::nullopt;
+ }
+
+ return result.transform.transform;
+ }
+
+
+ std::optional<robot::RobotState> RobotReader::getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment)
+ .getProviderSegment(name);
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ // TODO entitiesToRobotState()
+
+ 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);
+
+ // Here, we access the RobotUnit streaming data stored in the proprioception segment.
+ return robot::convertRobotState(instance);
+ }
+
+
+ std::optional<robot::RobotState::JointMap> RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // // clang-format off
+ // const armem::wm::ProviderSegment& providerSegment = memory
+ // .getCoreSegment(properties.proprioceptionCoreSegment)
+ // .getProviderSegment(name);
+ // // clang-format on
+ // const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ // // TODO entitiesToRobotState()
+
+ // 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);
+
+ // // Here, we access the RobotUnit streaming data stored in the proprioception segment.
+ // return robot::convertRobotState(instance);
+
+ return std::nullopt; // TODO implement
+ }
+
+
+
+ std::optional<robot::RobotDescription> RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.descriptionCoreSegment)
+ .getProviderSegment(name); // TODO(fabian.reister): all
+ // 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 robot::convertRobotDescription(instance);
+ }
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba4d3d4177831c4609c65eedb5b51094b37d4e03
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -0,0 +1,85 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/armem_robot/client/interfaces.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
+
+namespace armarx::armem::robot_state
+{
+ class RobotReader:
+ virtual public robot::ReaderInterface
+ {
+ public:
+ RobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~RobotReader() = default;
+
+ void connect();
+
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+
+ std::optional<robot::Robot> get(const std::string& name, const armem::Time& timestamp) override;
+ robot::Robot get(const robot::RobotDescription& description, const armem::Time& timestamp) override;
+
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
+ std::optional<robot::RobotState::JointMap> queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+ std::optional<robot::RobotState::Pose> queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+
+ private:
+
+
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::RobotState::JointMap> getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+ struct Properties
+ {
+ std::string memoryName = "RobotStateMemory";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
+ std::string proprioceptionCoreSegment = "Proprioception";
+ } properties;
+
+ const std::string propertyPrefix = "mem.robot_state.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
+
+ client::robot_state::localization::TransformReader transformReader;
+ };
+
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97633b788dcfae4cd93d6a1fd7a72850a9a009b1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -0,0 +1,73 @@
+#include "VirtualRobotReader.h"
+
+#include <optional>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+
+
+namespace armarx::armem::robot_state
+{
+
+ VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
+ RobotReader(component)
+ {
+ }
+
+ void VirtualRobotReader::connect()
+ {
+ RobotReader::connect();
+ }
+
+ // TODO(fabian.reister): register property defs
+ void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ RobotReader::registerPropertyDefinitions(def);
+ }
+
+ bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp)
+ {
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+
+ const robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{package, robot.getFilename()}};
+
+ const auto robotState = queryState(robotDescription, timestamp);
+ if (not robotState)
+ {
+ return false;
+ }
+
+ robot.setJointValues(robotState->jointMap);
+ robot.setGlobalPose(robotState->globalPose.matrix());
+
+ return true;
+ }
+
+ VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ {
+ ARMARX_INFO << "Querying robot description for robot '" << name << "'";
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ return nullptr;
+ }
+
+ const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
+
+ return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+
+ }
+
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..91e5392d2abd5b4f65678b35c0bf652d490c0e13
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotReader.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+namespace armarx::armem::robot_state
+{
+ /**
+ * @brief The VirtualRobotReader class.
+ *
+ * The aim of this class is to obtain a virtual robot instance and synchronize it
+ * with the data (joint positions, global pose, ...) stored in the working memory.
+ *
+ */
+ class VirtualRobotReader : virtual public RobotReader
+ {
+ public:
+ VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~VirtualRobotReader() = default;
+
+ void connect();
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+
+ VirtualRobot::RobotPtr getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode = VirtualRobot::RobotIO::RobotDescription::eStructure);
+ };
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index e7b55ef6fb1c47b91802c8191ec9bcd0018a60d7..ea636eeb11b0b274d8fbf113616883a001d696b9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -22,6 +22,7 @@
#include "TransformReader.h"
#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
#include <algorithm>
#include <iterator>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index fc789a7d9f7ea313bbf1c6c4edf088c9ecb257d0..3d7bf59eef0f97f18cf58087522630f87c9b0cbd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -71,7 +71,7 @@ namespace armarx::armem::client::robot_state::localization
} properties;
- const std::string propertyPrefix = "mem.localization.";
+ const std::string propertyPrefix = "mem.robot_state.";
armem::ClientReaderComponentPluginUser& memoryClient;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 8755cee1384432de6b9ca995d3382303d5934d7f..2327c141840bc74f8b43ab14df03b03f2b98ccdf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -13,7 +13,6 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package RobotAPI::ArmarXObjects::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -21,8 +20,8 @@
*/
#include "TransformWriter.h"
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
@@ -47,7 +46,8 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <optional>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+
namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 1aea36345abb2b8a1252d525ed016593ee414a06..aa9e295bf19b34229697ad43725884469c9d1172 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -74,7 +74,7 @@ namespace armarx::armem::client::robot_state::localization
std::string localizationSegment = "Localization";
} properties;
- const std::string propertyPrefix = "mem.localization.";
+ const std::string propertyPrefix = "mem.robot_state.";
armem::ClientWriterComponentPluginUser& memoryClient;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index 207660c68325fee922a7497ba771c014d597ef71..7f0c7c3396c7688ebe50b854579b107e9cd55b6f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -37,11 +37,15 @@ namespace armarx::armem::common::robot_state::localization
enum class Status
{
Success,
+ Error,
ErrorLookupIntoFuture,
ErrorFrameNotAvailable
} status;
- explicit operator bool() const { return status == Status::Success; }
+ explicit operator bool() const
+ {
+ return status == Status::Success;
+ }
std::string errorMessage = "";
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 577d4ce120fa4ad7d12a7645bead65124092d5fd..826c5b484768686e2edb8cee409df4136bcc85ab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -8,6 +8,8 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
#include "RobotAPI/libraries/armem_robot/types.h"
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
@@ -119,11 +121,17 @@ namespace armarx::armem::server::robot_state::description
ARMARX_CHECK_NOT_NULL(kinematicUnit);
const auto robotName = kinematicUnit->getRobotName();
+ const auto robotFilename = kinematicUnit->getRobotFilename();
+
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
const robot::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
- .xml = {"Armar6RT", kinematicUnit->getRobotFilename()}}; // FIXME
+ .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
storeRobotDescription(robotDescription);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 21bfd98fe652e9973478769640ab9d06c5ef7c21..2f603cf4961d90342fbd74770a0ef4d0d181c033 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -111,4 +111,4 @@ namespace armarx::armem::server::robot_state::description
};
-} // namespace armarx::armem::server::robot_state::localization
+} // namespace armarx::armem::server::robot_state::description