From 369ae22a36cb68c85e7657da3c86610484aad7bd Mon Sep 17 00:00:00 2001
From: "fabian.peller-konrad@kit.edu" <fabian.peller-konrad@kit.edu>
Date: Tue, 27 Apr 2021 12:28:47 +0200
Subject: [PATCH] updated sensor mapping, removed rsc from state memory
---
data/RobotAPI/robotStateMemoryConfig.txt | 279 +++++++++---------
.../RobotStateMemory/RobotStateMemory.cpp | 200 +++----------
.../RobotStateMemory/RobotStateMemory.h | 43 +--
.../armem_robot_mapping/MappingDataReader.cpp | 27 +-
4 files changed, 186 insertions(+), 363 deletions(-)
diff --git a/data/RobotAPI/robotStateMemoryConfig.txt b/data/RobotAPI/robotStateMemoryConfig.txt
index c764eae7e..bc928c169 100644
--- a/data/RobotAPI/robotStateMemoryConfig.txt
+++ b/data/RobotAPI/robotStateMemoryConfig.txt
@@ -1,3 +1,5 @@
+// Configuration set for the Humanoid Robot ARMAR-6
+// Left Hand unit
sens.Index L 1 Joint.acceleration => HandL
sens.Index L 1 Joint.gravityTorque => HandL
sens.Index L 1 Joint.inertiaTorque => HandL
@@ -19,27 +21,7 @@ sens.Index L 3 Joint.inverseDynamicsTorque => HandL
sens.Index L 3 Joint.position => HandL
sens.Index L 3 Joint.torque => HandL
sens.Index L 3 Joint.velocity => HandL
-sens.Index R 1 Joint.acceleration => HandR
-sens.Index R 1 Joint.gravityTorque => HandR
-sens.Index R 1 Joint.inertiaTorque => HandR
-sens.Index R 1 Joint.inverseDynamicsTorque => HandR
-sens.Index R 1 Joint.position => HandR
-sens.Index R 1 Joint.torque => HandR
-sens.Index R 1 Joint.velocity => HandR
-sens.Index R 2 Joint.acceleration => HandR
-sens.Index R 2 Joint.gravityTorque => HandR
-sens.Index R 2 Joint.inertiaTorque => HandR
-sens.Index R 2 Joint.inverseDynamicsTorque => HandR
-sens.Index R 2 Joint.position => HandR
-sens.Index R 2 Joint.torque => HandR
-sens.Index R 2 Joint.velocity => HandR
-sens.Index R 3 Joint.acceleration => HandR
-sens.Index R 3 Joint.gravityTorque => HandR
-sens.Index R 3 Joint.inertiaTorque => HandR
-sens.Index R 3 Joint.inverseDynamicsTorque => HandR
-sens.Index R 3 Joint.position => HandR
-sens.Index R 3 Joint.torque => HandR
-sens.Index R 3 Joint.velocity => HandR
+
sens.Middle L 1 Joint.acceleration => HandL
sens.Middle L 1 Joint.gravityTorque => HandL
sens.Middle L 1 Joint.inertiaTorque => HandL
@@ -61,27 +43,7 @@ sens.Middle L 3 Joint.inverseDynamicsTorque => HandL
sens.Middle L 3 Joint.position => HandL
sens.Middle L 3 Joint.torque => HandL
sens.Middle L 3 Joint.velocity => HandL
-sens.Middle R 1 Joint.acceleration => HandR
-sens.Middle R 1 Joint.gravityTorque => HandR
-sens.Middle R 1 Joint.inertiaTorque => HandR
-sens.Middle R 1 Joint.inverseDynamicsTorque => HandR
-sens.Middle R 1 Joint.position => HandR
-sens.Middle R 1 Joint.torque => HandR
-sens.Middle R 1 Joint.velocity => HandR
-sens.Middle R 2 Joint.acceleration => HandR
-sens.Middle R 2 Joint.gravityTorque => HandR
-sens.Middle R 2 Joint.inertiaTorque => HandR
-sens.Middle R 2 Joint.inverseDynamicsTorque => HandR
-sens.Middle R 2 Joint.position => HandR
-sens.Middle R 2 Joint.torque => HandR
-sens.Middle R 2 Joint.velocity => HandR
-sens.Middle R 3 Joint.acceleration => HandR
-sens.Middle R 3 Joint.gravityTorque => HandR
-sens.Middle R 3 Joint.inertiaTorque => HandR
-sens.Middle R 3 Joint.inverseDynamicsTorque => HandR
-sens.Middle R 3 Joint.position => HandR
-sens.Middle R 3 Joint.torque => HandR
-sens.Middle R 3 Joint.velocity => HandR
+
sens.Pinky L 1 Joint.acceleration => HandL
sens.Pinky L 1 Joint.gravityTorque => HandL
sens.Pinky L 1 Joint.inertiaTorque => HandL
@@ -103,101 +65,138 @@ sens.Pinky L 3 Joint.inverseDynamicsTorque => HandL
sens.Pinky L 3 Joint.position => HandL
sens.Pinky L 3 Joint.torque => HandL
sens.Pinky L 3 Joint.velocity => HandL
-sens.Pinky R 1 Joint.acceleration => HandR
-sens.Pinky R 1 Joint.gravityTorque => HandR
-sens.Pinky R 1 Joint.inertiaTorque => HandR
-sens.Pinky R 1 Joint.inverseDynamicsTorque => HandR
-sens.Pinky R 1 Joint.position => HandR
-sens.Pinky R 1 Joint.torque => HandR
-sens.Pinky R 1 Joint.velocity => HandR
-sens.Pinky R 2 Joint.acceleration => HandR
-sens.Pinky R 2 Joint.gravityTorque => HandR
-sens.Pinky R 2 Joint.inertiaTorque => HandR
-sens.Pinky R 2 Joint.inverseDynamicsTorque
-sens.Pinky R 2 Joint.position => HandR
-sens.Pinky R 2 Joint.torque => HandR
-sens.Pinky R 2 Joint.velocity => HandR
-sens.Pinky R 3 Joint.acceleration => HandR
-sens.Pinky R 3 Joint.gravityTorque => HandR
-sens.Pinky R 3 Joint.inertiaTorque => HandR
-sens.Pinky R 3 Joint.inverseDynamicsTorque => HandR
-sens.Pinky R 3 Joint.position => HandR
-sens.Pinky R 3 Joint.torque => HandR
-sens.Pinky R 3 Joint.velocity => HandR
-sens.Ring L 1 Joint.acceleration
-sens.Ring L 1 Joint.gravityTorque
-sens.Ring L 1 Joint.inertiaTorque
-sens.Ring L 1 Joint.inverseDynamicsTorque
-sens.Ring L 1 Joint.position
-sens.Ring L 1 Joint.torque
-sens.Ring L 1 Joint.velocity
-sens.Ring L 2 Joint.acceleration
-sens.Ring L 2 Joint.gravityTorque
-sens.Ring L 2 Joint.inertiaTorque
-sens.Ring L 2 Joint.inverseDynamicsTorque
-sens.Ring L 2 Joint.position
-sens.Ring L 2 Joint.torque
-sens.Ring L 2 Joint.velocity
-sens.Ring L 3 Joint.acceleration
-sens.Ring L 3 Joint.gravityTorque
-sens.Ring L 3 Joint.inertiaTorque
-sens.Ring L 3 Joint.inverseDynamicsTorque
-sens.Ring L 3 Joint.position
-sens.Ring L 3 Joint.torque
-sens.Ring L 3 Joint.velocity
-sens.Ring R 1 Joint.acceleration
-sens.Ring R 1 Joint.gravityTorque
-sens.Ring R 1 Joint.inertiaTorque
-sens.Ring R 1 Joint.inverseDynamicsTorque
-sens.Ring R 1 Joint.position
-sens.Ring R 1 Joint.torque
-sens.Ring R 1 Joint.velocity
-sens.Ring R 2 Joint.acceleration
-sens.Ring R 2 Joint.gravityTorque
-sens.Ring R 2 Joint.inertiaTorque
-sens.Ring R 2 Joint.inverseDynamicsTorque
-sens.Ring R 2 Joint.position
-sens.Ring R 2 Joint.torque
-sens.Ring R 2 Joint.velocity
-sens.Ring R 3 Joint.acceleration
-sens.Ring R 3 Joint.gravityTorque
-sens.Ring R 3 Joint.inertiaTorque
-sens.Ring R 3 Joint.inverseDynamicsTorque
-sens.Ring R 3 Joint.position
-sens.Ring R 3 Joint.torque
-sens.Ring R 3 Joint.velocity
-sens.Thumb L 1 Joint.acceleration
-sens.Thumb L 1 Joint.gravityTorque
-sens.Thumb L 1 Joint.inertiaTorque
-sens.Thumb L 1 Joint.inverseDynamicsTorque
-sens.Thumb L 1 Joint.position
-sens.Thumb L 1 Joint.torque
-sens.Thumb L 1 Joint.velocity
-sens.Thumb L 2 Joint.acceleration
-sens.Thumb L 2 Joint.gravityTorque
-sens.Thumb L 2 Joint.inertiaTorque
-sens.Thumb L 2 Joint.inverseDynamicsTorque
-sens.Thumb L 2 Joint.position
-sens.Thumb L 2 Joint.torque
-sens.Thumb L 2 Joint.velocity
-sens.Thumb R 1 Joint.acceleration
-sens.Thumb R 1 Joint.gravityTorque
-sens.Thumb R 1 Joint.inertiaTorque
-sens.Thumb R 1 Joint.inverseDynamicsTorque
-sens.Thumb R 1 Joint.position
-sens.Thumb R 1 Joint.torque
-sens.Thumb R 1 Joint.velocity
-sens.Thumb R 2 Joint.acceleration
-sens.Thumb R 2 Joint.gravityTorque
-sens.Thumb R 2 Joint.inertiaTorque
-sens.Thumb R 2 Joint.inverseDynamicsTorque
-sens.Thumb R 2 Joint.position
-sens.Thumb R 2 Joint.torque
-sens.Thumb R 2 Joint.velocity
-sens.TorsoJoint.acceleration
-sens.TorsoJoint.gravityTorque
-sens.TorsoJoint.inertiaTorque
-sens.TorsoJoint.inverseDynamicsTorque
-sens.TorsoJoint.position
-sens.TorsoJoint.torque
-sens.TorsoJoint.velocity
+
+sens.Ring L 1 Joint.acceleration => HandL
+sens.Ring L 1 Joint.gravityTorque => HandL
+sens.Ring L 1 Joint.inertiaTorque => HandL
+sens.Ring L 1 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 1 Joint.position => HandL
+sens.Ring L 1 Joint.torque => HandL
+sens.Ring L 1 Joint.velocity => HandL
+sens.Ring L 2 Joint.acceleration => HandL
+sens.Ring L 2 Joint.gravityTorque => HandL
+sens.Ring L 2 Joint.inertiaTorque => HandL
+sens.Ring L 2 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 2 Joint.position => HandL
+sens.Ring L 2 Joint.torque => HandL
+sens.Ring L 2 Joint.velocity => HandL
+sens.Ring L 3 Joint.acceleration => HandL
+sens.Ring L 3 Joint.gravityTorque => HandL
+sens.Ring L 3 Joint.inertiaTorque => HandL
+sens.Ring L 3 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 3 Joint.position => HandL
+sens.Ring L 3 Joint.torque => HandL
+sens.Ring L 3 Joint.velocity => HandL
+
+sens.Thumb L 1 Joint.acceleration => HandL
+sens.Thumb L 1 Joint.gravityTorque => HandL
+sens.Thumb L 1 Joint.inertiaTorque => HandL
+sens.Thumb L 1 Joint.inverseDynamicsTorque => HandL
+sens.Thumb L 1 Joint.position => HandL
+sens.Thumb L 1 Joint.torque => HandL
+sens.Thumb L 1 Joint.velocity => HandL
+sens.Thumb L 2 Joint.acceleration => HandL
+sens.Thumb L 2 Joint.gravityTorque => HandL
+sens.Thumb L 2 Joint.inertiaTorque => HandL
+sens.Thumb L 2 Joint.inverseDynamicsTorque => HandL
+sens.Thumb L 2 Joint.position => HandL
+sens.Thumb L 2 Joint.torque => HandL
+sens.Thumb L 2 Joint.velocity => HandL
+
+// Right Hand unit
+sens.Index R 1 Joint.acceleration => HandR
+sens.Index R 1 Joint.gravityTorque => HandR
+sens.Index R 1 Joint.inertiaTorque => HandR
+sens.Index R 1 Joint.inverseDynamicsTorque => HandR
+sens.Index R 1 Joint.position => HandR
+sens.Index R 1 Joint.torque => HandR
+sens.Index R 1 Joint.velocity => HandR
+sens.Index R 2 Joint.acceleration => HandR
+sens.Index R 2 Joint.gravityTorque => HandR
+sens.Index R 2 Joint.inertiaTorque => HandR
+sens.Index R 2 Joint.inverseDynamicsTorque => HandR
+sens.Index R 2 Joint.position => HandR
+sens.Index R 2 Joint.torque => HandR
+sens.Index R 2 Joint.velocity => HandR
+sens.Index R 3 Joint.acceleration => HandR
+sens.Index R 3 Joint.gravityTorque => HandR
+sens.Index R 3 Joint.inertiaTorque => HandR
+sens.Index R 3 Joint.inverseDynamicsTorque => HandR
+sens.Index R 3 Joint.position => HandR
+sens.Index R 3 Joint.torque => HandR
+sens.Index R 3 Joint.velocity => HandR
+
+sens.Middle R 1 Joint.acceleration => HandR
+sens.Middle R 1 Joint.gravityTorque => HandR
+sens.Middle R 1 Joint.inertiaTorque => HandR
+sens.Middle R 1 Joint.inverseDynamicsTorque => HandR
+sens.Middle R 1 Joint.position => HandR
+sens.Middle R 1 Joint.torque => HandR
+sens.Middle R 1 Joint.velocity => HandR
+sens.Middle R 2 Joint.acceleration => HandR
+sens.Middle R 2 Joint.gravityTorque => HandR
+sens.Middle R 2 Joint.inertiaTorque => HandR
+sens.Middle R 2 Joint.inverseDynamicsTorque => HandR
+sens.Middle R 2 Joint.position => HandR
+sens.Middle R 2 Joint.torque => HandR
+sens.Middle R 2 Joint.velocity => HandR
+sens.Middle R 3 Joint.acceleration => HandR
+sens.Middle R 3 Joint.gravityTorque => HandR
+sens.Middle R 3 Joint.inertiaTorque => HandR
+sens.Middle R 3 Joint.inverseDynamicsTorque => HandR
+sens.Middle R 3 Joint.position => HandR
+sens.Middle R 3 Joint.torque => HandR
+sens.Middle R 3 Joint.velocity => HandR
+
+sens.Ring R 1 Joint.acceleration => HandR
+sens.Ring R 1 Joint.gravityTorque => HandR
+sens.Ring R 1 Joint.inertiaTorque => HandR
+sens.Ring R 1 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 1 Joint.position => HandR
+sens.Ring R 1 Joint.torque => HandR
+sens.Ring R 1 Joint.velocity => HandR
+sens.Ring R 2 Joint.acceleration => HandR
+sens.Ring R 2 Joint.gravityTorque => HandR
+sens.Ring R 2 Joint.inertiaTorque => HandR
+sens.Ring R 2 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 2 Joint.position => HandR
+sens.Ring R 2 Joint.torque => HandR
+sens.Ring R 2 Joint.velocity => HandR
+sens.Ring R 3 Joint.acceleration => HandR
+sens.Ring R 3 Joint.gravityTorque => HandR
+sens.Ring R 3 Joint.inertiaTorque => HandR
+sens.Ring R 3 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 3 Joint.position => HandR
+sens.Ring R 3 Joint.torque => HandR
+sens.Ring R 3 Joint.velocity => HandR
+
+sens.Thumb R 1 Joint.acceleration => HandR
+sens.Thumb R 1 Joint.gravityTorque => HandR
+sens.Thumb R 1 Joint.inertiaTorque => HandR
+sens.Thumb R 1 Joint.inverseDynamicsTorque => HandR
+sens.Thumb R 1 Joint.position => HandR
+sens.Thumb R 1 Joint.torque => HandR
+sens.Thumb R 1 Joint.velocity => HandR
+sens.Thumb R 2 Joint.acceleration => HandR
+sens.Thumb R 2 Joint.gravityTorque => HandR
+sens.Thumb R 2 Joint.inertiaTorque => HandR
+sens.Thumb R 2 Joint.inverseDynamicsTorque => HandR
+sens.Thumb R 2 Joint.position => HandR
+sens.Thumb R 2 Joint.torque => HandR
+sens.Thumb R 2 Joint.velocity => HandR
+
+// Neck
+sens.Neck_1_Yaw.acceleration => Neck
+sens.Neck_1_Yaw.gravityTorque => Neck
+sens.Neck_1_Yaw.inertiaTorque => Neck
+sens.Neck_1_Yaw.inverseDynamicsTorque => Neck
+sens.Neck_1_Yaw.position => Neck
+sens.Neck_1_Yaw.torque => Neck
+sens.Neck_1_Yaw.velocity => Neck
+sens.Neck_2_Pitch.acceleration => Neck
+sens.Neck_2_Pitch.gravityTorque => Neck
+sens.Neck_2_Pitch.inertiaTorque => Neck
+sens.Neck_2_Pitch.inverseDynamicsTorque
+sens.Neck_2_Pitch.position => Neck
+sens.Neck_2_Pitch.torque => Neck
+sens.Neck_2_Pitch.velocity => Neck
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 34e37903f..2ab7912ee 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -33,6 +33,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
@@ -53,17 +54,13 @@ namespace armarx
defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "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, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
- defs->optional(robotStateComponentMemoryBatchSize, "RobotStateComponentMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotStateComponentPollFrequency, "RobotStateComponentUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY));
-
-
return defs;
}
std::string RobotStateMemory::getDefaultName() const
{
- return "RobotSensorMemory";
+ return "RobotStateMemory";
}
@@ -72,8 +69,25 @@ namespace armarx
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
- robotStateComponentMemoryBatchSize = std::max((unsigned int) 1, robotStateComponentMemoryBatchSize);
- robotStateComponentPollFrequency = std::clamp(robotStateComponentPollFrequency, 1, ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY);
+
+ Ice::StringSeq includePaths;
+ auto packages = armarx::Application::GetProjectDependencies();
+ packages.push_back(Application::GetProjectName());
+
+ for (const std::string& projectName : packages)
+ {
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ auto pathsString = project.getIncludePaths();
+ Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
+
+ ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
workingmemory.name() = workingMemoryName;
}
@@ -82,7 +96,6 @@ namespace armarx
void RobotStateMemory::onConnectComponent()
{
setupRobotUnitSegment();
- setupRobotStateComponentSegment();
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -91,25 +104,19 @@ namespace armarx
robotUnitConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
robotUnitStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
- robotStateComponentConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotStateComponentDataToAron, (1000 / robotStateComponentPollFrequency));
- robotStateComponentStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotStateComponentDataInMemory, (1000 / robotStateComponentPollFrequency));
-
startRobotUnitStream();
- startRobotStateComponentPoll();
}
void RobotStateMemory::onDisconnectComponent()
{
stopRobotUnitStream();
- startRobotStateComponentPoll();
}
void RobotStateMemory::onExitComponent()
{
stopRobotUnitStream();
- stopRobotStateComponentPoll();
}
/*************************************************************/
@@ -383,184 +390,45 @@ namespace armarx
// parse config
std::filesystem::path configPath(robotUnitConfigPath);
configSensorMapping.clear();
- if(std::filesystem::is_regular_file(configPath))
+ if (std::filesystem::is_regular_file(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 (line.size() > 0)
{
- if(simox::alg::starts_with(line, "//"))
+ if (simox::alg::starts_with(line, "//"))
{
continue;
}
std::vector<std::string> mappings = simox::alg::split(line, ",");
- for(const auto& mapping : mappings)
+ for (const auto& mapping : mappings)
{
std::vector<std::string> split = simox::alg::split(mapping, "=>");
- if(split.size() < 2)
+ if (split.size() < 2)
{
- ARMARX_WARNING << "A mapping in the RobotUnitConfig file is not valid. Ignoring it.";
+ 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)
{
- configSensorMapping.insert({split[i], split[split.size()-1]});
+ ARMARX_DEBUG << "Setting sensor value '" << split[i] << "' to group '" << split[split.size() - 1] << "'.";
+ configSensorMapping.insert({split[i], split[split.size() - 1]});
}
}
}
}
}
-
- robotUnitConversionTask->start();
- robotUnitStoringTask->start();
- }
-
-
- /*************************************************************/
- // RobotStateComponent Polling functions
- /*************************************************************/
- void RobotStateMemory::setupRobotStateComponentSegment()
- {
- ARMARX_INFO << "Adding core segment " << robotStateComponentCoreSegmentName;
- workingmemory.addCoreSegments({robotStateComponentCoreSegmentName});
-
- ARMARX_INFO << "Adding provider segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = robotStateComponentCoreSegmentName;
- input.providerSegmentName = robotStateComponentProviderSegmentName;
-
- // Todo add type
-
- auto result = addSegments({input})[0];
-
- if (!result.success)
+ else
{
- ARMARX_ERROR << "Could not add segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName << ". The error message is: " << result.errorMessage;
+ ARMARX_INFO << "The config path '" << robotUnitConfigPath << "' is not valid.";
}
- robotStateComponentProviderID.memoryName = workingMemoryName;
- robotStateComponentProviderID.coreSegmentName = robotStateComponentCoreSegmentName;
- robotStateComponentProviderID.providerSegmentName = robotStateComponentProviderSegmentName;
- }
-
- void RobotStateMemory::convertRobotStateComponentDataToAron()
- {
- ARMARX_DEBUG << "RobotStateComponentData: Generating new aron map for current timestep";
- auto start = std::chrono::high_resolution_clock::now();
-
- auto synchronizedRobot = getRobotStateComponent()->getSynchronizedRobot();
- auto framedPose = synchronizedRobot->getRootNode()->getGlobalPose();
-
- RobotStateComponentData convertedData;
- convertedData.timestamp = synchronizedRobot->getTimestamp()->timestamp;
-
- auto dict = std::make_shared<aron::datanavigator::DictNavigator>();
-
- auto x = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->position->x);
- dict->addElement("x", x);
-
- auto y = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->position->y);
- dict->addElement("y", y);
-
- auto z = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->position->z);
- dict->addElement("z", z);
-
- auto qw = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->orientation->qw);
- dict->addElement("qw", qw);
-
- auto qx = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->orientation->qx);
- dict->addElement("qx", qx);
-
- auto qy = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->orientation->qy);
- dict->addElement("qy", qy);
-
- auto qz = std::make_shared<aron::datanavigator::IntNavigator>(framedPose->orientation->qz);
- dict->addElement("z", qz);
-
- convertedData.data = dict;
-
- {
- std::lock_guard g{robotStateComponentDataMutex};
- robotStateComponentDataQueue.push(convertedData);
- }
-
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotStateComponentData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- }
-
- void RobotStateMemory::storeConvertedRobotStateComponentDataInMemory()
- {
- unsigned int size = 0;
- {
- std::lock_guard g{robotStateComponentDataMutex};
- size = robotStateComponentDataQueue.size(); // the size can only grow
- }
- if (size >= robotStateComponentMemoryBatchSize)
- {
- ARMARX_DEBUG << "RobotStateComponentData: Sending batch of " << robotStateComponentMemoryBatchSize << " 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 c;
- for (unsigned int i = 0; i < robotStateComponentMemoryBatchSize; ++i)
- {
- std::lock_guard g{robotStateComponentDataMutex};
- const auto& convertedData = robotStateComponentDataQueue.front();
-
- auto entityID = robotStateComponentProviderID;
- entityID.entityName = "GlobalPositionRootFrame";
-
- const auto& timeUSec = convertedData.timestamp;
- const auto& aron = convertedData.data;
-
- armem::data::EntityUpdate& update = c.updates.emplace_back();
- update.entityID = entityID;
- update.timeCreatedMicroSeconds = timeUSec;
- update.instancesData = {aron->toAronDictPtr()};
- robotStateComponentDataQueue.pop();
- }
-
- auto results = commit(c);
- auto stop = std::chrono::high_resolution_clock::now();
- ARMARX_DEBUG << "RobotStateComponentData: 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::stopRobotStateComponentPoll()
- {
- std::lock_guard g{startStopMutex};
- robotStateComponentConversionTask->stop();
- robotStateComponentStoringTask->stop();
- }
-
- void RobotStateMemory::startRobotStateComponentPoll()
- {
- std::lock_guard g{startStopMutex};
- if (robotStateComponentConversionTask->isRunning() || robotStateComponentStoringTask->isRunning())
- {
- if (robotStateComponentConversionTask->isRunning() && robotStateComponentStoringTask->isRunning())
- {
- return;
- }
- ARMARX_WARNING << "Found inconsistency in running tasks. Restarting all!";
- stopRobotStateComponentPoll();
- }
-
- ARMARX_INFO << "Start polling of robotStateComponent";
-
- robotStateComponentConversionTask->start();
- robotStateComponentStoringTask->start();
+ robotUnitConversionTask->start();
+ robotUnitStoringTask->start();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 727349b21..9787ac6f5 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -33,8 +33,6 @@
// BaseClass
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
@@ -58,8 +56,7 @@ namespace armarx
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser,
- virtual public RobotStateComponentPluginUser
+ virtual public RobotUnitComponentPluginUser
{
public:
@@ -94,15 +91,6 @@ namespace armarx
void startRobotUnitStream();
void stopRobotUnitStream();
- // RobotStateComponent Poll
- void setupRobotStateComponentSegment();
-
- void convertRobotStateComponentDataToAron();
- void storeConvertedRobotStateComponentDataInMemory();
-
- void startRobotStateComponentPoll();
- void stopRobotStateComponentPoll();
-
private:
std::string workingMemoryName = "RobotStateMemory";
bool addCoreSegmentOnUsage = false;
@@ -114,16 +102,12 @@ namespace armarx
std::string robotUnitProviderSegmentName = "RobotUnit"; // get robot name?
armem::data::MemoryID robotUnitProviderID;
- // TODO: Remove the whole connection to the RSC since this memory will replace it?
- std::string robotStateComponentCoreSegmentName = "Localization";
- std::string robotStateComponentProviderSegmentName = "RobotStateComponent"; // get robot name?
- armem::data::MemoryID robotStateComponentProviderID;
-
// RobotUnit stuff
RobotUnitDataStreaming::DataStreamingDescription keys;
std::vector<RobotUnitDataStreaming::DataEntry> keysList;
RobotUnitDataStreaming::Config cfg;
RobotUnitDataStreamingReceiverPtr handler;
+ std::map<std::string, std::string> configSensorMapping;
struct RobotUnitData
{
@@ -144,9 +128,6 @@ namespace armarx
unsigned int robotUnitMemoryBatchSize = 50;
std::string robotUnitConfigPath = "NO CONFIG SET";
- // RobotUnit stuff
- std::map<std::string, std::string> configSensorMapping;
-
// queue
std::queue<RobotUnitData> robotUnitDataQueue;
mutable std::mutex robotUnitDataMutex;
@@ -154,25 +135,5 @@ namespace armarx
// running tasks
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
-
- // RobotStateComponent stuff
- struct RobotStateComponentData
- {
- long timestamp;
- aron::datanavigator::DictNavigatorPtr data;
- };
-
- // params
- static constexpr int ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY = 100;
- int robotStateComponentPollFrequency = 50;
- unsigned int robotStateComponentMemoryBatchSize = 50;
-
- // queue
- std::queue<RobotStateComponentData> robotStateComponentDataQueue;
- mutable std::mutex robotStateComponentDataMutex;
-
- // running tasks
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotStateComponentConversionTask;
- armarx::PeriodicTask<RobotStateMemory>::pointer_type robotStateComponentStoringTask;
};
}
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 4174ebe9b..2094d02fb 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -23,26 +23,21 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/core/EntityInstance.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-namespace armarx::armem
-{
- class Entity;
-} // namespace armarx
-
namespace armarx::armem
{
@@ -108,7 +103,7 @@ namespace armarx::armem
.coreSegments().withName(properties.mappingMemoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min,query.timeRange.max);
+ .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
// clang-format on
}
@@ -116,7 +111,7 @@ namespace armarx::armem
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, Entity>& entities)
+ std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -125,7 +120,7 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
@@ -145,16 +140,16 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- if (entity.history.empty())
+ if (entity.history().empty())
{
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.history.size();
+ ARMARX_INFO << "History size: " << entity.history().size();
- for (const auto &[ss, entitySnapshot] : entity.history)
+ for (const auto &[ss, entitySnapshot] : entity.history())
{
- for (const auto& entityInstance : entitySnapshot.instances)
+ for (const auto& entityInstance : entitySnapshot.instances())
{
const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
--
GitLab