diff --git a/data/RobotAPI/robotStateMemoryConfig.txt b/data/RobotAPI/robotStateMemoryConfig.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bc928c16990df50ca55c96550f320efac999c332
--- /dev/null
+++ b/data/RobotAPI/robotStateMemoryConfig.txt
@@ -0,0 +1,202 @@
+// 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
+sens.Index L 1 Joint.inverseDynamicsTorque => HandL
+sens.Index L 1 Joint.position => HandL
+sens.Index L 1 Joint.torque => HandL
+sens.Index L 1 Joint.velocity => HandL
+sens.Index L 2 Joint.acceleration => HandL
+sens.Index L 2 Joint.gravityTorque => HandL
+sens.Index L 2 Joint.inertiaTorque => HandL
+sens.Index L 2 Joint.inverseDynamicsTorque => HandL
+sens.Index L 2 Joint.position => HandL
+sens.Index L 2 Joint.torque => HandL
+sens.Index L 2 Joint.velocity => HandL
+sens.Index L 3 Joint.acceleration => HandL
+sens.Index L 3 Joint.gravityTorque => HandL
+sens.Index L 3 Joint.inertiaTorque => HandL
+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.Middle L 1 Joint.acceleration => HandL
+sens.Middle L 1 Joint.gravityTorque => HandL
+sens.Middle L 1 Joint.inertiaTorque => HandL
+sens.Middle L 1 Joint.inverseDynamicsTorque => HandL
+sens.Middle L 1 Joint.position => HandL
+sens.Middle L 1 Joint.torque => HandL
+sens.Middle L 1 Joint.velocity => HandL
+sens.Middle L 2 Joint.acceleration => HandL
+sens.Middle L 2 Joint.gravityTorque => HandL
+sens.Middle L 2 Joint.inertiaTorque => HandL
+sens.Middle L 2 Joint.inverseDynamicsTorque => HandL
+sens.Middle L 2 Joint.position => HandL
+sens.Middle L 2 Joint.torque => HandL
+sens.Middle L 2 Joint.velocity => HandL
+sens.Middle L 3 Joint.acceleration => HandL
+sens.Middle L 3 Joint.gravityTorque => HandL
+sens.Middle L 3 Joint.inertiaTorque => HandL
+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.Pinky L 1 Joint.acceleration => HandL
+sens.Pinky L 1 Joint.gravityTorque => HandL
+sens.Pinky L 1 Joint.inertiaTorque => HandL
+sens.Pinky L 1 Joint.inverseDynamicsTorque => HandL
+sens.Pinky L 1 Joint.position => HandL
+sens.Pinky L 1 Joint.torque => HandL
+sens.Pinky L 1 Joint.velocity => HandL
+sens.Pinky L 2 Joint.acceleration => HandL
+sens.Pinky L 2 Joint.gravityTorque => HandL
+sens.Pinky L 2 Joint.inertiaTorque => HandL
+sens.Pinky L 2 Joint.inverseDynamicsTorque => HandL
+sens.Pinky L 2 Joint.position => HandL
+sens.Pinky L 2 Joint.torque => HandL
+sens.Pinky L 2 Joint.velocity => HandL
+sens.Pinky L 3 Joint.acceleration => HandL
+sens.Pinky L 3 Joint.gravityTorque => HandL
+sens.Pinky L 3 Joint.inertiaTorque => HandL
+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.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/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 83fd40e2dee27f8459c028474b01f178218f752f..48313a312edc1c6d4e6c19ee18f62e4f3072d87f 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -1,5 +1,5 @@
add_subdirectory(ExampleMemory)
add_subdirectory(GeneralPurposeMemory)
add_subdirectory(ObjectMemory)
-add_subdirectory(RobotSensorMemory)
+add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
similarity index 80%
rename from source/RobotAPI/components/armem/server/RobotSensorMemory/CMakeLists.txt
rename to source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 14939bd0373bc2df2d38d22e7d81133a35c75914..4667365b1a6187694ca7bdbd06003275828ead55 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -1,4 +1,4 @@
-armarx_component_set_name("RobotSensorMemory")
+armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
@@ -9,10 +9,10 @@ set(COMPONENT_LIBS
)
set(SOURCES
- RobotSensorMemory.cpp
+ RobotStateMemory.cpp
)
set(HEADERS
- RobotSensorMemory.h
+ RobotStateMemory.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
similarity index 60%
rename from source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
rename to source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index a719b42bf112d6d7a58f06066dee3b96efabae83..3298e26721c98aceec2034c5b585ab2de44d4c25 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -22,15 +22,18 @@
// STD
#include <algorithm>
+#include <iostream>
+#include <fstream>
// Header
-#include "RobotSensorMemory.h"
+#include "RobotStateMemory.h"
// Simox
#include <SimoxUtility/algorithm/string.h>
// 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>
@@ -39,79 +42,87 @@
namespace armarx
{
- RobotSensorMemory::RobotSensorMemory()
+ RobotStateMemory::RobotStateMemory()
{
}
- armarx::PropertyDefinitionsPtr RobotSensorMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->optional(robotUnitSensorPrefix, "SensorValuePrefix", "Prefix of all sensor values");
defs->optional(robotUnitMemoryBatchSize, "RobotUnitMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
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(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));
+ defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
return defs;
}
- std::string RobotSensorMemory::getDefaultName() const
+ std::string RobotStateMemory::getDefaultName() const
{
- return "RobotSensorMemory";
+ return "RobotStateMemory";
}
- void RobotSensorMemory::onInitComponent()
+ void RobotStateMemory::onInitComponent()
{
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;
}
- void RobotSensorMemory::onConnectComponent()
+ void RobotStateMemory::onConnectComponent()
{
setupRobotUnitSegment();
- setupRobotStateComponentSegment();
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
keys = handler->getDataDescription();
- robotUnitConversionTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
- robotUnitStoringTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
-
- robotStateComponentConversionTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::convertRobotStateComponentDataToAron, (1000 / robotStateComponentPollFrequency));
- robotStateComponentStoringTask = new PeriodicTask<RobotSensorMemory>(this, &RobotSensorMemory::storeConvertedRobotStateComponentDataInMemory, (1000 / robotStateComponentPollFrequency));
+ robotUnitConversionTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::convertRobotUnitStreamingDataToAron, (1000 / robotUnitPollFrequency));
+ robotUnitStoringTask = new PeriodicTask<RobotStateMemory>(this, &RobotStateMemory::storeConvertedRobotUnitDataInMemory, (1000 / robotUnitPollFrequency));
startRobotUnitStream();
- startRobotStateComponentPoll();
}
- void RobotSensorMemory::onDisconnectComponent()
+ void RobotStateMemory::onDisconnectComponent()
{
stopRobotUnitStream();
- startRobotStateComponentPoll();
}
- void RobotSensorMemory::onExitComponent()
+ void RobotStateMemory::onExitComponent()
{
stopRobotUnitStream();
- stopRobotStateComponentPoll();
}
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotSensorMemory::setupRobotUnitSegment()
+ void RobotStateMemory::setupRobotUnitSegment()
{
ARMARX_INFO << "Adding core segment " << robotUnitCoreSegmentName;
workingMemory.addCoreSegments({robotUnitCoreSegmentName});
@@ -141,7 +152,7 @@ namespace armarx
robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
}
- void RobotSensorMemory::convertRobotUnitStreamingDataToAron()
+ void RobotStateMemory::convertRobotUnitStreamingDataToAron()
{
auto& data = handler->getDataBuffer();
if (data.size() == 0)
@@ -156,14 +167,22 @@ namespace armarx
RobotUnitData convertedAndGroupedData;
for (const auto& [name, dataEntry] : keys.entries)
{
- size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
- if (dot_pos == std::string::npos)
+ std::string groupName = "";
+ if (configSensorMapping.find(name) != configSensorMapping.end())
{
- 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 = configSensorMapping.at(name);
+ }
+ else
+ {
+ size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
+ if (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, dot_pos);
}
- std::string groupName = name.substr(0, dot_pos);
RobotUnitData::RobotUnitDataGroup e;
if (auto it = convertedAndGroupedData.groups.find(groupName); it == convertedAndGroupedData.groups.end())
{
@@ -251,7 +270,7 @@ namespace armarx
ARMARX_DEBUG << "RobotUnitData: The total time needed to convert the data to aron is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
}
- void RobotSensorMemory::storeConvertedRobotUnitDataInMemory()
+ void RobotStateMemory::storeConvertedRobotUnitDataInMemory()
{
unsigned int size = 0;
{
@@ -301,14 +320,14 @@ namespace armarx
}
}
- void RobotSensorMemory::stopRobotUnitStream()
+ void RobotStateMemory::stopRobotUnitStream()
{
std::lock_guard g{startStopMutex};
robotUnitConversionTask->stop();
robotUnitStoringTask->stop();
}
- void RobotSensorMemory::startRobotUnitStream()
+ void RobotStateMemory::startRobotUnitStream()
{
std::lock_guard g{startStopMutex};
if (robotUnitConversionTask->isRunning() || robotUnitStoringTask->isRunning())
@@ -368,152 +387,48 @@ namespace armarx
}
ARMARX_INFO << ss.str();
- robotUnitConversionTask->start();
- robotUnitStoringTask->start();
- }
-
-
- /*************************************************************/
- // RobotStateComponent Polling functions
- /*************************************************************/
- void RobotSensorMemory::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)
- {
- ARMARX_ERROR << "Could not add segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName << ". The error message is: " << result.errorMessage;
- }
-
- robotStateComponentProviderID.memoryName = workingMemoryName;
- robotStateComponentProviderID.coreSegmentName = robotStateComponentCoreSegmentName;
- robotStateComponentProviderID.providerSegmentName = robotStateComponentProviderSegmentName;
- }
-
- void RobotSensorMemory::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;
-
+ // parse config
+ std::filesystem::path configPath(robotUnitConfigPath);
+ configSensorMapping.clear();
+ if (std::filesystem::is_regular_file(configPath))
{
- 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 RobotSensorMemory::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)
+ 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 (!result.success)
+ if (line.size() > 0)
{
- ARMARX_WARNING << "Could not add data to memory. Error message: " << result.errorMessage;
+ 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]});
+ }
+ }
}
}
}
- }
-
- void RobotSensorMemory::stopRobotStateComponentPoll()
- {
- std::lock_guard g{startStopMutex};
- robotStateComponentConversionTask->stop();
- robotStateComponentStoringTask->stop();
- }
-
- void RobotSensorMemory::startRobotStateComponentPoll()
- {
- std::lock_guard g{startStopMutex};
- if (robotStateComponentConversionTask->isRunning() || robotStateComponentStoringTask->isRunning())
+ else
{
- if (robotStateComponentConversionTask->isRunning() && robotStateComponentStoringTask->isRunning())
- {
- return;
- }
- ARMARX_WARNING << "Found inconsistency in running tasks. Restarting all!";
- stopRobotStateComponentPoll();
+ ARMARX_INFO << "The config path '" << robotUnitConfigPath << "' is not valid.";
}
- ARMARX_INFO << "Start polling of robotStateComponent";
-
- robotStateComponentConversionTask->start();
- robotStateComponentStoringTask->start();
+ robotUnitConversionTask->start();
+ robotUnitStoringTask->start();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
similarity index 68%
rename from source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h
rename to source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index b16ee1296319b386ee77760a2c81df53019e37c4..9787ac6f54f82dd0b91398fe18af136f7d0b1cf2 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.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>
@@ -55,15 +53,14 @@ namespace armarx
*
* Detailed description of class RobotSensorMemory.
*/
- class RobotSensorMemory :
+ class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser,
- virtual public RobotStateComponentPluginUser
+ virtual public RobotUnitComponentPluginUser
{
public:
- RobotSensorMemory();
+ RobotStateMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -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
{
@@ -142,33 +126,14 @@ namespace armarx
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<RobotSensorMemory>::pointer_type robotUnitConversionTask;
- armarx::PeriodicTask<RobotSensorMemory>::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<RobotSensorMemory>::pointer_type robotStateComponentConversionTask;
- armarx::PeriodicTask<RobotSensorMemory>::pointer_type robotStateComponentStoringTask;
+ armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
+ armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
};
}
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index d035b2b7926376c0e16e38bcfd6d8182f4609d87..99ed2fa4129d1caea1ea52960e0c37fe4ec9fd44 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -1,5 +1,6 @@
#include "MappingDataReader.h"
+// STD / STL
#include <cstring>
#include <vector>
#include <algorithm>
@@ -9,36 +10,45 @@
#include <type_traits>
#include <utility>
+// ICE
#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+// ArmarXCore
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+// RobotAPI Interfaces
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+
+// RobotAPI Aron
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+// RobotAPI Armem
#include <RobotAPI/libraries/armem/client/Query.h>
#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/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.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
{
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index c2afd4f51bc1c18dd1c509a7328f31720eb519bf..c136ba4ac8ebdf2b9195252b75c00844e9b56e49 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -18,7 +18,6 @@ set(LIBS
ArmarXCore
RobotAPIInterfaces
cppgen
-
Simox::SimoxUtility
# System libraries