Skip to content
Snippets Groups Projects

Draft: Make RobotStateMemory ready

Merged Draft: Make RobotStateMemory ready
armem/robot-state-memory into master
Merged Rainer Kartmann requested to merge armem/robot-state-memory into master
Viewing commit 276312cf
Prev Next
Show latest version
3 files
+ 6
− 9
Compare changes
  • Side-by-side
  • Inline
Files
3
source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+ 88
− 367
@@ -23,46 +23,40 @@
#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),
proprioceptionSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
localizationSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
: descriptionSegment(server::ComponentPluginUser::iceMemory),
proprioceptionSegment(server::ComponentPluginUser::iceMemory),
localizationSegment(server::ComponentPluginUser::iceMemory),
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,17 +73,22 @@ 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.");
descriptionSegment.defineProperties(defs);
proprioceptionSegment.defineProperties(defs);
localizationSegment.defineProperties(defs);
commonVisu.defineProperties(defs);
setRobotUnitAsOptionalDependency();
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, prefix + "desc.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
localizationSegment.defineProperties(defs, prefix + "loc.");
commonVisu.defineProperties(defs, prefix + "visu.");
return defs;
}
@@ -103,17 +102,16 @@ 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();
std::vector<std::string> includePaths;
std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
for (const std::string& projectName : packages)
@@ -124,44 +122,50 @@ namespace armarx::armem::server::robot_state
}
CMakePackageFinder project(projectName);
auto pathsString = project.getIncludePaths();
Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
std::string pathsString = project.getIncludePaths();
std::vector<std::string> projectIncludePaths = simox::alg::split(pathsString, ";,");
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(getRobotUnit()->getKinematicUnit());
descriptionSegment.connect(getArvizClient(), getRobotUnit());
descriptionSegment.connect(robotUnitPrx);
proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
proprioceptionSegment.connect(robotUnitPrx);
localizationSegment.connect(getArvizClient());
localizationSegment.connect();
commonVisu.connect(
getArvizClient()
);
commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
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
);
}, "Robot State Writer");
startRobotUnitStream();
}
@@ -178,340 +182,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();
}
}