From c7e532b47fc435c63b00c01d5befcd66c625c8e6 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 29 Nov 2023 16:54:05 +0100
Subject: [PATCH] Make configurable
---
.../MemoryToDebugObserver/Component.cpp | 66 ++++++++-----
.../client/MemoryToDebugObserver/Component.h | 6 +-
.../client/util/MemoryToDebugObserver.cpp | 97 ++++++++++++++-----
.../armem/client/util/MemoryToDebugObserver.h | 19 ++++
4 files changed, 138 insertions(+), 50 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
index 6d42b4106..89782e6df 100644
--- a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
@@ -22,6 +22,8 @@
#include "Component.h"
+#include <SimoxUtility/json.h>
+
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
@@ -34,6 +36,39 @@ namespace armarx::memory_to_debug_observer
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
+ {
+ 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.memoryToDebugObserver.plottedValues.push_back({
+ .entityID = robotEntityId,
+ .aronPath = {{"joints", attribute, jointName}},
+ });
+ }
+ }
+
+ simox::json::json j = properties.memoryToDebugObserver;
+ properties.memoryToDebugObserverJson = j.dump();
+ }
+ defs->optional(properties.memoryToDebugObserverJson,
+ "p.memoryToDebugObserverJson",
+ "Configuration of MemoryToDebugObserver in JSON format.");
+
+ defs->optional(properties.pollFrequencyHz, "p.pollFrequencyHz");
+
return defs;
}
@@ -52,27 +87,9 @@ namespace armarx::memory_to_debug_observer
{
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.memoryToDebugObserver.plottedValues.push_back({
- .entityID = robotEntityId,
- .aronPath = {{"joints", attribute, jointName}},
- });
- }
+ simox::json::json j = simox::json::json::parse(properties.memoryToDebugObserverJson);
+ j.get_to(properties.memoryToDebugObserver);
}
}
@@ -108,7 +125,7 @@ namespace armarx::memory_to_debug_observer
armem::client::util::MemoryToDebugObserver memoryToDebugObserver{
properties.memoryToDebugObserver, services};
- Frequency frequency = Frequency::Hertz(30);
+ Frequency frequency = Frequency::Hertz(properties.pollFrequencyHz);
Metronome metronome(frequency);
while (task and not task->isStopped())
{
@@ -122,11 +139,10 @@ namespace armarx::memory_to_debug_observer
{
using namespace armarx::RemoteGui::Client;
- if (tab.queryResult)
- {
- }
+ tab.group = GroupBox();
+ tab.group.setLabel("Todo ...");
- VBoxLayout root = {tab.queryResultGroup, VSpacer()};
+ VBoxLayout root = {tab.group, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
index 02b4bb249..07965a053 100644
--- a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
@@ -81,6 +81,9 @@ namespace armarx::memory_to_debug_observer
struct Properties
{
armem::client::util::MemoryToDebugObserver::Properties memoryToDebugObserver;
+ std::string memoryToDebugObserverJson;
+
+ float pollFrequencyHz = 30;
};
Properties properties;
@@ -91,8 +94,7 @@ namespace armarx::memory_to_debug_observer
{
std::atomic_bool rebuild = false;
- std::optional<armem::wm::Memory> queryResult;
- RemoteGui::Client::GroupBox queryResultGroup;
+ RemoteGui::Client::GroupBox group;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
index 5e8f44930..89e2cf20c 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -23,6 +23,7 @@
#include "MemoryToDebugObserver.h"
#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/armem/core/json_conversions.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>
@@ -104,12 +105,20 @@ namespace armarx::armem::client::util
Visitor visitor(services.debugObserver);
std::stringstream log;
+
+ // Group by memory segment to reduce number of queries.
+ std::map<MemoryID, std::vector<const MemoryValueID*>> valuesPerProviderSegment;
for (const MemoryValueID& valueId : properties.plottedValues)
+ {
+ valuesPerProviderSegment[valueId.entityID.getProviderSegmentID()].push_back(&valueId);
+ }
+
+ for (const auto& [providerSegmentID, values] : valuesPerProviderSegment)
{
armem::client::Reader* reader = nullptr;
try
{
- reader = getReader(valueId.entityID);
+ reader = getReader(providerSegmentID);
}
catch (armem::error::CouldNotResolveMemoryServer& e)
{
@@ -118,35 +127,47 @@ namespace armarx::armem::client::util
}
ARMARX_CHECK_NOT_NULL(reader);
- std::optional<armem::wm::EntitySnapshot> snapshot =
- reader->getLatestSnapshotIn(valueId.entityID);
-
- if (not snapshot.has_value())
+ const QueryResult result = reader->getLatestSnapshotsIn(providerSegmentID);
+ if (not result.success)
{
- log << "\nDid not find snapshot in entity " << valueId.entityID << ".";
+ log << "Query to provider segment " << providerSegmentID
+ << " failed: " << result.errorMessage;
continue;
}
- aron::data::DictPtr data = snapshot->getInstance(0).data();
- aron::data::VariantPtr valueVariant = data->navigateAbsolute(valueId.aronPath);
- if (not valueVariant)
+ for (const MemoryValueID* valueId : values)
{
- log << "\nDid not find " << valueId.aronPath.toString() << " in entity snapshot "
- << snapshot->id() << ".";
- continue;
+ const wm::Entity* entity = result.memory.findEntity(valueId->entityID);
+ if (entity == nullptr)
+ {
+ log << "\nDid not find entity " << valueId->entityID << " in provider segment "
+ << providerSegmentID << ".";
+ continue;
+ }
+
+ const wm::EntityInstance& instance = entity->getLatestInstance();
+ aron::data::VariantPtr valueVariant =
+ instance.data()->navigateAbsolute(valueId->aronPath);
+ if (not valueVariant)
+ {
+ log << "\nDid not find " << valueId->aronPath.toString()
+ << " in entity instance " << instance.id() << ".";
+ continue;
+ }
+
+ visitor.channelName = makeChannelName(valueId->entityID);
+ visitor.datafieldName = makeDatafieldName(valueId->aronPath);
+
+ aron::data::visit(visitor, valueVariant);
}
-
- 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 "
+ ARMARX_INFO << deactivateSpam(60)
+ << "Encountered issues while sending memory values to the debug observer "
"for plotting: "
<< log.str();
}
@@ -171,20 +192,50 @@ namespace armarx::armem::client::util
Reader*
MemoryToDebugObserver::getReader(const MemoryID& memoryID)
{
- armem::MemoryID key = memoryID.getProviderSegmentID();
-
- auto it = memoryReaders.find(key);
+ auto it = memoryReaders.find(memoryID);
if (it != memoryReaders.end())
{
return &it->second;
}
else
{
- armem::client::Reader reader = services.memoryNameSystem.getReader(key);
+ armem::client::Reader reader = services.memoryNameSystem.getReader(memoryID);
- auto [it, _] = memoryReaders.emplace(key, reader);
+ auto [it, _] = memoryReaders.emplace(memoryID, reader);
return &it->second;
}
}
} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+
+ void
+ util::to_json(simox::json::json& j, const MemoryValueID& id)
+ {
+ j["entityID"] = id.entityID.getItems();
+ j["aronPath"] = id.aronPath.getPath();
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryValueID& id)
+ {
+ id.entityID = MemoryID::fromItems(j.at("entityID").get<std::vector<std::string>>());
+ id.aronPath = {j.at("aronPath").get<std::vector<std::string>>()};
+ }
+
+ void
+ util::to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p)
+ {
+ j["plottedValues"] = p.plottedValues;
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p)
+ {
+ j.at("plottedValues").get_to(p.plottedValues);
+ }
+
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
index c254db3a3..918b78f90 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
@@ -22,6 +22,8 @@
#pragma once
+#include <SimoxUtility/json/json.h>
+
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
@@ -48,17 +50,28 @@ namespace armarx::armem::client::util
class MemoryToDebugObserver
{
public:
+ /**
+ * @brief Configuration.
+ *
+ * Can be converted to and from JSON.
+ */
struct Properties
{
std::vector<MemoryValueID> plottedValues;
};
+ /**
+ * @brief Required services.
+ */
struct Services
{
MemoryNameSystem& memoryNameSystem;
armarx::DebugObserverHelper& debugObserver;
};
+ /**
+ * @brief Constructor.
+ */
MemoryToDebugObserver(const Properties& properties, const Services& services);
/**
@@ -78,4 +91,10 @@ namespace armarx::armem::client::util
std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
};
+ void to_json(simox::json::json& j, const MemoryValueID& id);
+ void from_json(const simox::json::json& j, MemoryValueID& id);
+
+ void to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p);
+ void from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p);
+
} // namespace armarx::armem::client::util
--
GitLab