diff --git a/data/RobotAPI/robotStateMemoryConfig.txt b/data/RobotAPI/robotStateMemoryConfig.txt
index bc928c16990df50ca55c96550f320efac999c332..e6e6498e4ce03f08c6b930cb3b73c678a578f34c 100644
--- a/data/RobotAPI/robotStateMemoryConfig.txt
+++ b/data/RobotAPI/robotStateMemoryConfig.txt
@@ -1,5 +1,7 @@
// Configuration set for the Humanoid Robot ARMAR-6
-// Left Hand unit
+
+// Reft Hand
+
sens.Index L 1 Joint.acceleration => HandL
sens.Index L 1 Joint.gravityTorque => HandL
sens.Index L 1 Joint.inertiaTorque => HandL
@@ -103,7 +105,9 @@ sens.Thumb L 2 Joint.position => HandL
sens.Thumb L 2 Joint.torque => HandL
sens.Thumb L 2 Joint.velocity => HandL
-// Right Hand unit
+
+// Right Hand
+
sens.Index R 1 Joint.acceleration => HandR
sens.Index R 1 Joint.gravityTorque => HandR
sens.Index R 1 Joint.inertiaTorque => HandR
@@ -185,7 +189,165 @@ sens.Thumb R 2 Joint.position => HandR
sens.Thumb R 2 Joint.torque => HandR
sens.Thumb R 2 Joint.velocity => HandR
+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 => HandR
+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
+
+
+// Arm L
+
+sens.ArmL1_Cla1.acceleration => ArmL
+sens.ArmL1_Cla1.gravityTorque => ArmL
+sens.ArmL1_Cla1.inertiaTorque => ArmL
+sens.ArmL1_Cla1.inverseDynamicsTorque => ArmL
+sens.ArmL1_Cla1.position => ArmL
+sens.ArmL1_Cla1.torque => ArmL
+sens.ArmL1_Cla1.velocity => ArmL
+
+sens.ArmL2_Sho1.acceleration => ArmL
+sens.ArmL2_Sho1.gravityTorque => ArmL
+sens.ArmL2_Sho1.inertiaTorque => ArmL
+sens.ArmL2_Sho1.inverseDynamicsTorque => ArmL
+sens.ArmL2_Sho1.position => ArmL
+sens.ArmL2_Sho1.torque => ArmL
+sens.ArmL2_Sho1.velocity => ArmL
+
+sens.ArmL3_Sho2.acceleration => ArmL
+sens.ArmL3_Sho2.gravityTorque => ArmL
+sens.ArmL3_Sho2.inertiaTorque => ArmL
+sens.ArmL3_Sho2.inverseDynamicsTorque => ArmL
+sens.ArmL3_Sho2.position => ArmL
+sens.ArmL3_Sho2.torque => ArmL
+sens.ArmL3_Sho2.velocity => ArmL
+
+sens.ArmL4_Sho3.acceleration => ArmL
+sens.ArmL4_Sho3.gravityTorque => ArmL
+sens.ArmL4_Sho3.inertiaTorque => ArmL
+sens.ArmL4_Sho3.inverseDynamicsTorque => ArmL
+sens.ArmL4_Sho3.position => ArmL
+sens.ArmL4_Sho3.torque => ArmL
+sens.ArmL4_Sho3.velocity => ArmL
+
+sens.ArmL5_Elb1.acceleration => ArmL
+sens.ArmL5_Elb1.gravityTorque => ArmL
+sens.ArmL5_Elb1.inertiaTorque => ArmL
+sens.ArmL5_Elb1.inverseDynamicsTorque => ArmL
+sens.ArmL5_Elb1.position => ArmL
+sens.ArmL5_Elb1.torque => ArmL
+sens.ArmL5_Elb1.velocity => ArmL
+
+sens.ArmL6_Elb2.acceleration => ArmL
+sens.ArmL6_Elb2.gravityTorque => ArmL
+sens.ArmL6_Elb2.inertiaTorque => ArmL
+sens.ArmL6_Elb2.inverseDynamicsTorque => ArmL
+sens.ArmL6_Elb2.position => ArmL
+sens.ArmL6_Elb2.torque => ArmL
+sens.ArmL6_Elb2.velocity => ArmL
+
+sens.ArmL7_Wri1.acceleration => ArmL
+sens.ArmL7_Wri1.gravityTorque => ArmL
+sens.ArmL7_Wri1.inertiaTorque => ArmL
+sens.ArmL7_Wri1.inverseDynamicsTorque => ArmL
+sens.ArmL7_Wri1.position => ArmL
+sens.ArmL7_Wri1.torque => ArmL
+sens.ArmL7_Wri1.velocity => ArmL
+
+sens.ArmL8_Wri2.acceleration => ArmL
+sens.ArmL8_Wri2.gravityTorque => ArmL
+sens.ArmL8_Wri2.inertiaTorque => ArmL
+sens.ArmL8_Wri2.inverseDynamicsTorque => ArmL
+sens.ArmL8_Wri2.position => ArmL
+sens.ArmL8_Wri2.torque => ArmL
+sens.ArmL8_Wri2.velocity => ArmL
+
+
+// Arm R
+
+sens.ArmR1_Cla1.acceleration => ArmR
+sens.ArmR1_Cla1.gravityTorque => ArmR
+sens.ArmR1_Cla1.inertiaTorque => ArmR
+sens.ArmR1_Cla1.inverseDynamicsTorque => ArmR
+sens.ArmR1_Cla1.position => ArmR
+sens.ArmR1_Cla1.torque => ArmR
+sens.ArmR1_Cla1.velocity => ArmR
+
+sens.ArmR2_Sho1.acceleration => ArmR
+sens.ArmR2_Sho1.gravityTorque => ArmR
+sens.ArmR2_Sho1.inertiaTorque => ArmR
+sens.ArmR2_Sho1.inverseDynamicsTorque => ArmR
+sens.ArmR2_Sho1.position => ArmR
+sens.ArmR2_Sho1.torque => ArmR
+sens.ArmR2_Sho1.velocity => ArmR
+
+sens.ArmR3_Sho2.acceleration => ArmR
+sens.ArmR3_Sho2.gravityTorque => ArmR
+sens.ArmR3_Sho2.inertiaTorque => ArmR
+sens.ArmR3_Sho2.inverseDynamicsTorque => ArmR
+sens.ArmR3_Sho2.position => ArmR
+sens.ArmR3_Sho2.torque => ArmR
+sens.ArmR3_Sho2.velocity => ArmR
+
+sens.ArmR4_Sho3.acceleration => ArmR
+sens.ArmR4_Sho3.gravityTorque => ArmR
+sens.ArmR4_Sho3.inertiaTorque => ArmR
+sens.ArmR4_Sho3.inverseDynamicsTorque => ArmR
+sens.ArmR4_Sho3.position => ArmR
+sens.ArmR4_Sho3.torque => ArmR
+sens.ArmR4_Sho3.velocity => ArmR
+
+sens.ArmR5_Elb1.acceleration => ArmR
+sens.ArmR5_Elb1.gravityTorque => ArmR
+sens.ArmR5_Elb1.inertiaTorque => ArmR
+sens.ArmR5_Elb1.inverseDynamicsTorque => ArmR
+sens.ArmR5_Elb1.position => ArmR
+sens.ArmR5_Elb1.torque => ArmR
+sens.ArmR5_Elb1.velocity => ArmR
+
+sens.ArmR6_Elb2.acceleration => ArmR
+sens.ArmR6_Elb2.gravityTorque => ArmR
+sens.ArmR6_Elb2.inertiaTorque => ArmR
+sens.ArmR6_Elb2.inverseDynamicsTorque => ArmR
+sens.ArmR6_Elb2.position => ArmR
+sens.ArmR6_Elb2.torque => ArmR
+sens.ArmR6_Elb2.velocity => ArmR
+
+sens.ArmR7_Wri1.acceleration => ArmR
+sens.ArmR7_Wri1.gravityTorque => ArmR
+sens.ArmR7_Wri1.inertiaTorque => ArmR
+sens.ArmR7_Wri1.inverseDynamicsTorque => ArmR
+sens.ArmR7_Wri1.position => ArmR
+sens.ArmR7_Wri1.torque => ArmR
+sens.ArmR7_Wri1.velocity => ArmR
+
+sens.ArmR8_Wri2.acceleration => ArmR
+sens.ArmR8_Wri2.gravityTorque => ArmR
+sens.ArmR8_Wri2.inertiaTorque => ArmR
+sens.ArmR8_Wri2.inverseDynamicsTorque => ArmR
+sens.ArmR8_Wri2.position => ArmR
+sens.ArmR8_Wri2.torque => ArmR
+sens.ArmR8_Wri2.velocity => ArmR
+
+
// Neck
+
sens.Neck_1_Yaw.acceleration => Neck
sens.Neck_1_Yaw.gravityTorque => Neck
sens.Neck_1_Yaw.inertiaTorque => Neck
@@ -193,10 +355,70 @@ 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.inverseDynamicsTorque => Neck
sens.Neck_2_Pitch.position => Neck
sens.Neck_2_Pitch.torque => Neck
sens.Neck_2_Pitch.velocity => Neck
+
+
+// Torso
+
+sens.TorsoJoint.acceleration => Torso
+sens.TorsoJoint.gravityTorque => Torso
+sens.TorsoJoint.inertiaTorque => Torso
+sens.TorsoJoint.inverseDynamicsTorque => Torso
+sens.TorsoJoint.position => Torso
+sens.TorsoJoint.torque => Torso
+sens.TorsoJoint.velocity => Torso
+
+
+// Platform
+
+sens.Platform.accelerationX => Platform
+sens.Platform.accelerationY => Platform
+sens.Platform.accelerationRotation => Platform
+sens.Platform.absolutePositionX => Platform
+sens.Platform.absolutePositionY => Platform
+sens.Platform.absolutePositionRotation => Platform
+sens.Platform.relativePositionX => Platform
+sens.Platform.relativePositionY => Platform
+sens.Platform.relativePositionRotation => Platform
+sens.Platform.velocityX => Platform
+sens.Platform.velocityY => Platform
+sens.Platform.velocityRotation => Platform
+
+
+// FT L
+
+sens.FT L.fx => FT L
+sens.FT L.fy => FT L
+sens.FT L.fz => FT L
+sens.FT L.gravCompFx => FT L
+sens.FT L.gravCompFy => FT L
+sens.FT L.gravCompFz => FT L
+sens.FT L.gravCompTx => FT L
+sens.FT L.gravCompTy => FT L
+sens.FT L.gravCompTz => FT L
+sens.FT L.tx => FT L
+sens.FT L.ty => FT L
+sens.FT L.tz => FT L
+
+
+// FT R
+
+sens.FT R.fx => FT R
+sens.FT R.fy => FT R
+sens.FT R.fz => FT R
+sens.FT R.gravCompFx => FT R
+sens.FT R.gravCompFy => FT R
+sens.FT R.gravCompFz => FT R
+sens.FT R.gravCompTx => FT R
+sens.FT R.gravCompTy => FT R
+sens.FT R.gravCompTz => FT R
+sens.FT R.tx => FT R
+sens.FT R.ty => FT R
+sens.FT R.tz => FT R
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 3ed4b4612c5dd3f6e0afbd71630dba7a4753f460..e007e8f5e3ad47c57f9bb71c75e517d57af1addf 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -122,7 +122,7 @@ namespace armarx
using namespace armarx::RemoteGui::Client;
{
- std::scoped_lock lock(workingMemoryMutex);
+ // Core segments are locked by MemoryRemoteGui.
tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory);
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 91ea5b23400f568891326f6dd6c79b5e603d229c..c624379b89ae336aa561e364706f3780b7fbf0e9 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -59,22 +59,27 @@ namespace armarx::armem::server::obj
return defs;
}
+
ObjectMemory::ObjectMemory() :
server::ComponentPluginUser(),
- instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- classSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- attachmentSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ instance::SegmentAdapter(server::ComponentPluginUser::iceMemory),
+ classSegment(server::ComponentPluginUser::iceMemory),
+ attachmentSegment(server::ComponentPluginUser::iceMemory)
+ {
+ }
+
+
+ ObjectMemory::~ObjectMemory()
{
}
+
std::string ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
+
void ObjectMemory::onInitComponent()
{
workingMemory.name() = defaultMemoryName;
@@ -114,6 +119,7 @@ namespace armarx::armem::server::obj
});
}
+
void ObjectMemory::onConnectComponent()
{
// onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
@@ -141,17 +147,17 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
- attachmentSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
+ attachmentSegment.connect();
RemoteGui_startRunningTask();
}
+
void ObjectMemory::onDisconnectComponent()
{
}
+
void ObjectMemory::onExitComponent()
{
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index f29b2cef93efb2aa35a7983f4dce87d3db854b80..b90406be31ac76a40172d67076f931ebb1a6ba1a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -38,8 +38,6 @@
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
@@ -81,6 +79,7 @@ namespace armarx::armem::server::obj
public:
ObjectMemory();
+ virtual ~ObjectMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 137453970c1a97f0f3bd9653c067e959db21b4a5..a25161ff2cdf35173921720f68ad5bcec16cba30 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 111cced66ba9f678ba852d1005a10ded834d0946..c108b6e78ae6932f129ab4473f235d5f9740ea3b 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -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();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index e8174416e9b547f70af5e5c6b578242a0234743d..39048334904470b929e0ebe045083a63d4f59125 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -24,20 +24,13 @@
// STD
#include <atomic>
-#include <thread>
+#include <optional>
#include <queue>
-// Simox
-#include <SimoxUtility/meta/enum/adapt_enum.h>
-
// ArmarX
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-// BaseClass
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
@@ -46,6 +39,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,44 +66,41 @@ 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() override;
- virtual ~RobotStateMemory();
- /// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+
protected:
- /// @see armarx::ManagedIceObject::onInitComponent()
- void onInitComponent() override;
- /// @see armarx::ManagedIceObject::onConnectComponent()
- void onConnectComponent() override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onInitComponent() override;
+ void onConnectComponent() override;
void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @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 +118,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 0000000000000000000000000000000000000000..39439e2e77bbd8c3ebea44782ae44d62286c6ed9
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.cpp
@@ -0,0 +1,198 @@
+/*
+ * 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)
+ {
+ 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 endProprioception = start, endLocalization = start;
+
+ armem::CommitResult result;
+ {
+ // Commits lock the core segments.
+
+ result = memory.commitLocking(update.proprioception);
+ endProprioception = std::chrono::high_resolution_clock::now();
+
+ localizationSegment.commitTransformLocking(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 Proprioception [ms]", toDurationMs(start, 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 0000000000000000000000000000000000000000..93ba74d2d9d622f4d72d1d5a4d08acd97f3eb64b
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateWriter.h
@@ -0,0 +1,98 @@
+/*
+ * 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
+ );
+
+
+ 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 0000000000000000000000000000000000000000..9a9b72e2a74203cb9f44dd54796b397770257ecd
--- /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 0000000000000000000000000000000000000000..dc43dcddca3eb693fe5200dcc0a256a15be23cac
--- /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 0000000000000000000000000000000000000000..58b688557eb86e8efb28afd0329d8f3b50e8508c
--- /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 0000000000000000000000000000000000000000..a44c9c0f19a586f48b3f899ae314ca70cc223abe
--- /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 0000000000000000000000000000000000000000..8caf73057ac54df453f6e3c2d67295dc7e69ab89
--- /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 0000000000000000000000000000000000000000..dd2177c9e2f167af3946119f381dcd7ec43d170c
--- /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/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 4e6e7966cc4c71e819da47cd9a7ee06ccd536775..6647cf38585cb868a3df29e9c328bbfe8ac3a677 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -116,6 +116,19 @@ namespace armarx::plugins
}
+ void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond) const
+ {
+ ARMARX_INFO << "Waiting until robot unit is running ...";
+
+ while (not(termCond() or not isNullptr(getRobotUnit()) or getRobotUnit()->isRunning()))
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+
+ ARMARX_INFO << "Robot unit is up and running.";
+ }
+
+
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 80357a96e9a3bb6c889d7b73435baef90a385b54..4371934b97182947afbaad3d60fd5295a717615a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -34,6 +34,19 @@ namespace armarx
}
+ /**
+ * @brief Waits until the robot unit is running.
+ *
+ * Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
+ * not be fully initialized.
+ *
+ * @param termCond Termination condition.
+ * If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
+ * for the robot unit to become available.
+ */
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+
+
//controllers
public:
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index eb53706cb8836fc40891372811738240b3522aa0..c606ae195f054e3d5afab9e075ffa3b04d884dff 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -99,6 +99,8 @@ set(LIB_FILES
server/RemoteGuiAronDataVisitor.cpp
server/Segment.cpp
+ server/segment/SpecializedSegment.cpp
+
server/query_proc/base/BaseQueryProcessorBase.cpp
server/query_proc/base/EntityQueryProcessorBase.cpp
server/query_proc/base/ProviderSegmentQueryProcessorBase.cpp
@@ -140,6 +142,7 @@ set(LIB_HEADERS
core/aron_conversions.h
core/json_conversions.h
core/ice_conversions.h
+ core/ice_conversions_boost_templates.h
core/ice_conversions_templates.h
core/error.h
@@ -220,6 +223,8 @@ set(LIB_HEADERS
server/RemoteGuiAronDataVisitor.h
server/Segment.h
+ server/segment/SpecializedSegment.h
+
server/query_proc.h
server/query_proc/base/BaseQueryProcessorBase.h
server/query_proc/base/EntityQueryProcessorBase.h
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
index a68e742cd24489694da91ef73ff557a6e8be1294..a88f374d6bddede5fd0b05eb28481d524f5f6b0a 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
@@ -31,7 +31,8 @@ armarx::armem::client::ComponentPluginUser::setMemoryServer(server::MemoryInterf
armarx::armem::data::WaitForMemoryResult
armarx::armem::client::ComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
if (result.proxy)
{
setMemoryServer(result.proxy);
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index 3bdbde93bd046bcceb361d94183be5fd1c09db23..ae3f26b26557c11046d84cde8a07483e4ba9ad49 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -2,7 +2,7 @@
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
index 3204c83cc094f3895e4ace10a4b91c9cdfeb1070..3ac9f7438408460c157f7484d313ef0d6a3808a6 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -2,6 +2,7 @@
// ArmarX
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
// RobotAPI
@@ -47,11 +48,13 @@ namespace armarx::armem::client
armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(memoryName);
if (result.proxy)
{
setReadingMemoryServer(result.proxy);
}
return result;
}
+
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 0a2e37ba953e024858090cc64d6510dfebfd8a77..c0139e609e830e2c07517837f6cc3d48aff434db 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -5,7 +5,7 @@
#include <vector>
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
index 093f29e6fa2178f19ae13983e21772f17400856d..d4b95ca3565e8552322e7e7cdfac131335ca06f3 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
@@ -2,6 +2,7 @@
// ArmarX
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
// RobotAPI
@@ -43,9 +44,11 @@ namespace armarx::armem::client
memoryWriter.setWritingMemory(memory);
}
+
armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
+ armem::data::WaitForMemoryResult result;
+ result.proxy = memoryNameSystem.useServer(MemoryID().withMemoryName(memoryName));
if (result.proxy)
{
setWritingMemoryServer(result.proxy);
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 435881b48b27b9053dd5da5e87b794d3561f3b3c..3ae9a66bfab912237934d4bea6580a2e32a2c7fb 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -2,7 +2,7 @@
// ArmarX
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
// RobotAPI
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index ab227c33ee321475f129d8f864b5cbf31e3f4845..0775a71f977fde16966550f0506e9fd0f591e9a5 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -114,13 +114,13 @@ namespace armarx::armem::base
}
- inline const std::map<Time, EntitySnapshotT>& history() const
+ inline const ContainerT& history() const
{
return this->_container;
}
- inline std::map<Time, EntitySnapshotT>& history()
+ inline ContainerT& history()
{
- return const_cast<std::map<Time, EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
+ return const_cast<ContainerT&>(const_cast<const EntityBase*>(this)->history());
}
@@ -226,7 +226,7 @@ namespace armarx::armem::base
virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
{
// first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
if (refIt == this->_container.begin())
{
@@ -238,7 +238,7 @@ namespace armarx::armem::base
}
// last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+ typename ContainerT::const_iterator refItPrev = std::prev(refIt);
// ... or we return the element before if possible
if (refItPrev != this->_container.begin())
@@ -259,7 +259,7 @@ namespace armarx::armem::base
virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
{
// first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
if (refIt == this->_container.begin())
{
@@ -267,7 +267,7 @@ namespace armarx::armem::base
}
// last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+ typename ContainerT::const_iterator refItPrev = std::prev(refIt);
// we return the element before if possible
if (refItPrev != this->_container.begin())
@@ -311,7 +311,7 @@ namespace armarx::armem::base
virtual const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
{
// first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
if (refIt == this->_container.end())
{
@@ -330,7 +330,7 @@ namespace armarx::armem::base
virtual const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
{
// first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+ typename ContainerT::const_iterator refIt = this->_container.upper_bound(time);
if (refIt == this->_container.end())
{
@@ -343,7 +343,7 @@ namespace armarx::armem::base
}
// first element greater than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+ typename ContainerT::const_iterator refItNext = std::next(refIt);
if (refItNext != this->_container.end())
{
@@ -418,7 +418,7 @@ namespace armarx::armem::base
EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
{
- auto it = this->_container.emplace(snapshot.time(), std::move(snapshot)).first;
+ auto it = this->_container.emplace_hint(this->_container.end(), snapshot.time(), std::move(snapshot));
it->second.id().setEntityID(this->id());
return it->second;
}
@@ -473,7 +473,9 @@ namespace armarx::armem::base
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
+ virtual
+ const typename ContainerT::value_type&
+ getLatestItem() const
{
if (this->_container.empty())
{
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index d7c8b8cd203fc8b140cbed0b959110f0868a4b83..73e70478fb9713536c266b01e9004ceb838be2a4 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -223,7 +223,7 @@ namespace armarx::armem::base
* @param commit The commit.
* @return The resulting memory IDs.
*/
- std::vector<UpdateResult> update(const Commit& commit)
+ virtual std::vector<UpdateResult> update(const Commit& commit)
{
std::vector<UpdateResult> results;
for (const auto& update : commit.updates)
@@ -238,7 +238,7 @@ namespace armarx::armem::base
* @param update The update.
* @return The resulting entity snapshot's ID.
*/
- UpdateResult update(const EntityUpdate& update)
+ virtual UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.memoryName, this->name());
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
index 0f1bd0be881f477908464bcda5a4a858afb54488..b166468d81929da12c8204a6e2df9ab90b994528 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -96,7 +96,7 @@ namespace armarx::armem::d_ltm
return;
}
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b6681236bc3d0517d6fd61656a1dc598a8d6a36
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <map>
+
+#include <boost/container/flat_map.hpp>
+
+
+namespace armarx::armem
+{
+ // std::map <-> boost::container::flat_map
+
+ template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
+ void toIce(std::map<IceKeyT, IceValueT>& iceMap,
+ const boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ {
+ iceMap.clear();
+ for (const auto& [key, value] : cppMap)
+ {
+ iceMap.emplace(toIce<IceKeyT>(key), toIce<IceValueT>(value));
+ }
+ }
+
+ template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
+ void fromIce(const std::map<IceKeyT, IceValueT>& iceMap,
+ boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ {
+ cppMap.clear();
+ for (const auto& [key, value] : iceMap)
+ {
+ cppMap.emplace(fromIce<CppKeyT>(key), fromIce<CppValueT>(value));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index 153852eaf50f4d71573c1bafdc44b052c0e1317c..aeb2be76e6ae6951188af4174b9b11dc51d736fc 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -1,5 +1,6 @@
#include "Entity.h"
+
namespace armarx::armem::ltm
{
@@ -106,7 +107,7 @@ namespace armarx::armem::ltm
snapshot.addInstance(instance);
}
- _container.insert({time, snapshot});
+ _container.emplace(time, snapshot);
//truncateHistoryToSize();
return true;
}
@@ -142,7 +143,7 @@ namespace armarx::armem::ltm
return Base::getSnapshot(time);
}
- const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
+ auto Entity::getLatestItem() const -> const ContainerT::value_type&
{
// Directly query mongodb (cache cant know whether it is the latest or not)
// TODO
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index a5bca08c7ee0b7aff2a79d2c45aa37fc4baec1c4..20377befd61f741a642e139aafe1de3c2cc7f24b 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -71,7 +71,7 @@ namespace armarx::armem::ltm
protected:
// virtual overrides for LTM storage
- virtual const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const override;
+ virtual const ContainerT::value_type& getLatestItem() const override;
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index 285235b76e397952301c23d8218abb11f1f7aeaf..601f079350de8536aff130943d539eafee4e43b3 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -8,6 +8,36 @@
namespace armarx::armem::wm
{
+ CoreSegment::CoreSegment(const CoreSegment& other) :
+ CoreSegment::Base(other),
+ _mutex()
+ {
+ // Do not copy _mutex.
+ }
+
+
+ CoreSegment::CoreSegment(CoreSegment&& other) : CoreSegment::Base(other)
+ {
+ // Do not move _mutex.
+ }
+
+
+ CoreSegment& CoreSegment::operator=(const CoreSegment& other)
+ {
+ Base::operator=(other);
+ // Don't copy _mutex.
+ return *this;
+ }
+
+
+ CoreSegment& CoreSegment::operator=(CoreSegment&& other)
+ {
+ Base::operator=(other);
+ // Don't move _mutex.
+ return *this;
+ }
+
+
Commit CoreSegment::toCommit() const
{
Commit c;
@@ -18,6 +48,7 @@ namespace armarx::armem::wm
return c;
}
+
void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
{
other.id() = _id;
@@ -27,4 +58,79 @@ namespace armarx::armem::wm
other.addProviderSegment(s.copyWithoutData());
}
}
+
+
+ std::mutex& CoreSegment::mutex() const
+ {
+ return _mutex;
+ }
+
+
+ std::optional<wm::EntitySnapshot> CoreSegment::getLatestEntitySnapshot(const MemoryID& entityID) const
+ {
+ const wm::Entity& entity = this->getEntity(entityID);
+ if (entity.empty())
+ {
+ return std::nullopt;
+ }
+ else
+ {
+ return entity.getLatestSnapshot();
+ }
+ }
+
+
+ std::optional<wm::EntityInstance> CoreSegment::getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex) const
+ {
+ auto snapshot = getLatestEntitySnapshot(entityID);
+ if (snapshot.has_value()
+ and instanceIndex >= 0
+ and static_cast<size_t>(instanceIndex) < snapshot->size())
+ {
+ return snapshot->getInstance(instanceIndex);
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armarx::aron::datanavigator::DictNavigatorPtr
+ CoreSegment::getLatestEntityInstanceData(const MemoryID& entityID, int instanceIndex) const
+ {
+ auto instance = getLatestEntityInstance(entityID, instanceIndex);
+ if (instance.has_value())
+ {
+ return instance->data();
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+ std::optional<wm::EntitySnapshot>
+ CoreSegment::getLatestEntitySnapshotLocking(const MemoryID& entityID) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntitySnapshot(entityID);
+ }
+
+ std::optional<wm::EntityInstance>
+ CoreSegment::getLatestEntityInstanceLocking(const MemoryID& entityID, int instanceIndex) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntityInstance(entityID, instanceIndex);
+ }
+
+ armarx::aron::datanavigator::DictNavigatorPtr
+ CoreSegment::getLatestEntityInstanceDataLocking(const MemoryID& entityID, int instanceIndex) const
+ {
+ std::scoped_lock lock(_mutex);
+ return getLatestEntityInstanceData(entityID, instanceIndex);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index 7847bbb71cc6a1a269556aeec07738c396e42a68..86b9b439e72d4d686b2c52fe5fd39f0a9b6aaf1c 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -1,5 +1,8 @@
#pragma once
+#include <mutex>
+#include <optional>
+
#include "../base/CoreSegmentBase.h"
#include "ProviderSegment.h"
@@ -22,10 +25,10 @@ namespace armarx::armem::wm
using Base::CoreSegmentBase;
- CoreSegment(const CoreSegment& other) = default;
- CoreSegment(CoreSegment&& other) = default;
- CoreSegment& operator=(const CoreSegment& other) = default;
- CoreSegment& operator=(CoreSegment&& other) = default;
+ CoreSegment(const CoreSegment& other);
+ CoreSegment(CoreSegment&& other);
+ CoreSegment& operator=(const CoreSegment& other);
+ CoreSegment& operator=(CoreSegment&& other);
/**
* @brief Convert the content of this segmnet into a commit
@@ -33,10 +36,76 @@ namespace armarx::armem::wm
*/
Commit toCommit() const;
+ std::mutex& mutex() const;
+
+
+ // Non-locking interface
+
+ std::optional<wm::EntitySnapshot> getLatestEntitySnapshot(
+ const MemoryID& entityID) const;
+ std::optional<wm::EntityInstance> getLatestEntityInstance(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+ armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceData(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT> getLatestEntityInstanceDataAs(
+ const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ wm::EntityInstanceDataPtr data = getLatestEntityInstanceData(entityID, instanceIndex);
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ // Locking interface
+
+ std::optional<wm::EntitySnapshot> getLatestEntitySnapshotLocking(
+ const MemoryID& entityID) const;
+ std::optional<wm::EntityInstance> getLatestEntityInstanceLocking(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+ armarx::aron::datanavigator::DictNavigatorPtr getLatestEntityInstanceDataLocking(
+ const MemoryID& entityID, int instanceIndex = 0) const;
+
+
+ template <class AronDtoT>
+ std::optional<AronDtoT> getLatestEntityInstanceDataLockingAs(
+ const MemoryID& entityID, int instanceIndex = 0) const
+ {
+ // Keep lock to a minimum.
+ wm::EntityInstanceDataPtr data = nullptr;
+ {
+ std::scoped_lock lock(_mutex);
+ data = getLatestEntityInstanceData(entityID, instanceIndex);
+ }
+ if (data)
+ {
+ AronDtoT aron;
+ aron.fromAron(data);
+ return aron;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
protected:
virtual void _copySelfWithoutData(CoreSegment& other) const override;
+ mutable std::mutex _mutex;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
index 8e6eb1afe7e673baeab933397ebd16aa1781c18d..d3dc888d0304276bc6aaf1c677e8b33f4a1cae1a 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
@@ -8,6 +8,10 @@
namespace armarx::armem::wm
{
+ using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
+ using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
+
+
/**
* @brief Metadata of an entity instance.
*/
@@ -62,7 +66,7 @@ namespace armarx::armem::wm
return _metadata;
}
- inline armarx::aron::datanavigator::DictNavigatorPtr data() const
+ inline EntityInstanceDataPtr data() const
{
return _data;
}
@@ -97,7 +101,7 @@ namespace armarx::armem::wm
EntityInstanceMetadata _metadata;
/// The data. May be nullptr.
- armarx::aron::datanavigator::DictNavigatorPtr _data;
+ EntityInstanceDataPtr _data;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index 3d2efd2249f969308b126608d01533500f3bfa73..cd6c9cc58887cd5bd8833ffd06c92e30a0606ecb 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -1,8 +1,99 @@
#include "Memory.h"
+#include <map>
+#include <vector>
+
namespace armarx::armem::wm
{
+
+ std::vector<Memory::Base::UpdateResult>
+ Memory::update(const Commit& commit)
+ {
+ std::vector<Memory::Base::UpdateResult> result;
+ for (const EntityUpdate& update : commit.updates)
+ {
+ result.push_back(this->update(update));
+ }
+ return result;
+ }
+
+
+ std::vector<Memory::Base::UpdateResult>
+ Memory::updateLocking(const Commit& commit)
+ {
+ // Group updates by core segment, then update each core segment in a batch to only lock it once.
+ std::map<std::string, std::vector<const EntityUpdate*>> updatesPerCoreSegment;
+ for (const EntityUpdate& update : commit.updates)
+ {
+ updatesPerCoreSegment[update.entityID.coreSegmentName].push_back(&update);
+ }
+
+ std::vector<Memory::Base::UpdateResult> result;
+ // To throw an exception after the commit if a core segment is missing.
+ std::vector<std::string> missingCoreSegmentNames;
+ for (const auto& [coreSegmentName, updates] : updatesPerCoreSegment)
+ {
+ auto it = this->_container.find(coreSegmentName);
+ if (it != this->_container.end())
+ {
+ CoreSegment& coreSegment = it->second;
+
+ // Lock the core segment for the whole batch.
+ std::scoped_lock lock(coreSegment.mutex());
+
+ for (const EntityUpdate* update : updates)
+ {
+ auto r = coreSegment.update(*update);
+ Base::UpdateResult ret { r };
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ result.push_back(ret);
+ }
+ }
+ else
+ {
+ // Perform the other updates first, then throw afterwards.
+ missingCoreSegmentNames.push_back(coreSegmentName);
+ }
+ }
+ // Throw an exception if something went wrong.
+ if (not missingCoreSegmentNames.empty())
+ {
+ // Just throw an exception for the first entry. We can extend this exception in the future.
+ throw armem::error::MissingEntry::create<CoreSegment>(missingCoreSegmentNames.front(), *this);
+ }
+ return result;
+ }
+
+
+ Memory::Base::UpdateResult
+ Memory::update(const EntityUpdate& update)
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+
+ CoreSegment& segment = getCoreSegment(update.entityID.coreSegmentName);
+ Base::UpdateResult result = segment.update(update);
+ result.memoryUpdateType = UpdateType::UpdatedExisting;
+ return result;
+ }
+
+
+ Memory::Base::UpdateResult
+ Memory::updateLocking(const EntityUpdate& update)
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+
+ CoreSegment& segment = getCoreSegment(update.entityID.coreSegmentName);
+ Base::UpdateResult result;
+ {
+ std::scoped_lock lock(segment.mutex());
+ result = segment.update(update);
+ }
+ result.memoryUpdateType = UpdateType::UpdatedExisting;
+ return result;
+ }
+
+
Commit Memory::toCommit() const
{
Commit c;
@@ -13,6 +104,7 @@ namespace armarx::armem::wm
return c;
}
+
void Memory::_copySelfWithoutData(Memory& other) const
{
other.id() = _id;
@@ -21,4 +113,5 @@ namespace armarx::armem::wm
other.addCoreSegment(s.copyWithoutData());
}
}
+
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
index 3c64c0841b46a5c95f7f7365af26a1b20226f4dc..99a14dbdd291b0619733347cf2aec2ba365871f9 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -1,6 +1,6 @@
#pragma once
-#include "../base/MemoryBase.h"
+#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
#include "CoreSegment.h"
#include "detail/CopyWithoutData.h"
@@ -27,12 +27,30 @@ namespace armarx::armem::wm
Memory& operator=(const Memory& other) = default;
Memory& operator=(Memory&& other) = default;
+
+ virtual std::vector<Base::UpdateResult> update(const Commit& commit) override;
+ /**
+ * @brief Perform the commit, locking the core segments.
+ *
+ * Groups the commits by core segment, and updates each core segment
+ * in a batch, locking the core segment.
+ */
+ std::vector<Base::UpdateResult> updateLocking(const Commit& commit);
+
+ virtual Base::UpdateResult update(const EntityUpdate& update) override;
+ /**
+ * @brief Update the memory, locking the updated core segment.
+ */
+ Base::UpdateResult updateLocking(const EntityUpdate& update);
+
+
/**
* @brief Convert the content of this memory into a commit
* @return The resulting commit
*/
Commit toCommit() const;
+
protected:
virtual void _copySelfWithoutData(Memory& other) const override;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
index 1c1b46e138c7eb2d713ba15b76ba77a2588ea0ce..34390a1823a6f3f1651bbe59ec815a0c1ab48bd6 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -53,28 +53,28 @@ namespace armarx::armem::wm
bool visitEnter(const Memory& memory) override
{
- return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ return memoryConstFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
}
bool visitEnter(const CoreSegment& coreSegment) override
{
- return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ return coreSegmentConstFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
}
bool visitEnter(const ProviderSegment& providerSegment) override
{
- return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ return providerSegmentConstFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
}
bool visitEnter(const Entity& entity) override
{
- return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ return entityConstFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
}
bool visitEnter(const EntitySnapshot& snapshot) override
{
- return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ return snapshotConstFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
}
bool visit(const EntityInstance& instance) override
{
- return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ return instanceConstFn ? instanceConstFn(instance) : Visitor::visit(instance);
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index 7b3a1d05817c9c25675abe2e5b8ab011aba1d92d..ddb1a96a79fefe069c3edaae44c53a211fc97e30 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -1,5 +1,6 @@
#include "ComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -11,8 +12,7 @@
namespace armarx::armem::server::plugins
{
- ComponentPlugin::~ComponentPlugin()
- {}
+ ComponentPlugin::~ComponentPlugin() = default;
void ComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
@@ -118,6 +118,8 @@ namespace armarx::armem::server
addPlugin(plugin);
}
+ ComponentPluginUser::~ComponentPluginUser() = default;
+
// WRITING
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
@@ -128,7 +130,7 @@ namespace armarx::armem::server
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
return result;
}
@@ -136,7 +138,7 @@ namespace armarx::armem::server
data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
return iceMemory.commit(commitIce);
}
@@ -144,19 +146,20 @@ namespace armarx::armem::server
// READING
armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex);
+ // std::scoped_lock lock(workingMemoryMutex);
return iceMemory.query(input);
}
- // LTM LOADING
+
+ // LTM STORING
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(workingMemoryMutex, longtermMemoryMutex);
+ std::scoped_lock lock(/*workingMemoryMutex,*/ longtermMemoryMutex);
return iceMemory.store(input);
}
- // LTM STORING
+ // LTM LOADING
armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
{
std::scoped_lock lock(longtermMemoryMutex);
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index 5f9ef1d574df06b1644d733e494273ce0660dbbf..06ba31f7a128549f51be85de300a92bdd310cf95 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -2,7 +2,7 @@
#include <mutex>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
@@ -10,8 +10,8 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-
#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
+
#include "MemoryToIceAdapter.h"
@@ -73,7 +73,7 @@ namespace armarx::armem::server
public:
ComponentPluginUser();
- virtual ~ComponentPluginUser() override = default;
+ virtual ~ComponentPluginUser() override;
// WritingInterface interface
@@ -96,9 +96,11 @@ namespace armarx::armem::server
public:
+
/// The actual memory.
wm::Memory workingMemory;
- std::mutex workingMemoryMutex;
+ // [[deprecated ("The global working memory mutex is deprecated. Use the core segment mutexes instead.")]]
+ // std::mutex workingMemoryMutex;
bool longtermMemoryEnabled = true;
ltm::Memory longtermMemory;
@@ -112,6 +114,7 @@ namespace armarx::armem::server
private:
+
plugins::ComponentPlugin* plugin = nullptr;
};
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index 9ab34efd946541a37ae825d6338a355aec0dd722..9347cb7e9234723d33407693cb68ec1f5ea8448d 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -31,9 +31,10 @@ namespace armarx::armem::server
}
-
MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::CoreSegment& coreSegment) const
{
+ std::scoped_lock lock(coreSegment.mutex());
+
GroupBox group;
group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.providerSegments().size()));
@@ -50,7 +51,6 @@ namespace armarx::armem::server
}
-
MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::ProviderSegment& providerSegment) const
{
GroupBox group;
@@ -69,7 +69,6 @@ namespace armarx::armem::server
}
-
MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::Entity& entity) const
{
GroupBox group;
@@ -109,7 +108,6 @@ namespace armarx::armem::server
}
-
MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntitySnapshot& snapshot) const
{
GroupBox group;
@@ -129,6 +127,7 @@ namespace armarx::armem::server
return group;
}
+
MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntityInstance& instance) const
{
GroupBox group;
@@ -153,7 +152,6 @@ namespace armarx::armem::server
}
-
std::string MemoryRemoteGui::makeGroupLabel(
const std::string& term, const std::string& name, size_t size,
const std::string& namePrefix, const std::string& nameSuffix) const
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index a846cffac89c6118b6c476d3531e47cd49f96f8a..c40805a0f8a79413eb33418154a18194be6a22ba 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -55,6 +55,7 @@ namespace armarx::armem::server
if (input.providerSegmentName.size() > 0)
{
+ std::scoped_lock lock(coreSegment->mutex());
try
{
coreSegment->addProviderSegment(input.providerSegmentName);
@@ -94,6 +95,7 @@ namespace armarx::armem::server
return output;
}
+
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
@@ -140,9 +142,21 @@ namespace armarx::armem::server
armem::CommitResult
MemoryToIceAdapter::commit(const armem::Commit& commit)
+ {
+ return this->_commit(commit, false);
+ }
+
+
+ armem::CommitResult MemoryToIceAdapter::commitLocking(const armem::Commit& commit)
+ {
+ return this->_commit(commit, true);
+ }
+
+
+ armem::CommitResult MemoryToIceAdapter::_commit(const armem::Commit& commit, bool locking)
{
std::vector<data::MemoryID> updatedIDs;
- const bool publishUpdates = memoryListenerTopic;
+ const bool publishUpdates = bool(memoryListenerTopic);
CommitResult commitResult;
for (const EntityUpdate& update : commit.updates)
@@ -150,7 +164,9 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- auto updateResult = workingMemory->update(update);
+ auto updateResult = locking
+ ? workingMemory->updateLocking(update)
+ : workingMemory->update(update);
result.success = true;
result.snapshotID = updateResult.id;
@@ -200,29 +216,27 @@ namespace armarx::armem::server
{
ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::wm::query_proc::MemoryQueryProcessor wm_processor(
- input.withData ? armem::DataMode::WithData
- : armem::DataMode::NoData);
-
- armem::query::data::Result result;
- wm::Memory wm_result = wm_processor.process(input, *workingMemory, /* execute if: */ { query::data::QueryTarget::WM });
+ // Core segment processors will aquire the core segment locks.
+ armem::wm::query_proc::MemoryQueryProcessor wmProcessor(
+ input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
+ wm::Memory wmResult = wmProcessor.process(input, *workingMemory, /* execute if: */ { query::data::QueryTarget::WM });
- armem::ltm::query_proc::MemoryQueryProcessor ltm_processor;
- ltm::Memory ltm_result = ltm_processor.process(input, *longtermMemory, /* execute if: */ { query::data::QueryTarget::LTM });
+ armem::ltm::query_proc::MemoryQueryProcessor ltmProcessor;
+ ltm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory, /* execute if: */ { query::data::QueryTarget::LTM });
-
- if (ltm_result.hasData())
+ armem::query::data::Result result;
+ if (ltmResult.hasData())
{
// ATTENTION: This code block moves data from LTM back into WM.
// However, since some segments are constrained, the WM might send data back to LTM.
// This may also affect the data returned by the current query.
// However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
// remove the original memory reference from WM here.
- wm::Memory ltm_converted = ltm_result.convert();
- wm_result.append(ltm_converted);
+ wm::Memory ltmConverted = ltmResult.convert();
+ wmResult.append(ltmConverted);
// query again to limit output size (TODO: Skip if querytype is all)
- wm::Memory merged_result = wm_processor.process(input, wm_result);
+ wm::Memory merged_result = wmProcessor.process(input, wmResult);
result.memory = toIce<data::MemoryPtr>(merged_result);
// also move results of ltm to wm
@@ -233,7 +247,7 @@ namespace armarx::armem::server
}
else
{
- result.memory = toIce<data::MemoryPtr>(wm_result);
+ result.memory = toIce<data::MemoryPtr>(wmResult);
}
result.success = true;
@@ -245,6 +259,13 @@ namespace armarx::armem::server
return result;
}
+
+ client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
+ {
+ return client::QueryResult::fromIce(query(input.toIce()));
+ }
+
+
// LTM LOADING FROM LTM
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
@@ -255,6 +276,7 @@ namespace armarx::armem::server
return output;
}
+
// LTM STORING
data::StoreResult MemoryToIceAdapter::store(const armem::data::StoreInput& input)
{
@@ -282,9 +304,8 @@ namespace armarx::armem::server
return output;
}
- client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
- {
- return client::QueryResult::fromIce(query(input.toIce()));
- }
+
+
+
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 919c5fc8dc74ec3f742ac1c33365bc2c680ac768..f4b4b4c94eac3a51cb8c90e2fcabfc09288e3ead 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -39,6 +39,8 @@ namespace armarx::armem::server
data::CommitResult commit(const data::Commit& commitIce, Time timeArrived);
data::CommitResult commit(const data::Commit& commitIce);
armem::CommitResult commit(const armem::Commit& commit);
+ armem::CommitResult commitLocking(const armem::Commit& commit);
+
// READING
query::data::Result query(const armem::query::data::Input& input);
@@ -57,6 +59,11 @@ namespace armarx::armem::server
client::MemoryListenerInterfacePrx memoryListenerTopic;
+
+ private:
+
+ armem::CommitResult _commit(const armem::Commit& commit, bool locking);
+
};
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
index ce437fd67bdd2ba3aa6ac9943093525db99a581e..34e2abd99c889d65723065c8b4726b2ea0aa9bd1 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
@@ -1 +1,2 @@
#include "BaseQueryProcessorBase.h"
+
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index f71aede29731259471cf5e8dd660917b3e848bb1..95943dd2f18289360336c22b910913835a84c604 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -19,6 +19,9 @@ namespace armarx::armem::base::query_proc
public:
+ virtual ~BaseQueryProcessorBase() = default;
+
+
DataT process(const QueryT& query, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
{
DataT result = data.copyEmpty();
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index f8ab8c113e510e57a1ab2b53af6a06e60e393ce1..9d7b2518227e4ddf0c5d759963af193656e39894 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -1,17 +1,15 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
-
-#include "BaseQueryProcessorBase.h"
-#include "ProviderSegmentQueryProcessorBase.h"
-
#include <regex>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include "BaseQueryProcessorBase.h"
+#include "ProviderSegmentQueryProcessorBase.h"
namespace armarx::armem::base::query_proc
@@ -33,9 +31,9 @@ namespace armarx::armem::base::query_proc
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using Base::process;
- void process(_CoreSegmentT& result,
- const armem::query::data::CoreSegmentQuery& query,
- const _CoreSegmentT& coreSegment) const override
+ virtual void process(_CoreSegmentT& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const _CoreSegmentT& coreSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
index 4d1b9e14e21804f1e44f1a2fd6456da37a113c03..c57c2c2da39ca53ce97d27b498e69b648a16f25a 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/EntityQueryProcessor.h
@@ -19,11 +19,14 @@ namespace armarx::armem::d_ltm::query_proc
using Base = BaseQueryProcessor<d_ltm::Entity, armem::query::data::EntityQuery>;
public:
+
EntityQueryProcessor() :
Base()
{}
+
private:
+
void addResultSnapshot(d_ltm::Entity& result, d_ltm::Entity::ContainerT::const_iterator it) const override
{
addResultSnapshot(result, it->second);
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
index 99811dcc72d168dfecb0c5864f0760c4d5634a6e..933f9a6fd628b8c2747c2c6449499e596593935b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
@@ -1,11 +1,9 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-
#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/DataMode.h>
-#include "../base/BaseQueryProcessorBase.h"
+#include <RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h>
namespace armarx::armem::wm::query_proc
@@ -20,11 +18,15 @@ namespace armarx::armem::wm::query_proc
using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
public:
+
BaseQueryProcessor(DataMode dataMode = DataMode::WithData) :
dataMode(dataMode)
{}
+
protected:
+
DataMode dataMode;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
index afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a..bda9ad04de5ac7cd46be68a2f5f7ca2ec1419040 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
@@ -1 +1,39 @@
#include "CoreSegmentQueryProcessor.h"
+
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+
+
+namespace armarx::armem::wm::query_proc
+{
+
+ CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(DataMode dataMode) :
+ Base(dataMode), providerSegmentProcessor(dataMode)
+ {}
+
+
+ CoreSegmentQueryProcessor::~CoreSegmentQueryProcessor() = default;
+
+
+ void CoreSegmentQueryProcessor::process(
+ CoreSegment& result, const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
+ {
+ std::scoped_lock lock(coreSegment.mutex());
+ CoreSegmentQueryProcessorBase::process(result, query, coreSegment);
+ }
+
+
+ data::CoreSegment CoreSegmentQueryProcessor::processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const
+ {
+ data::CoreSegment data;
+ toIce(data, process(query, coreSegment));
+ return data;
+ }
+
+
+ ProviderSegment CoreSegmentQueryProcessor::providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegment& s) const
+ {
+ return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
index 390ab66f656ece6010457f12f09917d9fca413f6..f9e1d59c9f75866e3817440bb163ec5789c0f359 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
@@ -1,8 +1,11 @@
#pragma once
-#include "BaseQueryProcessor.h"
-#include "../base/CoreSegmentQueryProcessorBase.h"
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h>
+#include "BaseQueryProcessor.h"
#include "ProviderSegmentQueryProcessor.h"
@@ -18,23 +21,28 @@ namespace armarx::armem::wm::query_proc
using Base = BaseQueryProcessor<wm::CoreSegment, armem::query::data::CoreSegmentQuery>;
public:
- CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), providerSegmentProcessor(dataMode)
- {}
+
+ CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+ virtual ~CoreSegmentQueryProcessor() override;
using Base::process;
- data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const
- {
- return toIce<data::CoreSegment>(process(query, coreSegment));
- }
+
+ /// Locks the core segment, then delegates back to `CoreSegmentQueryProcessorBase`.
+ void process(CoreSegment& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegment& coreSegment) const override;
+
+ data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const;
+
protected:
- virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
- {
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
+
+ virtual ProviderSegment providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override;
+
private:
+
ProviderSegmentQueryProcessor providerSegmentProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
index c1c321b026b173c6552758fb9d8b9fdf722ea5a4..7ef31f2badfbffbf49f132c855b2143b8ddc10bd 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
@@ -1 +1,43 @@
#include "EntityQueryProcessor.h"
+
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+
+
+namespace armarx::armem::wm::query_proc
+{
+ EntityQueryProcessor::EntityQueryProcessor(DataMode dataMode) :
+ Base(dataMode)
+ {}
+
+
+ EntityQueryProcessor::~EntityQueryProcessor() = default;
+
+
+ void EntityQueryProcessor::addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const
+ {
+ bool withData = (dataMode == DataMode::WithData);
+ if (withData)
+ {
+ result.addSnapshot(snapshot.copy());
+ }
+ else
+ {
+ result.addSnapshot(snapshot.copyWithoutData());
+ }
+ }
+
+
+ data::Entity EntityQueryProcessor::processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const
+ {
+ data::Entity data;
+ toIce(data, process(query, entity));
+ return data;
+ }
+
+
+ void EntityQueryProcessor::addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const
+ {
+ addResultSnapshot(result, it->second);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
index 94dc905e489be501257be508cff03c7efee8f8b8..dde53a8ff3393dde385ccc49aa937fcf196476d0 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
@@ -1,9 +1,10 @@
#pragma once
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h>
+
#include "BaseQueryProcessor.h"
-#include "../base/EntityQueryProcessorBase.h"
-#include "EntityQueryProcessor.h"
namespace armarx::armem::wm::query_proc
{
@@ -17,33 +18,17 @@ namespace armarx::armem::wm::query_proc
using Base = BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>;
public:
- EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode)
- {}
- data::Entity processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const
- {
- return toIce<data::Entity>(process(query, entity));
- }
+ EntityQueryProcessor(DataMode dataMode = DataMode::WithData);
+ virtual ~EntityQueryProcessor() override;
+
+ data::Entity processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const;
+
private:
- void addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const override
- {
- addResultSnapshot(result, it->second);
- }
-
- void addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const override
- {
- bool withData = (dataMode == DataMode::WithData);
- if (withData)
- {
- result.addSnapshot(snapshot.copy());
- }
- else
- {
- result.addSnapshot(snapshot.copyWithoutData());
- }
- }
+
+ void addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const override;
+ void addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const override;
};
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
index 69b04de6c9e623286a5bda836dddfdc8b551b64a..dd249b61caaacaca11e369b19b8f4e0da1dc5cf5 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
@@ -1 +1,22 @@
#include "MemoryQueryProcessor.h"
+
+
+namespace armarx::armem::wm::query_proc
+{
+
+ MemoryQueryProcessor::MemoryQueryProcessor(DataMode dataMode) :
+ Base(dataMode), coreSegmentProcessor(dataMode)
+ {}
+
+
+ MemoryQueryProcessor::~MemoryQueryProcessor()
+ {
+ }
+
+
+ CoreSegment MemoryQueryProcessor::coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegment& s) const
+ {
+ return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
index 8284df202bbcf602e2433639fe85388ca13d75ff..c57eea94a464a2e2191e614b5f1d6095d716405a 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
@@ -1,10 +1,13 @@
#pragma once
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h>
+
#include "BaseQueryProcessor.h"
-#include "../base/MemoryQueryProcessorBase.h"
#include "CoreSegmentQueryProcessor.h"
+
namespace armarx::armem::wm::query_proc
{
/**
@@ -17,18 +20,20 @@ namespace armarx::armem::wm::query_proc
using Base = BaseQueryProcessor<wm::Memory, armem::query::data::MemoryQuery>;
public:
- MemoryQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), coreSegmentProcessor(dataMode)
- {}
+
+ MemoryQueryProcessor(DataMode dataMode = DataMode::WithData);
+ virtual ~MemoryQueryProcessor() override;
+
protected:
- virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
- {
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
+
+ virtual CoreSegment coreSegmentProcessorProcess(
+ const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegment& s) const override;
private:
+
CoreSegmentQueryProcessor coreSegmentProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
index 9a2a4405001f0904b74fc6afcf96813eef0879cd..a5e23184fa999dc44d72de7588cf77fad0503729 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
@@ -1 +1,30 @@
#include "ProviderSegmentQueryProcessor.h"
+
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+
+
+namespace armarx::armem::wm::query_proc
+{
+
+ ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(DataMode dataMode) :
+ Base(dataMode), entityProcessor(dataMode)
+ {}
+
+
+ ProviderSegmentQueryProcessor::~ProviderSegmentQueryProcessor() = default;
+
+
+ data::ProviderSegment ProviderSegmentQueryProcessor::processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const
+ {
+ data::ProviderSegment data;
+ toIce(data, process(query, providerSegment));
+ return data;
+ }
+
+
+ Entity ProviderSegmentQueryProcessor::entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const Entity& s) const
+ {
+ return entityProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
index 468138d9e2bbd526db3632019beca6a5a29f8269..8e4f5a5e85d6c3c7f58433a0c2ddcd50074c7d7e 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
@@ -1,10 +1,12 @@
#pragma once
-#include "BaseQueryProcessor.h"
-#include "../base/ProviderSegmentQueryProcessorBase.h"
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h>
+#include "BaseQueryProcessor.h"
#include "EntityQueryProcessor.h"
+
namespace armarx::armem::wm::query_proc
{
/**
@@ -17,23 +19,22 @@ namespace armarx::armem::wm::query_proc
using Base = BaseQueryProcessor<wm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
public:
- ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- Base(dataMode), entityProcessor(dataMode)
- {}
+
+ ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+ virtual ~ProviderSegmentQueryProcessor() override;
+
using Base::process;
- data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const
- {
- return toIce<data::ProviderSegment>(process(query, providerSegment));
- }
+ data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const;
+
protected:
- virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
- {
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
- }
+
+ virtual Entity entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const Entity& s) const override;
private:
+
EntityQueryProcessor entityProcessor;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8184e27c985877a19aaa873db07883685140b17b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
@@ -0,0 +1,79 @@
+#include "SpecializedSegment.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+
+namespace armarx::armem::server::segment
+{
+
+ SpecializedSegment::SpecializedSegment(
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType,
+ const std::string& defaultCoreSegmentName,
+ int64_t defaultMaxHistorySize
+ ) :
+ iceMemory(iceMemory), aronType(aronType)
+ {
+ setDefaultCoreSegmentName(defaultCoreSegmentName);
+ setDefaultMaxHistorySize(defaultMaxHistorySize);
+ }
+
+
+ SpecializedSegment::~SpecializedSegment()
+ {
+ }
+
+
+ void SpecializedSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(properties.coreSegmentName,
+ prefix + "seg.CoreSegmentName",
+ "Name of the " + properties.coreSegmentName + " core segment.");
+
+ defs->optional(properties.maxHistorySize,
+ prefix + "seg.MaxHistorySize",
+ "Maximal size of the " + properties.coreSegmentName + " entity histories (-1 for infinite).");
+ }
+
+
+ void SpecializedSegment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+ Logging::setTag(properties.coreSegmentName + " Core Segment");
+
+ ARMARX_INFO << "Adding core segment '" << properties.coreSegmentName << "'";
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(properties.coreSegmentName, aronType);
+ coreSegment->setMaxHistorySize(properties.maxHistorySize);
+ }
+
+
+ std::mutex& SpecializedSegment::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return coreSegment->mutex();
+ }
+
+
+ void SpecializedSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName)
+ {
+ this->properties.coreSegmentName = coreSegmentName;
+ }
+
+
+ void SpecializedSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
+ {
+ this->properties.maxHistorySize = maxHistorySize;
+ }
+
+
+ void SpecializedSegment::setAronType(aron::typenavigator::ObjectNavigatorPtr aronType)
+ {
+ this->aronType = aronType;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd3a456c55d785471e86a0d3f1e12a5491d3e32c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
@@ -0,0 +1,77 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+namespace armarx::aron::typenavigator
+{
+ using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
+}
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+}
+
+namespace armarx::armem::server::segment
+{
+
+ /**
+ * @brief Specialized management of a core segment.
+ */
+ class SpecializedSegment :
+ public armarx::Logging
+ {
+ public:
+
+ SpecializedSegment(
+ server::MemoryToIceAdapter& iceMemory,
+ aron::typenavigator::ObjectNavigatorPtr aronType = nullptr,
+ const std::string& defaultCoreSegmentName = "",
+ int64_t defaultMaxHistorySize = -1);
+
+ virtual ~SpecializedSegment();
+
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ virtual void init();
+ // void connect();
+
+ std::mutex& mutex() const;
+
+
+ protected:
+
+ void setDefaultCoreSegmentName(const std::string& coreSegmentName);
+ void setDefaultMaxHistorySize(int64_t maxHistorySize);
+ void setAronType(aron::typenavigator::ObjectNavigatorPtr aronType);
+
+
+ protected:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ aron::typenavigator::ObjectNavigatorPtr aronType;
+
+ struct Properties
+ {
+ std::string coreSegmentName = "";
+ int64_t maxHistorySize = -1;
+ };
+ Properties properties;
+
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 54eb919e1804ce099efe5ade4ea14294a202e0bb..ab26b1f8a38b5e18b16dc49a498c9369d6d918db 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -16,6 +16,7 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
RobotAPI::armem_robot
+
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -32,11 +33,6 @@ armarx_add_library(
server/instance/Visu.h
server/instance/ArticulatedObjectVisu.h
- # server/articulated_object_class/Segment.h
- # server/articulated_object_instance/Segment.h
- # server/articulated_object/SegmentAdapter.h
- # server/articulated_object_instance/Visu.h
-
server/attachments/Segment.h
client/articulated_object/Reader.h
@@ -64,12 +60,6 @@ armarx_add_library(
server/instance/Visu.cpp
server/instance/ArticulatedObjectVisu.cpp
- server/articulated_object_class/Segment.cpp
-
- # server/articulated_object_instance/Segment.cpp
- # server/articulated_object/SegmentAdapter.cpp
- # server/articulated_object_instance/Visu.cpp
-
server/attachments/Segment.cpp
client/articulated_object/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
deleted file mode 100644
index 425b5af02da13e58f4a654a0784029f5fbe9a9e0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ /dev/null
@@ -1,185 +0,0 @@
-#include "Segment.h"
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <sstream>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
-
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
-
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("ArticulatedObjectInstanceSegment");
- }
-
- Segment::~Segment() = default;
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
-
- defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
- defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
- "If true, load the objects from the objects package on startup.");
-
- }
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
- if (p.loadFromObjectsPackage)
- {
- loadByObjectFinder(p.objectsPackage);
- }
- }
-
- void Segment::connect(viz::Client arviz)
- {
- // this->visu = std::make_unique<Visu>(arviz, *this);
- }
-
- void Segment::loadByObjectFinder(const std::string& package)
- {
- ObjectFinder finder(package);
-
- const auto knownArticulatedObjectDescriptions = finder.findAllArticulatedObjectsByDataset(true);
- ARMARX_DEBUG << "Found " << knownArticulatedObjectDescriptions.size() << " articulated objects";
-
- loadObjectsIntoMemory(knownArticulatedObjectDescriptions, package);
- }
-
- void Segment::loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package)
- {
- const Time now = TimeUtil::GetTime();
-
- const MemoryID providerID = coreSegment->id().withProviderSegmentName(package);
- coreSegment->addProviderSegment(providerID.providerSegmentName);
-
- // ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
- // << objectFinder.getPackageName() << "' ...";
- Commit commit;
-
- for (const auto& [datasetName, descriptions] : datasets)
- {
-
- for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
- {
- EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName(desc.name);
- update.timeArrived = update.timeCreated = update.timeSent = now;
-
- arondto::RobotDescription aronRobotDescription;
- toAron(aronRobotDescription, desc);
- // TODO toAron(aronRobotDescription.timestamp, now);
-
- update.instancesData = { aronRobotDescription.toAron()};
- }
-
- ARMARX_INFO << "Loaded " << commit.updates.size() << " articulated object classes from '"
- << package << "' in dataset '" << datasetName << "'.";
- }
- iceMemory.commit(commit);
- }
-
-
-
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- {
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- {
- for (const auto& [name, entity] : provSeg.entities())
- {
- for (const auto& snapshot : simox::alg::get_values(entity.history()))
- {
- const auto& entityInstance = snapshot.getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
-
- if (not description)
- {
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- continue;
- }
-
- ARMARX_DEBUG << "Key is " << armem::MemoryID(snapshot.id());
-
- objects.emplace(armem::MemoryID(snapshot.id()), *description);
- }
- }
- }
-
- ARMARX_INFO << deactivateSpam(10) << "Number of known articulated object classes: " << objects.size();
-
- return objects;
- }
-
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
-} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
deleted file mode 100644
index e2a7bbc6fbf2c875e11fb25427f5a7a8809aee4c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * 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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <string>
-#include <optional>
-#include <mutex>
-#include <unordered_map>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
- class Visu;
-
- class Segment : public armarx::Logging
- {
- public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
-
- virtual ~Segment();
-
- void connect(viz::Client arviz);
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
-
- std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> getKnownObjectClasses() const;
-
-
- private:
- void loadByObjectFinder(const std::string& package);
- void loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package);
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
- struct Properties
- {
- std::string coreClassSegmentName = "ArticulatedObjectClass";
- int64_t maxHistorySize = -1;
-
- std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
- bool loadFromObjectsPackage = true;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
- };
-
-} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
deleted file mode 100644
index dba8c91a83e3449fd0dc2a4bdb8ef79632a572e0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ /dev/null
@@ -1,175 +0,0 @@
-#include "Segment.h"
-
-#include <sstream>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-
-#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
-
-#include "Visu.h"
-
-
-namespace armarx::armem::server::obj::articulated_object_instance
-{
-
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("ArticulatedObjectInstanceSegment");
- }
-
- Segment::~Segment() = default;
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix)
- {
- defs->optional(p.coreInstanceSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- }
-
- void Segment::init()
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
- }
-
- void Segment::connect(viz::Client arviz)
- {
- this->visu = std::make_unique<Visu>(arviz, *this);
- visu->init();
- }
-
- void Segment::setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment)
- {
- classSegment = &segment;
- }
-
- ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects() const
- {
- ARMARX_CHECK_NOT_NULL(classSegment);
- const auto knownObjectClasses = classSegment->getKnownObjectClasses();
-
- ARMARX_DEBUG << "Class segment has " << knownObjectClasses.size() << " known articulated objects";
-
- const auto escape = [](std::string & v)
- {
- v = simox::alg::replace_all(v, "/", "\\/");
- };
-
- const auto resolveDescriptionLink = [&](auto & articulatedObject, const auto & aronDescriptionLink) -> bool
- {
- armem::MemoryID descriptionLink;
- fromAron(aronDescriptionLink, descriptionLink);
-
- escape(descriptionLink.providerSegmentName);
-
- ARMARX_DEBUG << "Lookup key is " << descriptionLink;
-
- // const auto keys = simox::alg::get_keys(knownObjectClasses);
- // ARMARX_DEBUG << "Known keys " << keys;
-
- const auto it = knownObjectClasses.find(descriptionLink);
- if (it == knownObjectClasses.end())
- {
- ARMARX_WARNING << "Unknown object class " << descriptionLink
- << "Known classes are " << simox::alg::get_keys(knownObjectClasses);
- return false;
- }
-
- articulatedObject.description = it->second;
- return true;
- };
-
- ::armarx::armem::articulated_object::ArticulatedObjects objects;
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreInstanceSegmentName))
- {
- for (const auto& [_, entity] : provSeg.entities())
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- arondto::Robot aronArticulatedObject;
- aronArticulatedObject.fromAron(entityInstance.data());
-
- armem::articulated_object::ArticulatedObject articulatedObject;
- fromAron(aronArticulatedObject, articulatedObject);
-
- // resolve memory link for description
- const arondto::MemoryID& aronDescriptionLink = aronArticulatedObject.description;
- if (not resolveDescriptionLink(articulatedObject, aronDescriptionLink))
- {
- continue;
- }
-
- objects.push_back(articulatedObject);
- }
- }
-
- return objects;
- }
-
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
deleted file mode 100644
index 2d50be817effddb795e8cd6adf1bda992d177e87..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * 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/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <cstdint>
-#include <map>
-#include <string>
-#include <optional>
-#include <mutex>
-
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-
-namespace armarx::armem::server::obj::articulated_object_class
-{
- class Segment;
-}
-
-namespace armarx::armem::server::obj::articulated_object_instance
-{
- class Visu;
-
- class Segment : public armarx::Logging
- {
- public:
-
- struct CommitStats
- {
- int numUpdated = 0;
- };
-
-
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
-
- virtual ~Segment();
-
- void connect(viz::Client arviz);
-
- void defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix = "");
-
- void init();
-
- void setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment);
-
- ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects() const;
-
- private:
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
- articulated_object_class::Segment const* classSegment;
-
-
- struct Properties
- {
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
- };
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index f2490c841d0042a133fb1c8cf58740eb2af614cd..bb5d930da65b6ddb0e7966aa53e5e8fa766d383d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -4,6 +4,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include "RobotAPI/libraries/armem/util/util.h"
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
@@ -23,9 +24,8 @@
namespace armarx::armem::server::obj::attachments
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ iceMemory(memoryToIceAdapter)
{
Logging::setTag("Attachments");
}
@@ -34,7 +34,7 @@ namespace armarx::armem::server::obj::attachments
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
}
@@ -42,20 +42,22 @@ namespace armarx::armem::server::obj::attachments
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::Robot::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
- void Segment::connect(viz::Client arviz)
+ void Segment::connect()
{
- // this->visu = std::make_unique<Visu>(arviz, *this);
}
std::vector<armarx::armem::attachment::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
{
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ std::scoped_lock(coreSegment->mutex());
+
std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ for (const auto& [_, provSeg] : *coreSegment)
{
for (const auto& [name, entity] : provSeg.entities())
{
@@ -80,47 +82,4 @@ namespace armarx::armem::server::obj::attachments
return attachments;
}
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
index 3817bc22a51227422848ebbe1e4296aa570fee24..0b9302d93390c473b87c67066bfe173237f3774b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -27,7 +27,7 @@
#include <unordered_map>
#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
@@ -37,6 +37,7 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem_objects/types.h"
+
namespace armarx::armem
{
namespace server
@@ -59,16 +60,14 @@ namespace armarx::armem::server::obj::attachments
class Segment : public armarx::Logging
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
- void connect(viz::Client arviz);
-
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
void init();
+ void connect();
std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
@@ -77,31 +76,14 @@ namespace armarx::armem::server::obj::attachments
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
struct Properties
{
- std::string coreClassSegmentName = "Attachments";
+ std::string coreSegmentName = "Attachments";
int64_t maxHistorySize = -1;
};
Properties p;
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
};
} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 3e6ecfed37e3d75d60b3d509f97520c7627f0fcc..22608257c1abcf39340643870b704c2dadfcd54d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -3,9 +3,11 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <SimoxUtility/color/Color.h>
@@ -19,17 +21,21 @@
namespace armarx::armem::server::obj::clazz
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter,
+ arondto::ObjectClass::toAronType(), "Class", -1)
{
- Logging::setTag("ClassSegment");
}
+
+ Segment::~Segment()
+ {
+ }
+
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object clazz core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ SpecializedSegment::defineProperties(defs, prefix);
defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
@@ -38,12 +44,10 @@ namespace armarx::armem::server::obj::clazz
floorVis.defineProperties(defs, prefix + "Floor.");
}
+
void Segment::init()
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
+ SpecializedSegment::init();
if (p.loadFromObjectsPackage)
{
@@ -51,6 +55,7 @@ namespace armarx::armem::server::obj::clazz
}
}
+
void Segment::connect(viz::Client arviz)
{
this->arviz = arviz;
@@ -65,12 +70,14 @@ namespace armarx::armem::server::obj::clazz
loadByObjectFinder(ObjectFinder(objectsPackage));
}
+
void Segment::loadByObjectFinder(const ObjectFinder& finder)
{
this->objectFinder = finder;
loadByObjectFinder();
}
+
void Segment::loadByObjectFinder()
{
const Time now = TimeUtil::GetTime();
@@ -79,11 +86,6 @@ namespace armarx::armem::server::obj::clazz
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
- if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerID.providerSegmentName);
- }
-
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
Commit commit;
@@ -91,7 +93,7 @@ namespace armarx::armem::server::obj::clazz
{
info.setLogError(false);
- EntityUpdate& update = commit.updates.emplace_back();
+ EntityUpdate& update = commit.add();
update.entityID = providerID.withEntityName(info.id().str());
update.timeArrived = update.timeCreated = update.timeSent = now;
@@ -103,9 +105,10 @@ namespace armarx::armem::server::obj::clazz
}
ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
<< objectFinder.getPackageName() << "'.";
- iceMemory.commit(commit);
+ iceMemory.commitLocking(commit);
}
+
void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
{
const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
@@ -121,31 +124,32 @@ namespace armarx::armem::server::obj::clazz
{
try
{
- const armem::wm::Entity& entity = coreSegment->getEntity(entityID);
- const armem::wm::EntityInstance& instance = entity.getLatestSnapshot().getInstance(0);
-
- arondto::ObjectClass aron;
- aron.fromAron(instance.data());
+ std::optional<arondto::ObjectClass> aron =
+ coreSegment->getLatestEntityInstanceDataLockingAs<arondto::ObjectClass>(entityID, 0);
+ if (not aron.has_value())
+ {
+ return;
+ }
- if (!aron.simoxXmlPath.package.empty())
+ if (not aron->simoxXmlPath.package.empty())
{
layerObject.add(viz::Object(entityID.str())
- .file(aron.simoxXmlPath.package, aron.simoxXmlPath.path)
+ .file(aron->simoxXmlPath.package, aron->simoxXmlPath.path)
.pose(pose));
}
if (showAABB)
{
layerAABB.add(viz::Box("AABB")
- .pose(pose * simox::math::pose(aron.aabb.center))
- .size(aron.aabb.extents)
+ .pose(pose * simox::math::pose(aron->aabb.center))
+ .size(aron->aabb.extents)
.color(simox::Color::cyan(255, 64)));
}
if (showOOBB)
{
layerOOBB.add(viz::Box("OOBB")
- .pose(pose * simox::math::pose(aron.oobb.center, aron.oobb.orientation))
- .size(aron.oobb.extents)
+ .pose(pose * simox::math::pose(aron->oobb.center, aron->oobb.orientation))
+ .size(aron->oobb.extents)
.color(simox::Color::lime(255, 64)));
}
}
@@ -164,17 +168,16 @@ namespace armarx::armem::server::obj::clazz
arviz.commit(layerObject, layerOrigin, layerAABB, layerOOBB);
}
+
arondto::ObjectClass Segment::objectClassFromInfo(const ObjectInfo& info)
{
namespace fs = std::filesystem;
arondto::ObjectClass data;
-
toAron(data.id, info.id());
- auto setPathIfExists = [](
- armarx::arondto::PackagePath & aron,
- const PackageFileLocation & location)
+ auto setPathIfExists = [](armarx::arondto::PackagePath & aron,
+ const PackageFileLocation & location)
{
if (fs::is_regular_file(location.absolutePath))
{
@@ -223,21 +226,23 @@ namespace armarx::armem::server::obj::clazz
group.addChildren({layout, VSpacer()});
}
+
void Segment::RemoteGui::update(Segment& segment)
{
data.update(segment);
visu.update(segment);
}
+
void Segment::RemoteGui::Data::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
reloadButton.setLabel("Reload");
- maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
+ maxHistorySize.setValue(std::max(1, int(segment.properties.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(segment.p.maxHistorySize == -1);
+ infiniteHistory.setValue(segment.properties.maxHistorySize == -1);
GridLayout grid;
int row = 0;
@@ -253,27 +258,27 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
+
void Segment::RemoteGui::Data::update(Segment& segment)
{
if (reloadButton.wasClicked())
{
- std::scoped_lock lock(segment.memoryMutex);
+ // Core segment mutex will be locked on commit.
segment.loadByObjectFinder();
rebuild = true;
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- std::scoped_lock lock(segment.memoryMutex);
- segment.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ std::scoped_lock lock(segment.mutex());
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
if (segment.coreSegment)
{
- segment.coreSegment->setMaxHistorySize(long(segment.p.maxHistorySize));
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
}
}
}
-
void Segment::RemoteGui::Visu::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
@@ -308,6 +313,7 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
+
void Segment::RemoteGui::Visu::update(Segment& segment)
{
if (showButton.wasClicked())
@@ -315,7 +321,6 @@ namespace armarx::armem::server::obj::clazz
const size_t index = static_cast<size_t>(showComboBox.getIndex());
if (/*index >= 0 &&*/ index < showOptionsIndex.size())
{
- std::scoped_lock lock(segment.memoryMutex);
segment.visualizeClass(showOptionsIndex.at(index));
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 88eaedbefb6529b873dfe6033e6a2857672c0586..15c7c4f373f20c43c0febfb142675c5058c45909 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -3,9 +3,6 @@
#include <mutex>
#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -13,26 +10,30 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/server/class/FloorVis.h>
+namespace armarx::armem::arondto
+{
+ class ObjectClass;
+}
namespace armarx::armem::server::obj::clazz
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
- Segment(armem::server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(armem::server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
- void init();
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void init() override;
void connect(viz::Client arviz);
+
void loadByObjectFinder(const std::string& objectsPackage);
void loadByObjectFinder(const ObjectFinder& finder);
void loadByObjectFinder();
@@ -45,10 +46,6 @@ namespace armarx::armem::server::obj::clazz
private:
- armem::server::MemoryToIceAdapter& iceMemory;
- armem::wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
ObjectFinder objectFinder;
viz::Client arviz;
@@ -57,9 +54,6 @@ namespace armarx::armem::server::obj::clazz
struct Properties
{
- std::string coreSegmentName = "Class";
- long maxHistorySize = -1;
-
std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
bool loadFromObjectsPackage = true;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index db706075c9bd5431fefd2b04b7634cf5e8ff6747..09a412f4bfa40a1a6288e86dfe65fa8b50953061 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,6 +1,7 @@
#include "Segment.h"
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
@@ -10,6 +11,7 @@
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -42,12 +44,10 @@
namespace armarx::armem::server::obj::instance
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter,
+ arondto::ObjectInstance::toAronType(), "Instance", 64)
{
- Logging::setTag("InstanceSegment");
-
oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
{
// Try to get OOBB from repository.
@@ -79,10 +79,15 @@ namespace armarx::armem::server::obj::instance
}
+ Segment::~Segment()
+ {
+ }
+
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ SpecializedSegment::defineProperties(defs, prefix);
+
defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
"If true, no new snapshots are stored while an object is attached to a robot node.\n"
"If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
@@ -102,11 +107,7 @@ namespace armarx::armem::server::obj::instance
void Segment::init()
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
-
+ SpecializedSegment::init();
if (not p.sceneSnapshotToLoad.empty())
{
@@ -115,14 +116,17 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::connect(viz::Client arviz)
{
+ (void) arviz;
// ARMARX_INFO << "ArticulatedObjectVisu";
// this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
// visu->init();
}
+
Segment::CommitStats Segment::commitObjectPoses(
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
@@ -135,7 +139,7 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated = 0;
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
- const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+ const Time timestamp = Time::microSeconds(provided.timestampMicroSeconds);
// Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
@@ -208,9 +212,9 @@ namespace armarx::armem::server::obj::instance
void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
- ARMARX_CHECK_NOT_NULL(coreSegment);
Time now = TimeUtil::GetTime();
+ ARMARX_CHECK_NOT_NULL(coreSegment);
const MemoryID coreSegmentID = coreSegment->id();
Commit commit;
@@ -241,6 +245,7 @@ namespace armarx::armem::server::obj::instance
update.instancesData.push_back(dto.toAron());
}
+ // Commit non-locking.
iceMemory.commit(commit);
}
@@ -668,7 +673,6 @@ namespace armarx::armem::server::obj::instance
// Store non-attached pose in new snapshot.
storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
}
-
if (providerName.empty())
{
providerName = data.pose.providerName;
@@ -973,7 +977,6 @@ namespace armarx::armem::server::obj::instance
if (lockMemory)
{
- std::scoped_lock lock(memoryMutex);
commitSceneSnapshot(snapshot.value(), filename.string());
}
else
@@ -989,9 +992,9 @@ namespace armarx::armem::server::obj::instance
{
using namespace armarx::RemoteGui::Client;
- maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ maxHistorySize.setValue(std::max(1, int(data.properties.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ infiniteHistory.setValue(data.properties.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
storeLoadLine.setValue("Scene_" + timestampPlaceholder);
@@ -1026,39 +1029,39 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::RemoteGui::update(Segment& data)
+ void Segment::RemoteGui::update(Segment& segment)
{
if (loadButton.wasClicked())
{
bool lockMemory = true;
- data.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
+ segment.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
}
if (storeButton.wasClicked())
{
armem::obj::SceneSnapshot scene;
{
- std::scoped_lock lock(data.memoryMutex);
- scene = data.getSceneSnapshot();
+ std::scoped_lock lock(segment.mutex());
+ scene = segment.getSceneSnapshot();
}
- data.storeScene(storeLoadLine.getValue(), scene);
+ segment.storeScene(storeLoadLine.getValue(), scene);
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
- std::scoped_lock lock(data.memoryMutex);
+ std::scoped_lock lock(segment.mutex());
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (data.coreSegment)
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
{
- data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
}
}
- data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 3ec6a2f22f41569945cc0c0ed3f5fd0a4021241e..918a4efade3c596763d90fec427b18fa78a774bf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -10,32 +10,35 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/components/ArViz/Client/Client.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
-#include "Decay.h"
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include "ArticulatedObjectVisu.h"
+#include "Decay.h"
namespace armarx::armem::obj
{
struct SceneSnapshot;
}
+namespace armarx::armem::arondto
+{
+ class ObjectInstance;
+}
namespace armarx::armem::server::obj::instance
{
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
@@ -50,14 +53,12 @@ namespace armarx::armem::server::obj::instance
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
-
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void init() override;
void connect(viz::Client arviz);
@@ -179,16 +180,8 @@ namespace armarx::armem::server::obj::instance
private:
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
-
struct Properties
{
- std::string coreSegmentName = "Instance";
- // -1 would be infinite, but this can let the RAM grow quickly.
- long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
/// Package containing the scene snapshots
@@ -209,6 +202,7 @@ namespace armarx::armem::server::obj::instance
static const std::string timestampPlaceholder;
+
public:
struct RemoteGui
@@ -227,7 +221,9 @@ namespace armarx::armem::server::obj::instance
void update(Segment& data);
};
+
private:
+
std::unique_ptr<ArticulatedObjectVisu> visu;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index e0577877f2ac9e9c7e97e8aaa50d99c438fe4c9f..7cd8b1202ac34600f8bfc6ecc912037b49d8bee6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -37,17 +37,18 @@
namespace armarx::armem::server::obj::instance
{
- SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
- segment(iceMemory, memoryMutex),
- memoryMutex(memoryMutex)
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) :
+ segment(iceMemory)
{
}
+
std::string SegmentAdapter::getName() const
{
return Logging::tag.tagName;
}
+
void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
calibration.defineProperties(defs, prefix + "calibration.");
@@ -56,6 +57,7 @@ namespace armarx::armem::server::obj::instance
visu.defineProperties(defs, prefix + "visu.");
}
+
void SegmentAdapter::init()
{
segment.setTag(getName());
@@ -66,6 +68,7 @@ namespace armarx::armem::server::obj::instance
segment.init();
}
+
void SegmentAdapter::connect(
RobotStateComponentInterfacePrx robotStateComponent,
VirtualRobot::RobotPtr robot,
@@ -97,6 +100,7 @@ namespace armarx::armem::server::obj::instance
segment.connect(arviz);
}
+
void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
{
updateProviderInfo(providerName, info);
@@ -120,7 +124,7 @@ namespace armarx::armem::server::obj::instance
return;
}
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
std::stringstream ss;
for (const auto& id : info.supportedObjects)
{
@@ -161,7 +165,7 @@ namespace armarx::armem::server::obj::instance
}
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
if (segment.robot->hasRobotNode(calibration.robotNode))
@@ -215,7 +219,7 @@ namespace armarx::armem::server::obj::instance
TIMING_START(tGetObjectPoses);
TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
@@ -240,7 +244,7 @@ namespace armarx::armem::server::obj::instance
TIMING_START(GetObjectPoses);
TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
const IceUtil::Time now = TimeUtil::GetTime();
@@ -292,7 +296,7 @@ namespace armarx::armem::server::obj::instance
}
else
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
for (const auto& objectID : input.request.objectIDs)
{
bool found = true;
@@ -336,27 +340,31 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.providers;
}
+
Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return simox::alg::get_keys(segment.providers);
}
+
objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.getProviderInfo(providerName);
}
+
bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.providers.count(providerName) > 0;
}
@@ -364,28 +372,30 @@ namespace armarx::armem::server::obj::instance
objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.attachObjectToRobotNode(input);
}
+
objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.detachObjectFromRobotNode(input);
}
+
objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
return segment.detachAllObjectsFromRobotNodes(input);
}
objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
objpose::AgentFramesSeq output;
std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
@@ -398,6 +408,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::SignalHeadMovementOutput
SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
{
@@ -423,7 +434,7 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(segment.mutex());
const IceUtil::Time now = TimeUtil::GetTime();
@@ -493,6 +504,7 @@ namespace armarx::armem::server::obj::instance
group.addChild(layout);
}
+
void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
{
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
@@ -502,7 +514,7 @@ namespace armarx::armem::server::obj::instance
}
this->segment.update(adapter.segment);
{
- std::scoped_lock lock(adapter.memoryMutex);
+ std::scoped_lock lock(adapter.segment.mutex());
this->decay.update(adapter.segment.decay);
}
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index a1e356e53c77608937b0df952798d0427a3cc86b..353f2e0644dc044edfd1f260a76eb0c5d7857a11 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -55,7 +55,7 @@ namespace armarx::armem::server::obj::instance
{
public:
- SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+ SegmentAdapter(MemoryToIceAdapter& iceMemory);
std::string getName() const;
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
@@ -127,7 +127,6 @@ namespace armarx::armem::server::obj::instance
DebugObserverInterfacePrx debugObserver;
instance::Segment segment;
- std::mutex& memoryMutex;
instance::RobotHeadMovement robotHead;
std::mutex robotHeadMutex;
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 498a422fb201bf435cfb8ddbfdc43b71ba1944a0..defd8a3f48aa68b63673498549a731b573b9b9b4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -11,6 +11,8 @@ armarx_add_library(
# ArmarX
ArmarXCore
ArmarXCoreInterfaces
+ DebugObserverHelper
+ # ArmarXGui
ArmarXGuiComponentPlugins
# This package
RobotAPICore
@@ -73,9 +75,10 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
+ aron/JointState.xml
+ aron/Proprioception.xml
aron/TransformHeader.xml
aron/Transform.xml
- aron/JointState.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9a402c5f0f1fd80d18cdb92278404a12d5044845
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -0,0 +1,119 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!--
+Robot proprioception.
+-->
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::prop::arondto::Joints">
+
+ <ObjectChild key="positions">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="velocities">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="accellerations">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="torques">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="inertiaTorques">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="gravityTorques">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="inserveDynamicsTorques">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::prop::arondto::Platform">
+
+ <ObjectChild key="absolutePosition">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="relativePosition">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="velocity">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="accelleration">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ </Object>
+
+
+
+ <Object name="armarx::armem::prop::arondto::ForceTorque">
+
+ <ObjectChild key="force">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="torque">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensationForce">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ <ObjectChild key="gravityCompensationTorque">
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+
+ </Object>
+
+
+
+ <Object name="armarx::armem::prop::arondto::Proprioception">
+
+ <ObjectChild key="joints">
+ <armarx::armem::prop::arondto::Joints/>
+ </ObjectChild>
+
+ <ObjectChild key="platform">
+ <armarx::armem::prop::arondto::Platform/>
+ </ObjectChild>
+
+ <ObjectChild key="forceTorque">
+ <Dict>
+ <armarx::armem::prop::arondto::ForceTorque/>
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
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 13b5572d382059bcff17ce2bb5bfb04cc819b04b..b3f71e59ac2b33c19ff97fd0c238f5540503b6ee 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";
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 47660fc439e597f4c8f59bbcff666223c5964e18..4230394aa823c96e2382fd73af68d9663513b989 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -63,22 +63,21 @@ namespace armarx::armem::client::robot_state::localization
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- def->optional(properties.localizationSegment,
- propertyPrefix + "localizationSegment",
+ def->optional(properties.coreSegmentID.memoryName, propertyPrefix + "MemoryName");
+ def->optional(properties.coreSegmentID.coreSegmentName,
+ propertyPrefix + "LocalizationSegmentName",
"Name of the localization memory core segment to use.");
-
- def->optional(properties.memoryName, propertyPrefix + "Memory");
}
void TransformWriter::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
+ ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
<< "' ...";
try
{
- memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'";
+ memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,50 +90,19 @@ namespace armarx::armem::client::robot_state::localization
{
std::lock_guard g{memoryWriterMutex};
- ARMARX_DEBUG << "Trying to create core segment + provider segment";
-
- const auto providerId = [&]() -> std::optional<armem::MemoryID>
- {
- try
- {
- const auto result =
- memoryWriter.addSegment(properties.localizationSegment, transform.header.agent);
-
- if (not result.success)
- {
- ARMARX_WARNING << "Could not obtain provider id! Reason: " << result.errorMessage;
- return std::nullopt;
- }
-
- return armem::MemoryID(result.segmentID);
- }
- catch (...)
- {
- ARMARX_WARNING << "Could not obtain provider id!";
- return std::nullopt;
- }
- }();
-
- if (not providerId)
- {
- return false;
- }
-
+ const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
// const auto& timestamp = transform.header.timestamp;
- const auto timestamp = IceUtil::Time::now(); // FIXME remove
-
-
- const auto entityID = providerId->withEntityName(transform.header.parentFrame + "," +
- transform.header.frame).withTimestamp(timestamp);
+ const MemoryID entityID = providerId.withEntityName(
+ transform.header.parentFrame + "," + transform.header.frame);
+ const Time timestamp = Time::now(); // FIXME remove
armem::EntityUpdate update;
- update.entityID = entityID;
+ update.entityID = entityID;
+ update.timeCreated = timestamp;
arondto::Transform aronTransform;
toAron(aronTransform, transform);
-
update.instancesData = {aronTransform.toAron()};
- update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -149,10 +117,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
- // const std::string& TransformWriter::getPropertyPrefix() const
- // {
- // return propertyPrefix;
- // }
-
-
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 299c79b69222accb8290a7064229a9dfe59e8572..e553b073464e5fcf1444e14a4fef48e2034e808c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -61,6 +61,7 @@ namespace armarx::armem::client::robot_state::localization
bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
+
private:
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -70,8 +71,7 @@ namespace armarx::armem::client::robot_state::localization
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationSegment = "Localization";
+ MemoryID coreSegmentID { "RobotState", "Localization" };
} properties;
const std::string propertyPrefix = "mem.robot_state.";
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index a59aa5a4c6cdd3ebe552522c0ffd532e873da82a..c4d0e05b4a1120b0ba6d256ad646508d7556ab7d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -123,7 +123,7 @@ namespace armarx::armem::common::robot_state::localization
}
else
{
- ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ ARMARX_WARNING << deactivateSpam(10) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
<< "'";
}
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 9762ea0339755892ac79ef62dbfcc88353995b00..d6dcddd7d3fd568216a63657bfe6ae5ca1f10f9c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -1,7 +1,8 @@
#include "Visu.h"
-#include <Eigen/src/Geometry/Transform.h>
#include <algorithm>
+#include <exception>
+#include <string>
#include <Eigen/Geometry>
@@ -10,23 +11,29 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/math/pose.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "RobotAPI/components/ArViz/Client/Elements.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <exception>
-#include <string>
#include "../description/Segment.h"
#include "../localization/Segment.h"
#include "../proprioception/Segment.h"
+
namespace armarx::armem::server::robot_state
{
+ Visu::Visu(const description::Segment& descriptionSegment, const proprioception::Segment& proprioceptionSegment, const localization::Segment& localizationSegment)
+ : descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(
@@ -34,9 +41,31 @@ namespace armarx::armem::server::robot_state
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
- void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+
+ void Visu::init()
{
+ }
+
+
+ void Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ {
+ this->arviz = arviz;
+ if (debugObserver)
+ {
+ bool batchMode = true;
+ this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
+ }
+
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+ void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ {
const auto visualizeRobot = [&](const robot::Robot & robot)
{
const auto xmlPath = robot.description.xml.serialize();
@@ -57,6 +86,7 @@ namespace armarx::armem::server::robot_state
std::for_each(robots.begin(), robots.end(), visualizeRobot);
}
+
void Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
@@ -69,79 +99,13 @@ namespace armarx::armem::server::robot_state
for (const auto& frame : robotFrames)
{
Eigen::Affine3f from = pose;
+ Eigen::Affine3f to = pose * frame;
- pose = pose * frame; // to
-
- Eigen::Affine3f to = pose;
-
- const auto arr = viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation());
- layerFrames.add(arr);
+ layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
}
}
}
- void Visu::init() {}
-
- void Visu::connect(const viz::Client& arviz)
- {
- this->arviz = arviz;
-
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- updateTask->start();
- }
-
- // void Visu::RemoteGui::setup(const Visu& visu)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // enabled.setValue(visu.enabled);
- // inGlobalFrame.setValue(visu.inGlobalFrame);
- // alpha.setRange(0, 1.0);
- // alpha.setValue(visu.alpha);
- // alphaByConfidence.setValue(visu.alphaByConfidence);
- // oobbs.setValue(visu.oobbs);
- // objectFrames.setValue(visu.objectFrames);
- // {
- // float max = 10000;
- // objectFramesScale.setRange(0, max);
- // objectFramesScale.setDecimals(2);
- // objectFramesScale.setSteps(int(10 * max));
- // objectFramesScale.setValue(visu.objectFramesScale);
- // }
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
- // row++;
- // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
- // row++;
- // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
- // row++;
- // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
- // row++;
- // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
- // row++;
- // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
- // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
- // row++;
-
- // group.setLabel("Visualization");
- // group.addChild(grid);
- // }
-
- // void Visu::RemoteGui::update(Visu& visu)
- // {
- // visu.enabled = enabled.getValue();
- // visu.inGlobalFrame = inGlobalFrame.getValue();
- // visu.alpha = alpha.getValue();
- // visu.alphaByConfidence = alphaByConfidence.getValue();
- // visu.oobbs = oobbs.getValue();
- // visu.objectFrames = objectFrames.getValue();
- // visu.objectFramesScale = objectFramesScale.getValue();
- // }
robot::Robots combine(
const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
@@ -191,34 +155,52 @@ namespace armarx::armem::server::robot_state
return robots;
}
+
void Visu::visualizeRun()
{
-
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask && not updateTask->isStopped())
{
{
// std::scoped_lock lock(visuMutex);
ARMARX_DEBUG << "Update task";
-
if (p.enabled)
{
- TIMING_START(Visu);
+ TIMING_START(tVisuTotal);
// TODO(fabian.reister): use timestamp
-
- const auto timestamp = IceUtil::Time::now();
+ const Time timestamp = Time::now();
try
{
- const auto robotDescriptions =
- descriptionSegment.getRobotDescriptions(timestamp);
+ // Get data.
+ TIMING_START(tVisuGetData);
+
+ TIMING_START(tRobotDescriptions);
+ const description::Segment::RobotDescriptionMap robotDescriptions =
+ descriptionSegment.getRobotDescriptionsLocking(timestamp);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
+
+ TIMING_START(tGlobalRobotPoseMap);
const auto globalRobotPoseMap =
- localizationSegment.getRobotGlobalPoses(timestamp);
+ localizationSegment.getRobotGlobalPosesLocking(timestamp);
+ TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_DEBUG);
+
+ TIMING_START(tRobotFramePoses);
+ const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
+ TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
+
+ TIMING_START(tRobotJointPositionMap);
const auto robotJointPositionMap =
- proprioceptionSegment.getRobotJointPositions(timestamp);
+ proprioceptionSegment.getRobotJointPositionsLocking(
+ timestamp, debugObserver ? &*debugObserver : nullptr);
+ TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
- const auto frames = localizationSegment.getRobotFramePoses(timestamp);
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
// we need all 3 informations:
// - robot description
@@ -228,44 +210,55 @@ namespace armarx::armem::server::robot_state
const robot::Robots robots =
combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
- viz::Layer layer = arviz.layer("Robots");
- ARMARX_DEBUG << "visualizing robots";
+ ARMARX_DEBUG << "Visualize robots ...";
+ viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
- ARMARX_DEBUG << "Committing robots";
-
+ ARMARX_DEBUG << "Visualize frames ...";
viz::Layer layerFrames = arviz.layer("Frames");
-
- ARMARX_DEBUG << "visualizing frames";
visualizeFrames(layerFrames, frames);
- ARMARX_DEBUG << "Committing frames";
-
- arviz.commit({layer, layerFrames});
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
- ARMARX_DEBUG << "Done committing";
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ // Commit layers.
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
+ ARMARX_DEBUG << "Commit visualization ...";
+ TIMING_START(tVisuCommit);
+ arviz.commit({layer, layerFrames});
+ TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
+
+ if (debugObserver.has_value())
+ {
+ const std::string p = "Visu | ";
+ debugObserver->setDebugObserverDatafield(p + "t Total (ms)", tVisuTotal.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)", tVisuGetData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)", tRobotDescriptions.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Pose (ms)", tGlobalRobotPoseMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)", tRobotFramePoses.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.4 Joint Positions (ms)", tRobotJointPositionMap.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)", tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)", tVisuCommit.toMilliSecondsDouble());
+ }
}
catch (const std::exception& ex)
{
- ARMARX_WARNING << ex.what();
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << ex.what();
continue;
}
catch (...)
{
- ARMARX_WARNING << "Unknown exception";
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
continue;
}
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
}
}
cycle.waitForCycleDuration();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 7e4b3814ce3a2c03f4ea8fb38846d4378c3a18c6..95efae41963c63669e4e4bb11b202ba9ef948ec9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -21,10 +21,11 @@
#pragma once
+#include <optional>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
-// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -42,12 +43,10 @@ namespace armarx::armem::server::robot_state
{
class Segment;
}
-
namespace proprioception
{
class Segment;
}
-
namespace description
{
class Segment;
@@ -64,17 +63,13 @@ namespace armarx::armem::server::robot_state
Visu(const description::Segment& descriptionSegment,
const proprioception::Segment& proprioceptionSegment,
- const localization::Segment& localizationSegment)
- : descriptionSegment(descriptionSegment),
- proprioceptionSegment(proprioceptionSegment),
- localizationSegment(localizationSegment)
- {}
+ const localization::Segment& localizationSegment);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
void init();
+ void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
- void connect(const viz::Client& arviz);
protected:
@@ -89,7 +84,9 @@ namespace armarx::armem::server::robot_state
private:
+
viz::Client arviz;
+ std::optional<DebugObserverHelper> debugObserver;
const description::Segment& descriptionSegment;
const proprioception::Segment& proprioceptionSegment;
@@ -105,23 +102,6 @@ namespace armarx::armem::server::robot_state
SimpleRunningTask<>::pointer_type updateTask;
void visualizeRun();
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::CheckBox enabled;
-
- // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
- // armarx::RemoteGui::Client::FloatSlider alpha;
- // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
- // armarx::RemoteGui::Client::CheckBox oobbs;
- // armarx::RemoteGui::Client::CheckBox objectFrames;
- // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
-
- // // void setup(const Visu& visu);
- // // void update(Visu& visu);
- // };
-
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index d50d965608f7cf4ad8541467d75442303a293211..6f247e9bef4328d2d40dc289d0953cb2a3ddcd55 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,19 +1,20 @@
#include "Segment.h"
-#include <IceUtil/Time.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <sstream>
+#include <thread>
+
+#include <IceUtil/Time.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
-#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
@@ -24,51 +25,32 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <thread>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem::server::robot_state::description
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
- std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter, arondto::RobotDescription::toAronType(),
+ "Description")
{
- Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment,
- prefix + "seg.description.CoreSegment",
- "Name of the robot description core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.description.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
- }
- void Segment::init()
+ void Segment::connect(const RobotUnitInterfacePrx& robotUnitPrx)
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::RobotDescription::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
-
- void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
- {
- // this->visu = std::make_unique<Visu>(arviz, *this);
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
updateRobotDescription();
}
- void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+
+ void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
@@ -88,139 +70,76 @@ namespace armarx::armem::server::robot_state::description
Commit commit;
commit.updates.push_back(update);
-
- {
- // std::lock_guard g{memoryMutex};
- iceMemory.commit(commit);
- }
-
+ iceMemory.commitLocking(commit);
}
+
void Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
- auto kinematicUnit = robotUnit->getKinematicUnit();
-
- ARMARX_CHECK_NOT_NULL(kinematicUnit);
+ KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
+ if (kinematicUnit)
+ {
+ const std::string robotName = kinematicUnit->getRobotName();
+ const std::string robotFilename = kinematicUnit->getRobotFilename();
- const auto robotName = kinematicUnit->getRobotName();
- const auto robotFilename = kinematicUnit->getRobotFilename();
+ const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
- const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
- ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+ const robot::RobotDescription robotDescription
+ {
+ .name = kinematicUnit->getRobotName(),
+ .xml = {package, kinematicUnit->getRobotFilename()}
+ }; // FIXME
- const robot::RobotDescription robotDescription
+ commitRobotDescription(robotDescription);
+ }
+ else
{
- .name = kinematicUnit->getRobotName(),
- .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Cannot commit robot description.";
+ }
+ }
- storeRobotDescription(robotDescription);
+
+ Segment::RobotDescriptionMap Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
+ {
+ std::scoped_lock lock(mutex());
+ return getRobotDescriptions(timestamp);
}
+
Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ (void) timestamp; // Unused
RobotDescriptionMap robotDescriptions;
-
{
- // std::lock_guard g{memoryMutex};
-
- for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ for (const auto& [_, provSeg] : *coreSegment)
{
- for (const auto &[name, entity] : provSeg.entities())
+ for (const auto& [name, entity] : provSeg.entities())
{
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto description = robot::convertRobotDescription(entityInstance);
- if (not description)
+ if (description)
{
- ARMARX_WARNING << "Could not convert entity instance to "
- "'RobotDescription'";
- continue;
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
}
-
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
-
- robotDescriptions.emplace(description->name, *description);
}
}
}
- ARMARX_INFO << deactivateSpam(10) << "Number of known robot descriptions: " << robotDescriptions.size();
-
+ ARMARX_INFO << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- // {
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- // {
- // for (const auto& [name, entity] : provSeg.entities())
- // {
- // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // const auto description = articulated_object::convertRobotDescription(entityInstance);
-
- // if (not description)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- // continue;
- // }
-
- // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
-
- // objects.emplace(armem::MemoryID(entity.id()), *description);
- // }
- // }
-
- // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
-
- // return objects;
- // }
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 2f603cf4961d90342fbd74770a0ef4d0d181c033..d020b5543e2c2f59ff593fc70c753640679b5a2b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -21,94 +21,47 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <string>
#include <optional>
#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-
-namespace armarx::armem
-{
- namespace server
- {
- class MemoryToIceAdapter;
- }
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
namespace armarx::armem::server::robot_state::description
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
- virtual ~Segment();
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void connect(const RobotUnitInterfacePrx& robotUnitPrx);
- void init();
/// mapping "robot name" -> "robot description"
using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
+ RobotDescriptionMap getRobotDescriptionsLocking(const armem::Time& timestamp) const;
+
private:
- void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void commitRobotDescription(const robot::RobotDescription& robotDescription);
void updateRobotDescription();
-
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
-
RobotUnitInterfacePrx robotUnit;
- struct Properties
- {
- std::string coreSegment = "Description";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
};
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index e0dcf6c2bcd9d86012d98cf5aded6fee585ed866..dd01313495ac922287f36549342feb2575c7d053 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -1,7 +1,6 @@
#include "Segment.h"
// STL
-
#include <iterator>
#include <sstream>
@@ -12,6 +11,8 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -36,66 +37,50 @@
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+
namespace armarx::armem::server::robot_state::localization
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
- std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter, arondto::Transform::toAronType(),
+ "Localization", 1024)
{
- Logging::setTag("LocalizationSegment");
}
+
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment,
- prefix + "seg.localization.CoreSegment",
- "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize,
- prefix + "seg.localization.MaxHistorySize",
- "Maximal size of object poses history (-1 for infinite).");
- }
- void Segment::init()
+ void Segment::connect()
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
- coreSegment = &iceMemory.workingMemory->addCoreSegment(
- p.coreSegment, arondto::Transform::toAronType());
- coreSegment->setMaxHistorySize(p.maxHistorySize);
}
- void Segment::connect(viz::Client arviz)
+
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
- // this->visu = std::make_unique<Visu>(arviz, *this);
+ std::scoped_lock lock(mutex());
+ return getRobotFramePoses(timestamp);
}
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>>
- Segment::getRobotFramePoses(const armem::Time& timestamp) const
+
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> frames;
-
- const auto& localizationCoreSegment =
- iceMemory.workingMemory->getCoreSegment(p.coreSegment);
- const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
+ RobotFramePoseMap frames;
+ const auto knownRobots = simox::alg::get_keys(*coreSegment);
for (const auto& robotName : knownRobots)
{
-
TransformQuery query{.header{.parentFrame = GlobalFrame,
.frame = "root", // TODO, FIXME
.agent = robotName,
.timestamp = timestamp}};
- const auto result = TransformHelper::lookupTransformChain(localizationCoreSegment, query);
-
+ const auto result = TransformHelper::lookupTransformChain(*coreSegment, query);
if (not result)
{
// TODO
@@ -105,26 +90,32 @@ namespace armarx::armem::server::robot_state::localization
frames.emplace(robotName, result.transforms);
}
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Number of known robot pose chains: " << frames.size();
return frames;
}
- std::unordered_map<std::string, Eigen::Affine3f>
- Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+
+ Segment::RobotPoseMap
+ Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
{
+ std::scoped_lock lock(mutex());
+ return getRobotGlobalPoses(timestamp);
+ }
+
+ Segment::RobotPoseMap
+ Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+ {
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
+ RobotPoseMap robotGlobalPoses;
- const auto& localizationCoreSegment =
- iceMemory.workingMemory->getCoreSegment(p.coreSegment);
- const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
+ const std::vector<std::string> knownRobots = simox::alg::get_keys(*coreSegment);
- for (const auto& robotName : knownRobots)
+ for (const std::string& robotName : knownRobots)
{
TransformQuery query{.header{.parentFrame = GlobalFrame,
@@ -132,8 +123,7 @@ namespace armarx::armem::server::robot_state::localization
.agent = robotName,
.timestamp = timestamp}};
- const auto result = TransformHelper::lookupTransform(localizationCoreSegment, query);
-
+ const auto result = TransformHelper::lookupTransform(*coreSegment, query);
if (not result)
{
// TODO
@@ -143,106 +133,47 @@ namespace armarx::armem::server::robot_state::localization
robotGlobalPoses.emplace(robotName, result.transform.transform);
}
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Number of known robot poses: " << robotGlobalPoses.size();
return robotGlobalPoses;
}
- bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
+
+ bool Segment::commitTransform(const armarx::armem::robot_state::Transform& transform)
{
- const auto& timestamp = transform.header.timestamp;
+ Commit commit;
+ commit.add(makeUpdate(transform));
- const MemoryID providerID =
- coreSegment->id().withProviderSegmentName(transform.header.agent);
- if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::Transform::toAronType());
- }
+ const armem::CommitResult result = iceMemory.commit(commit);
+ return result.allSuccess();
+ }
+
+ bool Segment::commitTransformLocking(const armarx::armem::robot_state::Transform& transform)
+ {
Commit commit;
+ commit.add(makeUpdate(transform));
- EntityUpdate& update = commit.updates.emplace_back();
- update.entityID =
- providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ const armem::CommitResult result = iceMemory.commitLocking(commit);
+ return result.allSuccess();
+ }
+
+
+ EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const
+ {
+ const armem::Time& timestamp = transform.header.timestamp;
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent);
+
+ EntityUpdate update;
+ update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
update.timeArrived = update.timeCreated = update.timeSent = timestamp;
arondto::Transform aronTransform;
toAron(aronTransform, transform);
update.instancesData = {aronTransform.toAron()};
- const armem::CommitResult result = iceMemory.commit(commit);
- return result.allSuccess();
+ return update;
}
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
- // {
- // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
-
- // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- // {
- // for (const auto& [name, entity] : provSeg.entities())
- // {
- // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // const auto description = articulated_object::convertRobotDescription(entityInstance);
-
- // if (not description)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- // continue;
- // }
-
- // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
-
- // objects.emplace(armem::MemoryID(entity.id()), *description);
- // }
- // }
-
- // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
-
- // return objects;
- // }
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 307eaae17babf66aa9d0a1e759cbc348cbb05008..d573c37ec8c90fc25a4de80eed877bc32f825dcd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -23,86 +23,53 @@
#include <string>
#include <optional>
-#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
+#include <Eigen/Geometry>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
+
namespace armarx::armem
{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
+ class EntityUpdate;
} // namespace armarx::armem
-
namespace armarx::armem::server::robot_state::localization
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
- virtual ~Segment();
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
- void connect(viz::Client arviz);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ public:
- void init();
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> getRobotFramePoses(const armem::Time& timestamp) const;
- bool storeTransform(const armarx::armem::robot_state::Transform& transform);
+ void connect();
- private:
+ RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
+ RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
+ RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
+ RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
- struct Properties
- {
- std::string coreSegment = "Localization";
- int64_t maxHistorySize = -1;
- };
- Properties p;
+ bool commitTransform(const armarx::armem::robot_state::Transform& transform);
+ bool commitTransformLocking(const armarx::armem::robot_state::Transform& transform);
- // std::unique_ptr<Visu> visu;
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
+ private:
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+ EntityUpdate makeUpdate(const armarx::armem::robot_state::Transform& transform) const;
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index ac5e5e1c36e66d47251700ec6565a9fb61ce5430..5e01e84ebc5724f672c7f32d01497624d0759b3e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,176 +1,166 @@
#include "Segment.h"
-#include <mutex>
#include <sstream>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
namespace armarx::armem::server::robot_state::proprioception
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedSegment(memoryToIceAdapter, prop::arondto::Proprioception::toAronType(),
+ "Proprioception", 1024)
{
- Logging::setTag("ProprioceptionSegment");
}
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.coreSegment, prefix + "seg.proprioception.CoreSegment", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- }
- void Segment::init()
+ void Segment::connect(RobotUnitInterfacePrx robotUnitPrx)
{
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+ this->robotUnit = robotUnitPrx;
- ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
+ std::string providerSegmentName = "Robot";
+ KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
+ if (kinematicUnit)
+ {
+ providerSegmentName = kinematicUnit->getRobotName();
+ }
+ else
+ {
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
+ }
+ this->robotUnitProviderID = coreSegment->id().withProviderSegmentName(providerSegmentName);
}
- void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
- {
- robotUnit = robotUnitPrx;
-
- auto kinematicUnit = robotUnit->getKinematicUnit();
- const auto providerSegmentName = kinematicUnit->getRobotName();
-
- // TODO what is the purpose?
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
- ARMARX_INFO << "Adding provider segment " << p.coreSegment << "/" << providerSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = p.coreSegment;
- input.providerSegmentName = providerSegmentName;
+ Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
+ {
+ TIMING_START(tRobotJointPositionsLock);
+ std::scoped_lock lock(mutex());
+ TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_DEBUG);
+ if (debugObserver)
{
- std::lock_guard g{memoryMutex};
- auto result = iceMemory.addSegments({input})[0];
-
- if (!result.success)
- {
- ARMARX_ERROR << "Could not add segment " << p.coreSegment << "/" << providerSegmentName << ". The error message is: " << result.errorMessage;
- }
+ debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
}
-
- robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
- robotUnitProviderID.coreSegmentName = p.coreSegment;
- robotUnitProviderID.providerSegmentName = providerSegmentName;
+ return getRobotJointPositions(timestamp, debugObserver);
}
- std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
+
+ Segment::RobotJointPositionMap Segment::getRobotJointPositions(
+ const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
{
- std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ namespace adn = aron::datanavigator;
+ ARMARX_CHECK_NOT_NULL(coreSegment);
- std::lock_guard g{memoryMutex};
+ RobotJointPositionMap jointMap;
- for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ TIMING_START(tProcessProviders);
+ for (const auto& [robotName, provSeg] : *coreSegment)
{
+ TIMING_START(tProcessEntities);
+ int i = 0;
for (const auto& [name, entity] : provSeg.entities())
{
- try
+ TIMING_START(tProcessEntity);
+
+ TIMING_START(tGetLatestInstance);
+ if (entity.empty())
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+ continue;
+ }
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ if (snapshot.empty())
+ {
+ continue;
+ }
+ const wm::EntityInstance& entityInstance = snapshot.getInstance(0);
+ TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG);
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
- }
- jointMap[robotName].emplace(jointState->name, jointState->position);
+ TIMING_START(tCastJointState);
+ aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
+ // Avoid throwing an exception.
+ if (!(data->hasElement("name") && data->hasElement("position")))
+ {
+ continue;
+ }
+ const std::string jointName = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
+ const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
+ TIMING_END_COMMENT_STREAM(tCastJointState, "tCastJointState " + std::to_string(i), ARMARX_DEBUG);
+ TIMING_START(tEmplace);
+ if (not jointName.empty())
+ {
+ jointMap[robotName].emplace(jointName, jointPosition);
}
- catch (...) // empty history etc
+ else
{
+ // This may happen, just ignore it.
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ }
+ TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + std::to_string(i), ARMARX_DEBUG);
+ TIMING_END_COMMENT_STREAM(tProcessEntity, "tProcessEntity " + std::to_string(i), ARMARX_DEBUG);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.1 GetLatestInstance (ms)", tGetLatestInstance.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.2 CastJointState (ms)", tCastJointState.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.3 Emplace (ms)", tEmplace.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.T Process Entity (ms)", tProcessEntity.toMilliSecondsDouble());
}
+ ++i;
+ }
+ TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
}
}
+ TIMING_END_STREAM(tProcessProviders, ARMARX_DEBUG);
+
+ ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
- ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield(dp + "t 1 Process Providers (ms)", tProcessProviders.toMilliSecondsDouble());
+ }
return jointMap;
}
+
const armem::MemoryID& Segment::getRobotUnitProviderID() const
{
return robotUnitProviderID;
}
-
-
- // void Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.p.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // row++;
- // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- // row++;
- // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- // row++;
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void Segment::RemoteGui::update(Segment& data)
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- // || discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // std::scoped_lock lock(data.memoryMutex);
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (data.coreSegment)
- // {
- // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- // }
- // }
-
- // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- // }
- // }
-
} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index bc83a946bcadafbc85a15d55cff79c3a18af970f..87ef6a4e2b7937f3196a2ee0ce4610b22fd1e1cb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -21,89 +21,53 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <string>
#include <optional>
-#include <mutex>
#include <unordered_map>
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-
-// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotAPI/components/ArViz/Client/Client.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-namespace armarx::armem
+namespace armarx
{
- namespace server
- {
- class MemoryToIceAdapter;
- }
-
- namespace wm
- {
- class CoreSegment;
- }
-} // namespace armarx::armem
-
-
+ class DebugObserverHelper;
+}
namespace armarx::armem::server::robot_state::proprioception
{
- class Visu;
-
- class Segment : public armarx::Logging
+ class Segment : public segment::SpecializedSegment
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
- virtual ~Segment();
+ using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
+
+ public:
- void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void connect(RobotUnitInterfacePrx robotUnitPrx);
- void init();
- std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
+ RobotJointPositionMap getRobotJointPositions(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
+ RobotJointPositionMap getRobotJointPositionsLocking(
+ const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
- private:
- server::MemoryToIceAdapter& iceMemory;
- wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
+ private:
RobotUnitInterfacePrx robotUnit;
-
armem::MemoryID robotUnitProviderID;
- struct Properties
- {
- std::string coreSegment = "Proprioception";
- int64_t maxHistorySize = -1;
- };
- Properties p;
-
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
+ // Debug Observer prefix
+ const std::string dp = "Proprioception::getRobotJointPositions() | ";
};
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index bdc05099f108f0abaecb8db68cc58f47b8688a6c..00f355baa9c01e66792a886b027ad4186b808016 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -55,6 +55,7 @@ namespace armarx::aron
{
nextMaybeType = getRandomElement(type::ALL_MAYBE_TYPES);
}
+ (void) nextMaybeType; // Unused
switch (nextType)
{