diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
index 70e1acea7b4b2af6339f14ef01cae92ed733c420..6d42b4106f87d9999d13724f48c39524b28e8a99 100644
--- a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
@@ -25,11 +25,6 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include <RobotAPI/libraries/armem/core/error/mns.h>
-#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
-#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
-
namespace armarx::memory_to_debug_observer
{
@@ -39,7 +34,6 @@ namespace armarx::memory_to_debug_observer
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
-
return defs;
}
@@ -74,7 +68,7 @@ namespace armarx::memory_to_debug_observer
};
for (const std::string& attribute : attributes)
{
- properties.plottedValues.push_back(MemoryValueID{
+ properties.memoryToDebugObserver.plottedValues.push_back({
.entityID = robotEntityId,
.aronPath = {{"joints", attribute, jointName}},
});
@@ -96,6 +90,7 @@ namespace armarx::memory_to_debug_observer
Component::onDisconnectComponent()
{
task->stop();
+ task = nullptr;
}
void
@@ -106,166 +101,22 @@ namespace armarx::memory_to_debug_observer
void
Component::runLoop()
{
+ armem::client::util::MemoryToDebugObserver::Services services{
+ .memoryNameSystem = memoryNameSystem(),
+ .debugObserver = getDebugObserverComponentPlugin(),
+ };
+ armem::client::util::MemoryToDebugObserver memoryToDebugObserver{
+ properties.memoryToDebugObserver, services};
+
Frequency frequency = Frequency::Hertz(30);
Metronome metronome(frequency);
- while (not task->isStopped())
+ while (task and not task->isStopped())
{
- loopOnce();
+ memoryToDebugObserver.pollOnce();
metronome.waitForNextTick();
}
}
- class Visitor : public aron::data::ConstVariantVisitor
- {
- public:
- Visitor(armarx::plugins::DebugObserverComponentPlugin& debugObserver) :
- debugObserver{debugObserver}
- {
- }
-
- // void visitAronVariant(const data::DictPtr&) override;
- // void visitAronVariant(const data::ListPtr&) override;
- // void visitAronVariant(const data::NDArrayPtr&) override;
- void
- visitAronVariant(const aron::data::IntPtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- void
- visitAronVariant(const aron::data::LongPtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- void
- visitAronVariant(const aron::data::FloatPtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- void
- visitAronVariant(const aron::data::DoublePtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- void
- visitAronVariant(const aron::data::BoolPtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- void
- visitAronVariant(const aron::data::StringPtr& v) override
- {
- setDatafield(v->getValue());
- }
-
- template <class ValueT>
- void
- setDatafield(const ValueT& value)
- {
- debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
- ++count;
- }
-
- armarx::plugins::DebugObserverComponentPlugin& debugObserver;
- std::string channelName;
- std::string datafieldName;
-
- int count = 0;
- };
-
- void
- Component::loopOnce()
- {
- Visitor visitor(getDebugObserverComponentPlugin());
-
- std::stringstream log;
- for (const MemoryValueID& valueId : properties.plottedValues)
- {
- armem::client::Reader* reader = getReader(valueId.entityID, log);
- if (reader == nullptr)
- {
- // Log is written in getReader.
- continue;
- }
-
- std::optional<armem::wm::EntitySnapshot> snapshot =
- reader->getLatestSnapshotIn(valueId.entityID);
-
- if (not snapshot.has_value())
- {
- log << "\nDid not find snapshot in entity " << valueId.entityID << ".";
- continue;
- }
-
- aron::data::DictPtr data = snapshot->getInstance(0).data();
- aron::data::VariantPtr valueVariant = data->navigateAbsolute(valueId.aronPath);
- if (not valueVariant)
- {
- log << "\nDid not find " << valueId.aronPath.toString() << " in entity snapshot "
- << snapshot->id() << ".";
- continue;
- }
-
-
- auto makeDatafieldName = [](const aron::Path& path) -> std::string
- {
- // The first element is always "ARON->", which we can discard for readability.
- std::string str = path.toString();
- str =
- simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
- str = simox::alg::replace_all(str, path.getDelimeter(), ">");
- return str;
- };
-
- visitor.channelName = simox::alg::replace_all(valueId.entityID.str(), "/", ">");
- visitor.datafieldName = makeDatafieldName(valueId.aronPath);
-
- aron::data::visit(visitor, valueVariant);
- }
-
- ARMARX_INFO << "Sending debug observer batch with " << visitor.count << " values ...";
- sendDebugObserverBatch();
-
- if (not log.str().empty())
- {
- ARMARX_INFO << "Encountered issues while sending memory values to the debug observer "
- "for plotting: "
- << log.str();
- }
- }
-
- armem::client::Reader*
- Component::getReader(const armem::MemoryID& memoryID, std::stringstream& log)
- {
- armem::MemoryID key = memoryID.getProviderSegmentID();
-
- auto it = memoryReaders.find(key);
- if (it != memoryReaders.end())
- {
- return &it->second;
- }
- else
- {
- armem::client::Reader reader;
- try
- {
- reader = memoryNameSystem().getReader(key);
- }
- catch (armem::error::CouldNotResolveMemoryServer& e)
- {
- log << "\n" << e.what();
- return nullptr;
- }
-
- auto [it, _] = memoryReaders.emplace(key, reader);
- return &it->second;
- }
- }
-
void
Component::createRemoteGuiTab()
{
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
index ad76e6c9b4285b58142756b9a5df4f18e1726a11..02b4bb24909d77599065554df46b9e8deac32d59 100644
--- a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
@@ -23,31 +23,17 @@
#pragma once
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h>
namespace armarx::memory_to_debug_observer
{
-
- struct MemoryValueID
- {
- armem::MemoryID entityID;
- aron::Path aronPath;
- };
-
- class MemoryToDebugObserver
- {
- };
-
/**
* @defgroup Component-MemoryToDebugObserver MemoryToDebugObserver
* @ingroup RobotAPI-Components
@@ -58,6 +44,9 @@ namespace armarx::memory_to_debug_observer
* @class Component
* @ingroup Component-MemoryToDebugObserver
* @brief Implementation of \ref Component-MemoryToDebugObserver.
+ *
+ * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+ * implementation.
*/
class Component :
virtual public armarx::Component,
@@ -86,21 +75,16 @@ namespace armarx::memory_to_debug_observer
void onExitComponent() override;
void runLoop();
- void loopOnce();
-
- armem::client::Reader* getReader(const armem::MemoryID& memoryID, std::stringstream& log);
private:
struct Properties
{
- std::vector<MemoryValueID> plottedValues;
+ armem::client::util::MemoryToDebugObserver::Properties memoryToDebugObserver;
};
Properties properties;
- std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
-
armarx::RunningTask<Component>::pointer_type task;
struct RemoteGuiTab : RemoteGui::Client::Tab
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 9030bd17597399abf79e3413ec71a789f3a113ca..670fef073807ec08cf5938f77a5876f099d89f07 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -7,6 +7,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
+ DebugObserverHelper
RemoteGui
aron
aroncommon
@@ -64,8 +65,9 @@ set(LIB_FILES
client/plugins/PluginUser.cpp
client/plugins/Plugin.cpp
- client/util/SubscriptionHandle.cpp
client/util/MemoryListener.cpp
+ client/util/MemoryToDebugObserver.cpp
+ client/util/SubscriptionHandle.cpp
client/util/SimpleReaderBase.cpp
client/util/SimpleWriterBase.cpp
@@ -157,10 +159,11 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
- client/util/SubscriptionHandle.h
+ client/util/MemoryToDebugObserver.h
client/util/MemoryListener.h
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
+ client/util/SubscriptionHandle.h
server.h
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e8f44930e104219c8c2a33c34ecc0a9e18d9b5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -0,0 +1,190 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "MemoryToDebugObserver.h"
+
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::client::util
+{
+
+ MemoryToDebugObserver::MemoryToDebugObserver(const Properties& properties,
+ const Services& services) :
+ properties(properties), services(services)
+ {
+ services.debugObserver.setDebugObserverBatchModeEnabled(true);
+ }
+
+ class Visitor : public aron::data::ConstVariantVisitor
+ {
+ public:
+ Visitor(armarx::DebugObserverHelper& debugObserver) : debugObserver{debugObserver}
+ {
+ }
+
+ // void visitAronVariant(const data::DictPtr&) override;
+ // void visitAronVariant(const data::ListPtr&) override;
+ // void visitAronVariant(const data::NDArrayPtr&) override;
+ void
+ visitAronVariant(const aron::data::IntPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::LongPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::FloatPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::DoublePtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::BoolPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::StringPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ template <class ValueT>
+ void
+ setDatafield(const ValueT& value)
+ {
+ debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
+ ++count;
+ }
+
+ armarx::DebugObserverHelper& debugObserver;
+ std::string channelName;
+ std::string datafieldName;
+
+ int count = 0;
+ };
+
+ void
+ MemoryToDebugObserver::pollOnce()
+ {
+ Visitor visitor(services.debugObserver);
+
+ std::stringstream log;
+ for (const MemoryValueID& valueId : properties.plottedValues)
+ {
+ armem::client::Reader* reader = nullptr;
+ try
+ {
+ reader = getReader(valueId.entityID);
+ }
+ catch (armem::error::CouldNotResolveMemoryServer& e)
+ {
+ log << "\n" << e.what();
+ continue;
+ }
+ ARMARX_CHECK_NOT_NULL(reader);
+
+ std::optional<armem::wm::EntitySnapshot> snapshot =
+ reader->getLatestSnapshotIn(valueId.entityID);
+
+ if (not snapshot.has_value())
+ {
+ log << "\nDid not find snapshot in entity " << valueId.entityID << ".";
+ continue;
+ }
+
+ aron::data::DictPtr data = snapshot->getInstance(0).data();
+ aron::data::VariantPtr valueVariant = data->navigateAbsolute(valueId.aronPath);
+ if (not valueVariant)
+ {
+ log << "\nDid not find " << valueId.aronPath.toString() << " in entity snapshot "
+ << snapshot->id() << ".";
+ continue;
+ }
+
+ visitor.channelName = makeChannelName(valueId.entityID);
+ visitor.datafieldName = makeDatafieldName(valueId.aronPath);
+
+ aron::data::visit(visitor, valueVariant);
+ }
+
+ services.debugObserver.sendDebugObserverBatch();
+
+ if (not log.str().empty())
+ {
+ ARMARX_INFO << "Encountered issues while sending memory values to the debug observer "
+ "for plotting: "
+ << log.str();
+ }
+ }
+
+ std::string
+ MemoryToDebugObserver::makeChannelName(const MemoryID& memoryID)
+ {
+ return simox::alg::replace_all(memoryID.str(), "/", ">");
+ }
+
+ std::string
+ MemoryToDebugObserver::makeDatafieldName(const aron::Path& path)
+ {
+ // The first element is always "ARON->", which we can discard for readability.
+ std::string str = path.toString();
+ str = simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
+ str = simox::alg::replace_all(str, path.getDelimeter(), ">");
+ return str;
+ }
+
+ Reader*
+ MemoryToDebugObserver::getReader(const MemoryID& memoryID)
+ {
+ armem::MemoryID key = memoryID.getProviderSegmentID();
+
+ auto it = memoryReaders.find(key);
+ if (it != memoryReaders.end())
+ {
+ return &it->second;
+ }
+ else
+ {
+ armem::client::Reader reader = services.memoryNameSystem.getReader(key);
+
+ auto [it, _] = memoryReaders.emplace(key, reader);
+ return &it->second;
+ }
+ }
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..c254db3a33235b2400d5d3138a642b97a7316209
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
@@ -0,0 +1,81 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+namespace armarx::armem::client::util
+{
+ /**
+ * @brief ID of an ARON value in the memory.
+ */
+ struct MemoryValueID
+ {
+ armem::MemoryID entityID;
+ aron::Path aronPath;
+ };
+
+ /**
+ * @brief Transfers data from memory servers to the DebugObserver.
+ *
+ * This allows to visualize the values in the LivePlotter.
+ */
+ class MemoryToDebugObserver
+ {
+ public:
+ struct Properties
+ {
+ std::vector<MemoryValueID> plottedValues;
+ };
+
+ struct Services
+ {
+ MemoryNameSystem& memoryNameSystem;
+ armarx::DebugObserverHelper& debugObserver;
+ };
+
+ MemoryToDebugObserver(const Properties& properties, const Services& services);
+
+ /**
+ * @brief Query values from the memory and send them to the debug observer.
+ */
+ void pollOnce();
+
+ private:
+ static std::string makeChannelName(const armem::MemoryID& memoryID);
+ static std::string makeDatafieldName(const aron::Path& path);
+
+ armem::client::Reader* getReader(const armem::MemoryID& memoryID);
+
+ Properties properties;
+ Services services;
+
+ std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
+ };
+
+} // namespace armarx::armem::client::util