From e45b87e5d7ca4bc2cc5b371ca2d15c2c3b4a54dc Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 29 Nov 2023 15:07:54 +0100
Subject: [PATCH] Implement MemoryPlotter core functionality
---
.../client/MemoryPlotter/MemoryPlotter.cpp | 190 +++++++++++++++++-
.../client/MemoryPlotter/MemoryPlotter.h | 18 +-
2 files changed, 204 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.cpp b/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.cpp
index e442247df..d292688d7 100644
--- a/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.cpp
+++ b/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.cpp
@@ -25,9 +25,15 @@
#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
{
+
armarx::PropertyDefinitionsPtr
MemoryPlotter::createPropertyDefinitions()
{
@@ -38,6 +44,10 @@ namespace armarx
return defs;
}
+ MemoryPlotter::MemoryPlotter()
+ {
+ }
+
std::string
MemoryPlotter::getDefaultName() const
{
@@ -47,6 +57,30 @@ namespace armarx
void
MemoryPlotter::onInitComponent()
{
+ DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled(true);
+
+ armem::MemoryID robotEntityId{"RobotState", "Proprioception", "Armar7", "Armar7"};
+
+ std::vector<std::string> jointNames{
+ "Neck_1_Yaw",
+ "Neck_2_Hemisphere_A",
+ "Neck_3_Hemisphere_B",
+ };
+
+ for (const std::string& jointName : jointNames)
+ {
+ std::vector<std::string> attributes{
+ "position",
+ "velocity",
+ };
+ for (const std::string& attribute : attributes)
+ {
+ properties.plottedValues.push_back(MemoryValueID{
+ .entityID = robotEntityId,
+ .aronPath = {{"joints", attribute, jointName}},
+ });
+ }
+ }
}
void
@@ -55,7 +89,7 @@ namespace armarx
createRemoteGuiTab();
RemoteGui_startRunningTask();
- task = new RunningTask<MemoryPlotter>(this, &MemoryPlotter::run);
+ task = new RunningTask<MemoryPlotter>(this, &MemoryPlotter::runLoop);
task->start();
}
@@ -71,16 +105,168 @@ namespace armarx
}
void
- MemoryPlotter::run()
+ MemoryPlotter::runLoop()
{
Frequency frequency = Frequency::Hertz(30);
Metronome metronome(frequency);
while (not task->isStopped())
{
+ loopOnce();
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
+ MemoryPlotter::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*
+ MemoryPlotter::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
MemoryPlotter::createRemoteGuiTab()
{
diff --git a/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.h b/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.h
index 5e3067985..e60c3dae4 100644
--- a/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.h
+++ b/source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.h
@@ -22,6 +22,7 @@
#pragma once
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -37,6 +38,13 @@
namespace armarx
{
+
+ struct MemoryValueID
+ {
+ armem::MemoryID entityID;
+ aron::Path aronPath;
+ };
+
/**
* @defgroup Component-MemoryPlotter MemoryPlotter
* @ingroup RobotAPI-Components
@@ -55,6 +63,8 @@ namespace armarx
virtual public armarx::LightweightRemoteGuiComponentPluginUser
{
public:
+ MemoryPlotter();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -72,15 +82,19 @@ namespace armarx
void onDisconnectComponent() override;
void onExitComponent() override;
- void run();
+ void runLoop();
+ void loopOnce();
+
+ armem::client::Reader* getReader(const armem::MemoryID& memoryID, std::stringstream& log);
private:
struct Properties
{
+ std::vector<MemoryValueID> plottedValues;
};
- Properties p;
+ Properties properties;
std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
--
GitLab