From 017d5e968912a94b650f8b418495d86ff4cbcefb Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 12 Jul 2021 09:30:44 +0200
Subject: [PATCH] Refactor RobotStateMemory component class, change name to
"RobotState"
---
.../server/RobotStateMemory/CMakeLists.txt | 17 +-
.../RobotStateMemory/RobotStateMemory.cpp | 427 ++++--------------
.../RobotStateMemory/RobotStateMemory.h | 75 +--
.../RobotStateMemory/RobotStateWriter.cpp | 202 +++++++++
.../RobotStateMemory/RobotStateWriter.h | 99 ++++
.../server/RobotStateMemory/RobotUnitData.cpp | 9 +
.../server/RobotStateMemory/RobotUnitData.h | 31 ++
.../RobotStateMemory/RobotUnitReader.cpp | 201 +++++++++
.../server/RobotStateMemory/RobotUnitReader.h | 84 ++++
.../RobotStateMemory/aron_conversions.cpp | 69 +++
.../RobotStateMemory/aron_conversions.h | 23 +
.../client/common/RobotReader.h | 2 +-
12 files changed, 847 insertions(+), 392 deletions(-)
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp
create mode 100644 source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 137453970..a25161ff2 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -2,20 +2,31 @@ armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
+ # ArmarXCore
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXCoreComponentPlugins
+ # ArmarXGUI
ArmarXGuiComponentPlugins
- RobotAPICore
- RobotAPIInterfaces
- RobotAPIComponentPlugins
+ # RobotAPI
+ RobotAPICore RobotAPIInterfaces
+ RobotAPIComponentPlugins
armem
armem_robot_state
)
set(SOURCES
+ aron_conversions.cpp
RobotStateMemory.cpp
+ RobotStateWriter.cpp
+ RobotUnitData.cpp
+ RobotUnitReader.cpp
)
set(HEADERS
+ aron_conversions.h
RobotStateMemory.h
+ RobotStateWriter.h
+ RobotUnitData.h
+ RobotUnitReader.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 111cced66..653aaaf6e 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -23,36 +23,27 @@
#include "RobotStateMemory.h"
// STD
-#include <algorithm>
-#include <iostream>
-#include <fstream>
#include <memory>
-// Eigen
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
// Simox
#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+
+#include "aron_conversions.h"
+
namespace armarx::armem::server::robot_state
{
+
RobotStateMemory::RobotStateMemory()
: descriptionSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
@@ -62,7 +53,13 @@ namespace armarx::armem::server::robot_state
server::ComponentPluginUser::workingMemoryMutex),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
+ addPlugin(debugObserver);
+ ARMARX_CHECK_NOT_NULL(debugObserver);
+ addPlugin(robotUnit.plugin);
+ ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
+ robotUnit.reader.setTag(getName());
+ robotUnit.writer.setTag(getName());
}
RobotStateMemory::~RobotStateMemory() = default;
@@ -79,18 +76,23 @@ namespace armarx::armem::server::robot_state
const std::string robotUnitPrefix{sensorValuePrefix};
- defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
- defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
- defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
+ defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
+ "Prefix of all sensor values.");
+ defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
+ "The size of the entity snapshot to send to the memory. Minimum is 1.")
+ .setMin(1);
+ defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
+ "The frequency to store values in Hz. All other values get discarded. "
+ "Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
+ .setMin(1).setMax(ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ defs->optional(robotUnit.configPath, robotUnitPrefix + "ConfigPath",
+ "Specify a configuration file to group the sensor values specifically.");
descriptionSegment.defineProperties(defs);
proprioceptionSegment.defineProperties(defs);
localizationSegment.defineProperties(defs);
commonVisu.defineProperties(defs);
- setRobotUnitAsOptionalDependency();
-
return defs;
}
@@ -103,14 +105,13 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onInitComponent()
{
-
descriptionSegment.init();
proprioceptionSegment.init();
localizationSegment.init();
commonVisu.init();
- robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
- robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
Ice::StringSeq includePaths;
auto packages = armarx::Application::GetProjectDependencies();
@@ -129,39 +130,46 @@ namespace armarx::armem::server::robot_state
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
-
- // workingMemory.name() = workingMemoryName;
+ ArmarXDataPath::getAbsolutePath(robotUnit.configPath, robotUnit.configPath, includePaths);
}
void RobotStateMemory::onConnectComponent()
{
+ ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
- if (getRobotUnit())
+ if (robotUnit.plugin->getRobotUnit())
{
- waitUntilRobotUnitIsRunning();
+ robotUnit.plugin->waitUntilRobotUnitIsRunning();
}
+ RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
+ ARMARX_CHECK_NOT_NULL(robotUnitPrx->getKinematicUnit());
- ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
+ descriptionSegment.connect(getArvizClient(), robotUnitPrx);
- descriptionSegment.connect(getArvizClient(), getRobotUnit());
-
- proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
- toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
+ proprioceptionSegment.connect(getArvizClient(), robotUnitPrx);
localizationSegment.connect(getArvizClient());
- commonVisu.connect(
- getArvizClient()
- );
+ commonVisu.connect(getArvizClient());
- cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
- handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
- keys = handler->getDataDescription();
+ robotUnit.reader.connect(*robotUnit.plugin, *debugObserver);
+ robotUnit.writer.connect(*debugObserver);
+ robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
- robotUnitConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
- robotUnitStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
+ robotUnit.reader.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.reader.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
+ );
+ }, "Robot Unit Reader");
+ robotUnit.writer.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.writer.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
+ iceMemory, localizationSegment, workingMemoryMutex
+ );
+ }, "Robot State Writer");
startRobotUnitStream();
}
@@ -178,340 +186,57 @@ namespace armarx::armem::server::robot_state
stopRobotUnitStream();
}
+
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotStateMemory::convertRobotUnitStreamingDataToAron()
- {
- auto& data = handler->getDataBuffer();
- if (data.size() == 0)
- {
- return;
- }
- auto& currentTimestep = data.back();
-
- ARMARX_DEBUG << "RobotUnitData: Generating new aron map for current timestep";
- auto start = std::chrono::high_resolution_clock::now();
- RobotUnitData convertedAndGroupedData;
- for (const auto& [nameEntry, dataEntry] : keys.entries)
- {
- std::string name = nameEntry;
- std::string jointOrWhateverName;
-
- std::string groupName = "";
- if (configSensorMapping.find(name) != configSensorMapping.end())
- {
- groupName = configSensorMapping.at(name);
- }
- else
- {
- const size_t first_dot_pos = name.find(".");
- const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
- if (second_dot_pos == std::string::npos)
- {
- ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
- continue;
- }
- groupName = name.substr(0, second_dot_pos);
-
- jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
-
- name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
- }
-
- RobotUnitData::RobotUnitDataGroup e;
- if (auto it = convertedAndGroupedData.groups.find(groupName); it == convertedAndGroupedData.groups.end())
- {
- // generate new dict for the group
- auto dict = std::make_shared<aron::datanavigator::DictNavigator>();
- auto encGroupName = std::make_shared<aron::datanavigator::StringNavigator>(groupName);
- dict->addElement("EncoderGroupName", encGroupName);
-
- auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
- dict->addElement("IterationId", iterId);
-
- const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
- dict->addElement("name", nameId);
-
- e.timestamp = currentTimestep.timestampUSec;
- e.name = groupName;
- e.data = dict;
- }
- else
- {
- // reuse existing entry
- e = it->second;
- }
-
- switch (dataEntry.type)
- {
- case RobotUnitDataStreaming::NodeTypeFloat:
- {
- float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::FloatNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeBool:
- {
- bool value = RobotUnitDataStreamingReceiver::GetAs<bool>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::BoolNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeByte:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Byte>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeShort:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Short>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeInt:
- {
- int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Int>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::IntNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeLong:
- {
- long value = RobotUnitDataStreamingReceiver::GetAs<Ice::Long>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::LongNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- case RobotUnitDataStreaming::NodeTypeDouble:
- {
- double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(currentTimestep, dataEntry);
- auto aron = std::make_shared<aron::datanavigator::DoubleNavigator>(value);
- e.data->addElement(name, aron);
- break;
- }
- default:
- throw LocalException("The enum type should not exist! Perhaps someone changed the RobotUnitDataStreaming::NodeType enum definition?");
- }
-
- convertedAndGroupedData.groups.insert({groupName, e});
- }
- {
- std::lock_guard g{robotUnitDataMutex};
- robotUnitDataQueue.push(convertedAndGroupedData);
- }
-
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- }
-
- void RobotStateMemory::storeConvertedRobotUnitDataInMemory()
- {
- unsigned int size = 0;
- {
- std::lock_guard g{robotUnitDataMutex};
- size = robotUnitDataQueue.size(); // the size can only grow
- }
- if (size >= robotUnitMemoryBatchSize)
- {
- ARMARX_DEBUG << "RobotUnitData: Sending batch of " << robotUnitMemoryBatchSize << " timesteps to memory... The size of the queue is: " << size;
- auto start = std::chrono::high_resolution_clock::now();
-
- // send batch to memory
- armem::data::Commit proprioceptionCommit;
- armem::data::Commit odometryCommit;
-
- for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
- {
- std::lock_guard g{robotUnitDataMutex};
- const auto& convertedAndGroupedData = robotUnitDataQueue.front();
- for (const auto& [encName, encTimestep] : convertedAndGroupedData.groups)
- {
- ARMARX_CHECK_EQUAL(encName, encTimestep.name);
-
- auto entityID = robotUnitProviderID;
- entityID.entityName = encName;
-
- const auto& timeUSec = encTimestep.timestamp;
- const auto& aron = encTimestep.data;
-
- armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
- update.entityID = entityID;
- update.timeCreatedMicroSeconds = timeUSec;
- update.instancesData = {aron->toAronDictPtr()};
- }
-
- // odometry pose -> localization segment
- auto it = convertedAndGroupedData.groups.find("sens.Platform");
- if (it != convertedAndGroupedData.groups.end())
- {
- ARMARX_DEBUG << "Found odometry data.";
-
- const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
- const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
- const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
- const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
-
- Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
- odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
-
- // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
- const auto timestamp = armem::Time::now();
-
- const ::armarx::armem::robot_state::Transform transform
- {
- .header =
- {
- .parentFrame = OdometryFrame,
- .frame = "root", // TODO: robot root node
- .agent = robotUnitProviderID.providerSegmentName,
- .timestamp = timestamp
- },
- .transform = odometryPose
- };
-
- localizationSegment.storeTransform(transform);
-
- }
- else
- {
- ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
- }
-
-
- robotUnitDataQueue.pop();
- }
-
- auto results = commit(proprioceptionCommit);
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
-
- for (const auto& result : results.results)
- {
- if (!result.success)
- {
- ARMARX_WARNING << "Could not add data to memory. Error message: " << result.errorMessage;
- }
- }
- }
- }
-
- void RobotStateMemory::stopRobotUnitStream()
- {
- std::lock_guard g{startStopMutex};
- robotUnitConversionTask->stop();
- robotUnitStoringTask->stop();
- }
void RobotStateMemory::startRobotUnitStream()
{
- std::lock_guard g{startStopMutex};
- if (robotUnitConversionTask->isRunning() || robotUnitStoringTask->isRunning())
+ std::lock_guard lock{startStopMutex};
+ if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
{
- if (robotUnitConversionTask->isRunning() && robotUnitStoringTask->isRunning())
+ if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
{
return;
}
ARMARX_WARNING << "Found inconsistency in running tasks. Restarting all!";
stopRobotUnitStream();
}
-
- std::stringstream ss;
- ss << "Getting sensor values for:" << std::endl;
- for (const auto& [name, dataEntry] : keys.entries)
{
- std::string type = "";
- switch (dataEntry.type)
+ std::stringstream ss;
+ ss << "Getting sensor values for:" << std::endl;
+ for (const auto& [name, dataEntry] : robotUnit.reader.description.entries)
{
- case RobotUnitDataStreaming::NodeTypeBool:
- {
- type = "Bool";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeByte:
- {
- type = "Byte";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeShort:
- {
- type = "Short";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeInt:
- {
- type = "Int";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeLong:
- {
- type = "Long";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeFloat:
- {
- type = "Float";
- break;
- }
- case RobotUnitDataStreaming::NodeTypeDouble:
- {
- type = "Double";
- break;
- }
+ const std::string type = RobotUnitDataStreaming::DataEntryNames.to_name(dataEntry.type);
+ ss << "\t" << name << " (type: '" << type << "')" << std::endl;
}
- ss << "\t" << name << " (type: '" << type << "')" << std::endl;
+ ARMARX_INFO << ss.str();
}
- ARMARX_INFO << ss.str();
// parse config
- std::filesystem::path configPath(robotUnitConfigPath);
- configSensorMapping.clear();
- if (std::filesystem::is_regular_file(configPath))
+ if (std::filesystem::is_regular_file(robotUnit.configPath))
{
- ARMARX_INFO << "Found a configuration file at: " << robotUnitConfigPath;
- // a simple self-made parser for the config file. Extend it if you need to
- std::ifstream in(configPath);
- std::string line;
- while (std::getline(in, line))
- {
- if (line.size() > 0)
- {
- if (simox::alg::starts_with(line, "//"))
- {
- continue;
- }
-
- std::vector<std::string> mappings = simox::alg::split(line, ",");
- for (const auto& mapping : mappings)
- {
- std::vector<std::string> split = simox::alg::split(mapping, "=>");
- if (split.size() < 2)
- {
- ARMARX_WARNING << "A mapping in the RobotUnitConfig file is not valid. Ignoring it: " << mapping;
- continue;
- }
- for (unsigned int i = 0; i < split.size() - 1; ++i)
- {
- ARMARX_DEBUG << "Setting sensor value '" << split[i] << "' to group '" << split[split.size() - 1] << "'.";
- configSensorMapping.insert({split[i], split[split.size() - 1]});
- }
- }
- }
- }
+ ARMARX_INFO << "Found a configuration file at: " << robotUnit.configPath;
+ // A simple self-made parser for the config file. Extend it if you need to.
+ robotUnit.reader.configSensorMapping = RobotUnitReader::readConfig(robotUnit.configPath);
}
else
{
- ARMARX_INFO << "The config path '" << robotUnitConfigPath << "' is not valid.";
+ ARMARX_INFO << "The config path '" << robotUnit.configPath << "' is not valid.";
}
- robotUnitConversionTask->start();
- robotUnitStoringTask->start();
+ robotUnit.reader.task->start();
+ robotUnit.writer.task->start();
+ }
+
+
+ void RobotStateMemory::stopRobotUnitStream()
+ {
+ std::lock_guard lock{startStopMutex};
+ robotUnit.reader.task->stop();
+ robotUnit.writer.task->stop();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index e8174416e..d249678b6 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -24,18 +24,18 @@
// STD
#include <atomic>
-#include <thread>
+#include <optional>
#include <queue>
+#include <thread>
// Simox
#include <SimoxUtility/meta/enum/adapt_enum.h>
// ArmarX
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
// BaseClass
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
@@ -46,6 +46,16 @@
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include "RobotUnitData.h"
+#include "RobotUnitReader.h"
+#include "RobotStateWriter.h"
+
+
+namespace armarx::plugins
+{
+ class DebugObserverComponentPlugin;
+ class RobotUnitComponentPlugin;
+}
namespace armarx::armem::server::robot_state
{
@@ -63,18 +73,18 @@ namespace armarx::armem::server::robot_state
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser,
virtual public armarx::ArVizComponentPluginUser
{
public:
- RobotStateMemory();
- virtual ~RobotStateMemory();
+ RobotStateMemory();
+ virtual ~RobotStateMemory() override;
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
+
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -90,17 +100,21 @@ namespace armarx::armem::server::robot_state
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
private:
- // RobotUnit Streaming
- void convertRobotUnitStreamingDataToAron();
- void storeConvertedRobotUnitDataInMemory();
+
+ /// Reads from `robotUnitDataQueue` and commits into memory.
+ void convertRobotUnitStreamingDataToAronTask();
+ /// Reads from `robotUnitDataQueue` and commits into memory.
+ void storeConvertedRobotUnitDataInMemoryTask();
void startRobotUnitStream();
void stopRobotUnitStream();
+
private:
- std::string workingMemoryName = "RobotStateMemory";
- bool addCoreSegmentOnUsage = false;
+
+ armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
mutable std::recursive_mutex startStopMutex;
@@ -118,41 +132,28 @@ namespace armarx::armem::server::robot_state
// Joint visu for all segments => robot pose and configuration
Visu commonVisu;
- // RobotUnit stuff
- RobotUnitDataStreaming::DataStreamingDescription keys;
- std::vector<RobotUnitDataStreaming::DataEntry> keysList;
- RobotUnitDataStreaming::Config cfg;
- RobotUnitDataStreamingReceiverPtr handler;
- std::map<std::string, std::string> configSensorMapping;
- struct RobotUnitData
+ struct RobotUnit
{
- struct RobotUnitDataGroup
- {
- long timestamp;
- std::string name;
- aron::datanavigator::DictNavigatorPtr data;
- };
-
- std::map<std::string, RobotUnitDataGroup> groups;
+ int pollFrequency = 50;
+ std::string configPath = "NO CONFIG SET";
+
+ armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
+ RobotUnitReader reader;
+ RobotStateWriter writer;
+
+ // queue
+ std::queue<RobotUnitData> dataQueue;
+ mutable std::mutex dataMutex;
};
+ RobotUnit robotUnit;
+
// params
static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
static constexpr const char* sensorValuePrefix = "RobotUnit.";
- int robotUnitPollFrequency = 50;
- std::string robotUnitSensorPrefix = "sens.*";
- unsigned int robotUnitMemoryBatchSize = 50;
- std::string robotUnitConfigPath = "NO CONFIG SET";
-
- // queue
- std::queue<RobotUnitData> robotUnitDataQueue;
- mutable std::mutex robotUnitDataMutex;
- // running tasks
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
new file mode 100644
index 000000000..8b49307bd
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
@@ -0,0 +1,202 @@
+/*
+ * 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::RobotSensorMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStateWriter.h"
+
+// STL
+#include <chrono>
+
+// Simox
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
+ {
+ {
+ // Thread-local copy of debug observer helper.
+ this->debugObserver = DebugObserverHelper(
+ Logging::tag.tagName, debugObserver.getDebugObserver(), true
+ );
+ }
+ }
+
+
+ static float toDurationMs(
+ std::chrono::high_resolution_clock::time_point start,
+ std::chrono::high_resolution_clock::time_point end)
+ {
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+ return duration.count() / 1000.f;
+ }
+
+
+ void RobotStateWriter::run(
+ float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
+ MemoryToIceAdapter& memory, localization::Segment& localizationSegment,
+ std::mutex& workingMemoryMutex)
+ {
+ CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
+ while (task and not task->isStopped())
+ {
+ size_t queueSize = 0;
+ std::queue<RobotUnitData> batch;
+ {
+ std::lock_guard lock{dataMutex};
+ queueSize = dataQueue.size();
+ if (dataQueue.size() >= properties.memoryBatchSize)
+ {
+ std::swap(batch, dataQueue);
+ }
+ }
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", queueSize);
+ }
+ if (batch.size() > 0)
+ {
+ Update update = buildUpdate(batch);
+
+ auto start = std::chrono::high_resolution_clock::now();
+ auto endLock = start, endProprioception = start, endLocalization = start;
+
+ armem::CommitResult result;
+ {
+ std::scoped_lock lock(workingMemoryMutex);
+ endLock = std::chrono::high_resolution_clock::now();
+
+ result = memory.commit(update.proprioception);
+ endProprioception = std::chrono::high_resolution_clock::now();
+
+ localizationSegment.storeTransform(update.localization);
+ endLocalization = std::chrono::high_resolution_clock::now();
+ }
+ if (not result.allSuccess())
+ {
+ ARMARX_WARNING << "Could not add data to memory. Error message: " << result.allErrorMessages();
+ }
+ if (debugObserver)
+ {
+ auto end = std::chrono::high_resolution_clock::now();
+
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit [ms]", toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Lock [ms]", toDurationMs(start, endLock));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Proprioception [ms]", toDurationMs(endLock, endProprioception));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit Localization [ms]", toDurationMs(endProprioception, endLocalization));
+ }
+ }
+
+ if (debugObserver)
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData> dataQueue)
+ {
+ ARMARX_CHECK_GREATER(dataQueue.size(), 0);
+
+ ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
+ auto start = std::chrono::high_resolution_clock::now();
+
+ // Send batch to memory
+ Update update;
+
+ while (dataQueue.size() > 0)
+ {
+ const RobotUnitData& convertedAndGroupedData = dataQueue.front();
+
+ for (const auto& [encName, encTimestep] : convertedAndGroupedData.groups)
+ {
+ ARMARX_CHECK_EQUAL(encName, encTimestep.name);
+
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(encName);
+ up.timeCreated = encTimestep.timestamp;
+ up.instancesData = { encTimestep.data };
+ }
+
+ // odometry pose -> localization segment
+ auto it = convertedAndGroupedData.groups.find(properties.platformGroupName);
+ if (it != convertedAndGroupedData.groups.end())
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+
+ const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
+ const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
+ const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
+ const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+
+ // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
+
+ armem::robot_state::Transform& transform = update.localization;
+ transform.header.parentFrame = armarx::OdometryFrame;
+ transform.header.frame = "root"; // TODO: robot root node
+ transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
+ transform.header.timestamp = it->second.timestamp;
+ transform.transform = odometryPose;
+ }
+ else if (!noOdometryDataLogged)
+ {
+ ARMARX_INFO << "No odometry data! (No group with name '" << properties.platformGroupName << "'.). \n"
+ << "If you are using a mobile platform this should not have happened.";
+ noOdometryDataLogged = true;
+ }
+
+ dataQueue.pop();
+ }
+
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << duration;
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Build Update [ms]", duration.count() / 1000.f);
+ }
+
+ return update;
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
new file mode 100644
index 000000000..8111357d2
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
@@ -0,0 +1,99 @@
+/*
+ * 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::RobotSensorMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+#include "RobotUnitData.h"
+
+
+namespace armarx::plugins
+{
+ class DebugObserverComponentPlugin;
+}
+namespace armarx::armem::server
+{
+ class MemoryToIceAdapter;
+}
+namespace armarx::armem::server::robot_state::localization
+{
+ class Segment;
+}
+namespace armarx::armem::server::robot_state
+{
+
+ class RobotStateWriter : public armarx::Logging
+ {
+ public:
+
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+
+ /// Reads data from `dataQueue` and commits to the memory.
+ void run(float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment,
+ std::mutex& workingMemoryMutex
+ );
+
+
+ struct Update
+ {
+ armem::Commit proprioception;
+ armem::robot_state::Transform localization;
+ };
+ Update buildUpdate(std::queue<RobotUnitData> dataQueue);
+
+
+ public:
+
+ struct Properties
+ {
+ unsigned int memoryBatchSize = 50;
+ armem::MemoryID robotUnitProviderID;
+ std::string platformGroupName = "sens.Platform";
+ };
+ Properties properties;
+
+ std::optional<DebugObserverHelper> debugObserver;
+
+
+ armarx::SimpleRunningTask<>::pointer_type task = nullptr;
+
+
+ private:
+
+ bool noOdometryDataLogged = false;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp
new file mode 100644
index 000000000..9a9b72e2a
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.cpp
@@ -0,0 +1,9 @@
+#include "RobotUnitData.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
new file mode 100644
index 000000000..dc43dcddc
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitData.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+
+namespace armarx::aron::datanavigator
+{
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+}
+
+namespace armarx::armem::server::robot_state
+{
+
+ struct RobotUnitData
+ {
+ struct RobotUnitDataGroup
+ {
+ armem::Time timestamp;
+ std::string name;
+ aron::datanavigator::DictNavigatorPtr data;
+ };
+
+ std::map<std::string, RobotUnitDataGroup> groups;
+ };
+
+}
+
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
new file mode 100644
index 000000000..58b688557
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
@@ -0,0 +1,201 @@
+#include "RobotUnitReader.h"
+
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/primitive/String.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <istream>
+#include <filesystem>
+#include <fstream>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::map<std::string, std::string> RobotUnitReader::readConfig(const std::string& configPath)
+ {
+ ARMARX_CHECK(std::filesystem::is_regular_file(configPath));
+
+ std::map<std::string, std::string> config;
+
+ std::ifstream in(configPath);
+ std::string line;
+ while (std::getline(in, line))
+ {
+ if (line.size() > 0)
+ {
+ if (simox::alg::starts_with(line, "//"))
+ {
+ continue;
+ }
+
+ std::vector<std::string> mappings = simox::alg::split(line, ",");
+ for (const auto& mapping : mappings)
+ {
+ std::vector<std::string> split = simox::alg::split(mapping, "=>");
+ if (split.size() < 2)
+ {
+ ARMARX_WARNING_S << "A mapping in the RobotUnitConfig file is not valid. Ignoring it: " << mapping;
+ continue;
+ }
+ for (unsigned int i = 0; i < split.size() - 1; ++i)
+ {
+ ARMARX_DEBUG_S << "Setting sensor value '" << split[i] << "' to group '" << split[split.size() - 1] << "'.";
+ config.emplace(split[i], split[split.size() - 1]);
+ }
+ }
+ }
+ }
+
+ return config;
+ }
+
+
+ void RobotUnitReader::connect(
+ armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
+ {
+ {
+ config.loggingNames.push_back(properties.sensorPrefix);
+ receiver = robotUnitPlugin.startDataSatreming(config);
+ description = receiver->getDataDescription();
+ }
+ {
+ // Thread-local copy of debug observer helper.
+ debugObserver = DebugObserverHelper(
+ Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true
+ );
+ }
+ }
+
+
+ void RobotUnitReader::run(
+ float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex)
+ {
+ CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
+ while (task and not task->isStopped())
+ {
+ if (std::optional<RobotUnitData> data = fetchAndGroupLatestRobotUnitData())
+ {
+ std::lock_guard g{dataMutex};
+ dataQueue.push(data.value());
+ }
+
+ if (debugObserver)
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ std::optional<RobotUnitData> RobotUnitReader::fetchAndGroupLatestRobotUnitData()
+ {
+ const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
+ if (not data.has_value())
+ {
+ return std::nullopt;
+ }
+
+ ARMARX_DEBUG << "RobotUnitData: Generating new aron map for current timestep";
+ auto start = std::chrono::high_resolution_clock::now();
+
+ RobotUnitData convertedAndGroupedData;
+ for (const auto& [nameEntry, dataEntry] : description.entries)
+ {
+ std::string name = nameEntry;
+ std::string jointOrWhateverName;
+
+ std::string groupName = "";
+ if (auto it = configSensorMapping.find(name); it != configSensorMapping.end())
+ {
+ groupName = it->second;
+ }
+ else
+ {
+ const size_t first_dot_pos = name.find(".");
+ const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
+ if (second_dot_pos == std::string::npos)
+ {
+ ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". "
+ << "All sensors must be called sens.X.Y where X is the name of the group "
+ << "and Y is the actual sensor. Ignoring this sensor.";
+ continue;
+ }
+ groupName = name.substr(0, second_dot_pos);
+
+ jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
+
+ name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
+ }
+
+ auto it = convertedAndGroupedData.groups.find(groupName);
+ if (it == convertedAndGroupedData.groups.end())
+ {
+ namespace adn = aron::datanavigator;
+
+ // Generate new dict for the group
+ RobotUnitData::RobotUnitDataGroup group;
+ group.timestamp = armem::Time::microSeconds(data->timestampUSec);
+ group.name = groupName;
+ group.data = std::make_shared<adn::DictNavigator>();
+
+ group.data->addElement("EncoderGroupName", std::make_shared<adn::StringNavigator>(groupName));
+ group.data->addElement("IterationId", std::make_shared<adn::LongNavigator>(data->iterationId));
+ group.data->addElement("name", std::make_shared<adn::StringNavigator>(jointOrWhateverName));
+
+ auto r = convertedAndGroupedData.groups.emplace(groupName, group);
+ it = r.first;
+ }
+
+ RobotUnitData::RobotUnitDataGroup& group = it->second;
+ group.data->addElement(name, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
+ }
+
+ auto stop = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+ ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << duration;
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
+ }
+
+ return convertedAndGroupedData;
+ }
+
+
+ std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
+ {
+ std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+ }
+ if (data.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ RobotUnitDataStreaming::TimeStep currentTimestep = data.back();
+ data.clear();
+ return currentTimestep;
+ }
+ }
+
+
+}
+
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
new file mode 100644
index 000000000..a44c9c0f1
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.h
@@ -0,0 +1,84 @@
+#pragma once
+
+#include <queue>
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+
+#include "RobotUnitData.h"
+
+
+namespace armarx::plugins
+{
+ class RobotUnitComponentPlugin;
+ class DebugObserverComponentPlugin;
+}
+namespace armarx
+{
+ using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
+}
+
+namespace armarx::armem::server::robot_state
+{
+
+ class RobotUnitReader : public armarx::Logging
+ {
+ public:
+
+ static std::map<std::string, std::string> readConfig(const std::string& configPath);
+
+
+ public:
+
+ void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
+
+
+
+ /// Reads data from `handler` and fills `robotUnitDataQueue`.
+ void run(float pollFrequency,
+ std::queue<RobotUnitData>& dataQueue,
+ std::mutex& dataMutex);
+
+ std::optional<RobotUnitData> fetchAndGroupLatestRobotUnitData();
+
+
+ private:
+
+ /// Fetch the latest timestep and clear the robot unit buffer.
+ std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
+
+
+ public:
+
+ struct Properties
+ {
+ std::string sensorPrefix = "sens.*";
+ };
+ Properties properties;
+
+ std::optional<DebugObserverHelper> debugObserver;
+
+
+ RobotUnitDataStreaming::Config config;
+ RobotUnitDataStreamingReceiverPtr receiver;
+ RobotUnitDataStreaming::DataStreamingDescription description;
+
+ /// RobotUnit name -> group name.
+ std::map<std::string, std::string> configSensorMapping;
+
+ armarx::SimpleRunningTask<>::pointer_type task = nullptr;
+
+ };
+
+}
+
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp
new file mode 100644
index 000000000..8caf73057
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+
+namespace armarx
+{
+ aron::datanavigator::NavigatorPtr RobotUnitDataStreaming::toAron(
+ const TimeStep& timestep,
+ const DataEntry& dataEntry)
+ {
+ switch (dataEntry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ {
+ float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::FloatNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeBool:
+ {
+ bool value = RobotUnitDataStreamingReceiver::GetAs<bool>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::BoolNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeByte:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Byte>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeShort:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Short>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeInt:
+ {
+ int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Int>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeLong:
+ {
+ long value = RobotUnitDataStreamingReceiver::GetAs<Ice::Long>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::LongNavigator>(value);
+ }
+ case RobotUnitDataStreaming::NodeTypeDouble:
+ {
+ double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(timestep, dataEntry);
+ return std::make_shared<aron::datanavigator::DoubleNavigator>(value);
+ }
+ default:
+ ARMARX_UNEXPECTED_ENUM_VALUE(RobotUnitDataStreaming::NodeType, dataEntry.type);
+ }
+ }
+
+
+ const simox::meta::EnumNames<RobotUnitDataStreaming::DataEntryType> RobotUnitDataStreaming::DataEntryNames =
+ {
+ { NodeTypeBool, "Bool" },
+ { NodeTypeByte, "Byte" },
+ { NodeTypeShort, "Short" },
+ { NodeTypeInt, "Int" },
+ { NodeTypeLong, "Long" },
+ { NodeTypeFloat, "Float" },
+ { NodeTypeDouble, "Double" },
+ };
+
+}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h b/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h
new file mode 100644
index 000000000..dd2177c9e
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/aron_conversions.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <memory>
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+
+namespace armarx::aron::datanavigator
+{
+ using NavigatorPtr = std::shared_ptr<class Navigator>;
+}
+
+namespace armarx::RobotUnitDataStreaming
+{
+ aron::datanavigator::NavigatorPtr toAron(
+ const TimeStep& timestep,
+ const DataEntry& dataEntry);
+
+ extern const simox::meta::EnumNames<DataEntryType> DataEntryNames;
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 13b5572d3..b3f71e59a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -82,7 +82,7 @@ namespace armarx::armem::robot_state
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string descriptionCoreSegment = "Description";
std::string localizationCoreSegment = "Localization";
std::string proprioceptionCoreSegment = "Proprioception";
--
GitLab