diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 98450f59a9acce6bf2788644c34f49d40392eb6e..2388c61e39a8e0582f63b11dec0ca45ec51c8d24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -280,7 +280,7 @@
propertyName="ObstacleDetectionName"
propertyIsOptional="true"
propertyDefaultValue="PlatformObstacleAvoidance" />
- <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
+ <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
humanName="Dynamic obstacle manager interface"
typeName="DynamicObstacleManagerInterfacePrx"
memberName="dynamicObstacleManager"
@@ -320,13 +320,13 @@
propertyName="DebugDrawerToArVizLayerBlackWhitelistTopicName"
propertyIsOptional="true"
propertyDefaultValue="DebugDrawerToArVizLayerBlackWhitelistUpdates" />
- <Proxy include="RobotAPI/interface/objectpose/ObjectPoseObserver.h"
- humanName="ObjectPoseObserver"
- typeName="objpose::ObjectPoseObserverInterfacePrx"
- memberName="ObjectPoseObserver"
- getterName="getObjectPoseObserver"
- propertyName="ObjectPoseObserverName"
+ <Proxy include="RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h"
+ humanName="ObjectPoseStorage"
+ typeName="objpose::ObjectPoseStorageInterfacePrx"
+ memberName="ObjectPoseStorage"
+ getterName="getObjectPoseStorage"
+ propertyName="ObjectPoseStorageName"
propertyIsOptional="true"
- propertyDefaultValue="ObjectPoseObserver" />
+ propertyDefaultValue="ObjectMemory" />
</Lib>
</VariantInfo>
diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index d0a545f7a2a6c2d0fe730d3169b208760aec0abe..d9e24e657c2b57a11bac7d34b2df65dff2e9eb8d 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -84,6 +84,14 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.EnableProfiling = false
+# ArmarX.ExampleMemory.:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory. = ""
+
+
# ArmarX.ExampleMemory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -155,10 +163,10 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ArMemMemoryNameSystem
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.mns.MemoryNameSystemName = ArMemMemoryNameSystem
+# ArmarX.ExampleMemory.mns.MemoryNameSystemName = MemoryNameSystem
# ArmarX.ExampleMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
diff --git a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index 76d3d877581dc891a0aa8a4092e1072d9d06dc87..10325be750b47ccdfa1827265cce4d881d58d2ed 100644
--- a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -138,10 +138,10 @@ ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ArMemMemoryNameSystem
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemName = ArMemMemoryNameSystem
+# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemName = MemoryNameSystem
# ArmarX.ExampleMemoryClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
diff --git a/scenarios/ArMemExample/config/MemoryNameSystem.cfg b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
index 869022057679ebbbe32a28eb4a9962b11239e6f7..7dd22218243ca4f9e67e843da8b42916f3b8568a 100644
--- a/scenarios/ArMemExample/config/MemoryNameSystem.cfg
+++ b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
@@ -115,7 +115,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.MemoryNameSystem.ObjectName = "ArMemMemoryNameSystem"
+# ArmarX.MemoryNameSystem.ObjectName = ""
# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
diff --git a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
similarity index 55%
rename from scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
rename to scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 1aa5238bfbb09c201635d6504393e499e7e21593..95ec0bddab3697b5270df9c454318d38b63e6467 100644
--- a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -1,11 +1,13 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="ObjectPoseObserverExample" creation="2020-08-03.10:09:13" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="ObjectPoseObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="ArMemObjectMemory" creation="2021-04-22.11:29:22" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ObjectPoseObserverExample/config/ArVizStorage.cfg b/scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/ArVizStorage.cfg
rename to scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/DebugObserver.cfg b/scenarios/ArMemObjectMemory/config/DebugObserver.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/DebugObserver.cfg
rename to scenarios/ArMemObjectMemory/config/DebugObserver.cfg
diff --git a/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
similarity index 56%
rename from scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
rename to scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 279193cc2f6feef27a705bf30dbae8451f7bfe14..22981bc341684bcac781c01b26249ba6376c1ad2 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# ObjectPoseObserver properties
+# ObjectMemory properties
# ==================================================================
# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
@@ -92,262 +92,330 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.ObjectPoseObserver.ArVizTopicName: Name of the ArViz topic
+# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ArVizTopicName = ArVizTopic
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
-# ArmarX.ObjectPoseObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# ArmarX.ObjectMemory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.CreateUpdateFrequenciesChannel = false
+# ArmarX.ObjectMemory.EnableProfiling = false
-# ArmarX.ObjectPoseObserver.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ObjectMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: false
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.EnableProfiling = false
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
-# ArmarX.ObjectPoseObserver.KinematicUnitObserverName: Name of the kinematic unit observer.
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.KinematicUnitObserverName = KinematicUnitObserver
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: 50
+# - Default: Object
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency = 50
+# ArmarX.ObjectMemory.mem.MemoryName = Object
-# ArmarX.ObjectPoseObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# ArmarX.ObjectMemory.mem.cls.CoreSegmentName: Name of the object clazz core segment.
# Attributes:
-# - Default: 5000
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistorySize = 5000
+# ArmarX.ObjectMemory.mem.cls.CoreSegmentName = Class
-# ArmarX.ObjectPoseObserver.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
-# - Default: Undefined
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ObjectPoseObserver.MinimumLoggingLevel = Undefined
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
-# ArmarX.ObjectPoseObserver.ObjectName: Name of IceGrid well-known object
+# ArmarX.ObjectMemory.mem.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
# Attributes:
-# - Default: ""
+# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ObjectName = ""
+# ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
-# ArmarX.ObjectPoseObserver.ObjectPoseTopicName: Name of the Object Pose Topic.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage: Name of the objects package to load from.
# Attributes:
-# - Default: ObjectPoseTopic
+# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ObjectPoseTopicName = ObjectPoseTopic
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
-# ArmarX.ObjectPoseObserver.RemoteGuiName: Name of the remote gui provider
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: Instance
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.RemoteGuiName = RemoteGuiProvider
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName = Instance
-# ArmarX.ObjectPoseObserver.RemoteStateComponentName: Name of the robot state component
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
# Attributes:
-# - Default: RobotStateComponent
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.RemoteStateComponentName = RobotStateComponent
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize = -1
-# ArmarX.ObjectPoseObserver.calibration.offset: Offset for the node to be calibrated.
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.calibration.offset = 0
+# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
-# ArmarX.ObjectPoseObserver.calibration.robotNode: Robot node which can be calibrated.
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
# Attributes:
# - Default: Neck_2_Pitch
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.calibration.robotNode = Neck_2_Pitch
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
-# ArmarX.ObjectPoseObserver.decay.delaySeconds: Duration after latest localization before decay starts.
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
# Attributes:
# - Default: 5
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.delaySeconds = 5
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
-# ArmarX.ObjectPoseObserver.decay.durationSeconds: How long to reach minimal confidence.
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
# Attributes:
# - Default: 20
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.durationSeconds = 20
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
-# ArmarX.ObjectPoseObserver.decay.enabled: If true, object poses decay over time when not localized anymore.
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.decay.enabled = false
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
-# ArmarX.ObjectPoseObserver.decay.maxConfidence: Confidence when decay starts.
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.maxConfidence = 1
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
-# ArmarX.ObjectPoseObserver.decay.minConfidence: Confidence after decay duration.
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.minConfidence = 0
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
-# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
# Attributes:
# - Default: 0.100000001
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence = 0.100000001
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
-# ArmarX.ObjectPoseObserver.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.head.checkHeadVelocity = true
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
-# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
# Attributes:
# - Default: 100
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS = 100
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
-# ArmarX.ObjectPoseObserver.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
# Attributes:
# - Default: 0.0500000007
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
-# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
-# Attributes:
-# - Default: DebugObserver
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver = DebugObserver
-
-
-# ArmarX.ObjectPoseObserver.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.alpha = 1
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
-# ArmarX.ObjectPoseObserver.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.alphaByConfidence = false
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
-# ArmarX.ObjectPoseObserver.visu.enabled: Enable or disable visualization of objects.
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-ArmarX.ObjectPoseObserver.visu.enabled = true
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
-# ArmarX.ObjectPoseObserver.visu.frequenzyHz: Frequency of visualization.
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
# Attributes:
# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.frequenzyHz = 25
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
-# ArmarX.ObjectPoseObserver.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.inGlobalFrame = true
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
-# ArmarX.ObjectPoseObserver.visu.objectFrames: Enable showing object frames.
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.objectFrames = false
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
-# ArmarX.ObjectPoseObserver.visu.objectFramesScale: Scaling of object frames.
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.objectFramesScale = 1
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
-# ArmarX.ObjectPoseObserver.visu.oobbs: Enable showing oriented bounding boxes.
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
diff --git a/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f75bad15e903fd399e990eb38dce28887c537d98
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ObjectPoseClientExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObjectPoseClientExample.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectPoseClientExample.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.ObjectPoseClientExample.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseClientExample.EnableProfiling = false
+
+
+# ArmarX.ObjectPoseClientExample.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ObjectPoseClientExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectPoseClientExample.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.ObjectPoseClientExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ObjectName = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
similarity index 98%
rename from scenarios/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg
rename to scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
index 53ccf9cb429c6f9b94ebf0aed72f3a72a9c368a9..45075e6114eae82fe89f50317c0bced824c96b46 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
@@ -132,7 +132,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {KIT/Amicelli, KIT/YellowSaltCylinder}
-ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, YCB/002_master_chef_can
+# ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, KIT/YellowSaltCylinder
# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
diff --git a/scenarios/ObjectPoseObserverExample/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemObjectMemory/config/RemoteGuiProviderApp.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RemoteGuiProviderApp.cfg
rename to scenarios/ArMemObjectMemory/config/RemoteGuiProviderApp.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/RobotStateComponent.cfg b/scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RobotStateComponent.cfg
rename to scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/RobotToArVizApp.cfg b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RobotToArVizApp.cfg
rename to scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/global.cfg b/scenarios/ArMemObjectMemory/config/global.cfg
similarity index 96%
rename from scenarios/ObjectPoseObserverExample/config/global.cfg
rename to scenarios/ArMemObjectMemory/config/global.cfg
index 7c603bf9905a566cbf240054770023ad225aebbc..39e1d4a1aac92cf8ffdb032a033f2d88d9534601 100644
--- a/scenarios/ObjectPoseObserverExample/config/global.cfg
+++ b/scenarios/ArMemObjectMemory/config/global.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# Global Config from Scenario ObjectPoseObserverExample
+# Global Config from Scenario ArMemObjectMemory
# ==================================================================
# RobotConfig.AgentName: Custom Property
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 7e1ec4053f83a4d33bb323bd129ff87f0d7b703a..5a21a1cd4e02935799bdd4788aa7e2cac5ca12c8 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -89,6 +89,9 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
+add_library(RobotAPI::ArViz ALIAS ArViz)
+
+
armarx_component_set_name("ArVizStorage")
set(COMPONENT_LIBS
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 57e7a5c74665b112f0999c37be4bd2ff37be5687..3ce3ea6f48c69658788abc3ba2ed00c6a81e1002 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -21,6 +21,15 @@ namespace armarx::viz
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
+
+ Object& Object::alpha(float alpha)
+ {
+ if (alpha < 1)
+ {
+ overrideColor(simox::Color::white().with_alpha(alpha));
+ }
+ return *this;
+ }
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index b6eadc5bd0cea09050d4dc6f8ddb79dc84bd9227..297a22469d52be8b7cda3c71438f6ee0617c0091 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -38,7 +38,7 @@ namespace Eigen
namespace armarx
{
- ///@see <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+ // <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
class ObjectID;
}
@@ -479,6 +479,7 @@ namespace armarx::viz
Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& alpha(float alpha);
Object& useCollisionModel()
{
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index cec5e53f76ea3d9d9a52745e3e50923e7ca01b2d..d19343cee661810483206528bfccaa62c614bff4 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -18,7 +18,7 @@ add_subdirectory(KITHandUnit)
add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
add_subdirectory(NaturalIKTest)
-add_subdirectory(ObjectPoseObserver)
+add_subdirectory(ObjectPoseClientExample)
add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(RobotHealth)
add_subdirectory(RobotNameService)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1ff92199fbec6f531cb69daf5ffa9b8266abbd59
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt
@@ -0,0 +1,25 @@
+armarx_component_set_name("ObjectPoseClientExample")
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCoreComponentPlugins
+ # RobotAPI
+ RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+)
+
+set(SOURCES
+ ObjectPoseClientExample.cpp
+)
+set(HEADERS
+ ObjectPoseClientExample.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6d5c668ecd0ad66bf47036d1b300efa9e814803
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -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::ObjectPoseClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectPoseClientExample.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr ObjectPoseClientExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ return defs;
+ }
+
+ std::string ObjectPoseClientExample::getDefaultName() const
+ {
+ return "ObjectPoseClientExample";
+ }
+
+ void ObjectPoseClientExample::onInitComponent()
+ {
+ }
+
+ void ObjectPoseClientExample::onConnectComponent()
+ {
+ setDebugObserverBatchModeEnabled(true);
+
+ objectProcessingTask = new SimpleRunningTask<>([this]()
+ {
+ this->objectProcessingTaskRun();
+ });
+ objectProcessingTask->start();
+ }
+
+ void ObjectPoseClientExample::onDisconnectComponent()
+ {
+ }
+
+ void ObjectPoseClientExample::onExitComponent()
+ {
+ }
+
+
+ void ObjectPoseClientExample::objectProcessingTaskRun()
+ {
+ CycleUtil cycle(50);
+
+ while (objectProcessingTask && !objectProcessingTask->isStopped())
+ {
+ const objpose::ObjectPoseSeq objectPoses = ObjectPoseClient::getObjectPoses();
+
+ ARMARX_VERBOSE << "Received poses of " << objectPoses.size() << " objects.";
+
+ {
+ setDebugObserverDatafield("NumObjectPoses", objectPoses.size());
+ sendDebugObserverBatch();
+ }
+
+ {
+ // Visualize the objects.
+ viz::Layer layer = arviz.layer("Objects");
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ layer.add(viz::Object(objectPose.objectID.str())
+ .pose(objectPose.objectPoseGlobal)
+ .fileByObjectFinder(objectPose.objectID)
+ .alpha(objectPose.confidence));
+ }
+ arviz.commit(layer);
+ }
+
+ cycle.waitForCycleDuration();
+ }
+ }
+}
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..c49e28f3518755cc492ffa31b67710b005abf7a3
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -0,0 +1,84 @@
+/*
+ * 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::ObjectPoseClientExample
+ * @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 <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+// Include the ClientPlugin
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+
+
+namespace armarx
+{
+
+ /**
+ * @defgroup Component-ObjectPoseClientExample ObjectPoseClientExample
+ * @ingroup RobotAPI-Components
+ *
+ * An example showing how to get object poses from the ObjectPoseStorage.
+ *
+ * @class ObjectPoseClientExample
+ * @ingroup Component-ObjectPoseClientExample
+ * @brief Brief description of class ObjectPoseClientExample.
+ *
+ * Gets and visualizes object poses from the ObjectPoseStorage.
+ */
+ class ObjectPoseClientExample :
+ virtual public armarx::Component
+ , virtual public armarx::DebugObserverComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ // Derive from the client plugin.
+ , virtual public armarx::ObjectPoseClientPluginUser
+
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+
+ void objectProcessingTaskRun();
+
+
+ private:
+
+ armarx::SimpleRunningTask<>::pointer_type objectProcessingTask;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
deleted file mode 100644
index 74da78adf1af125b3ccab6669b1b47a2a9d0d9e8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ /dev/null
@@ -1,45 +0,0 @@
-armarx_component_set_name("ObjectPoseObserver")
-
-
-set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces
- ArmarXGuiComponentPlugins
- RobotAPIArmarXObjects RobotAPIComponentPlugins
- ArViz
-
- ${PROJECT_NAME}Interfaces
-)
-
-set(SOURCES
- ObjectPoseObserver.cpp
-
- detail/Data.cpp
- detail/Decay.cpp
- detail/RobotHeadMovement.cpp
- detail/Visu.cpp
-
- plugins/ObjectPoseProviderPlugin.cpp
- plugins/ObjectPoseClientPlugin.cpp
- plugins/RequestedObjects.cpp
-)
-set(HEADERS
- ObjectPoseObserver.h
-
- detail/Data.h
- detail/Decay.h
- detail/RobotHeadMovement.h
- detail/Visu.h
-
- plugins/ObjectPoseProviderPlugin.h
- plugins/ObjectPoseClientPlugin.h
- plugins/RequestedObjects.h
-)
-
-armarx_add_component("${SOURCES}" "${HEADERS}")
-
-
-# add unit tests
-add_subdirectory(test)
-
-# generate the application
-armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
deleted file mode 100644
index 62a56ebf248e9601ea4e281f84a0d043bef68a61..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ /dev/null
@@ -1,600 +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/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @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 "ObjectPoseObserver.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-
-
-namespace armarx
-{
-
- ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
- armarx::ObserverPropertyDefinitions(prefix)
- {
- }
-
- armarx::PropertyDefinitionsPtr ObjectPoseObserver::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));
-
- defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
- defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
- defs->topic(debugObserver);
-
- calibration.defineProperties(defs, "calibration.");
- data.defineProperties(defs);
- robotHead.defineProperties(defs, "head.");
- visu.defineProperties(defs, "visu.");
-
- return defs;
- }
-
- std::string ObjectPoseObserver::getDefaultName() const
- {
- return "ObjectPoseObserver";
- }
-
- void ObjectPoseObserver::onInitObserver()
- {
- data.setTag(getName());
- data.decay.setTag(getName());
- robotHead.setTag(getName());
- visu.setTag(getName());
-
- usingTopicFromProperty("ObjectPoseTopicName");
- }
-
- void ObjectPoseObserver::onConnectObserver()
- {
- // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
- // So we need to always make sure to guard a call to addRobot
- if (!RobotState::hasRobot("robot"))
- {
- data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
- }
- data.robotStateComponent = getRobotStateComponent();
-
- getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
- robotHead.debugObserver = debugObserver;
- robotHead.fetchDatafields();
-
- visu.arviz = arviz;
- if (!visu.updateTask)
- {
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- visu.updateTask->start();
- }
-
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
-
- void ObjectPoseObserver::onDisconnectComponent()
- {
- }
-
- void ObjectPoseObserver::onExitObserver()
- {
- }
-
-
- void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- {
- updateProviderInfo(providerName, info);
- }
-
-
- void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- {
- ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
- updateObjectPoses(providerName, providedPoses);
- }
-
-
- void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
- {
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
- {
- std::scoped_lock lock(dataMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- data.providers[providerName] = info;
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType),
- "The object type (known or unknown)");
- offerOrUpdateDataField(providerName, "numSupportedObjects", int(info.supportedObjects.size()),
- "Number of requestable objects.");
- }
-
-
- void ObjectPoseObserver::updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses)
- {
- TIMING_START(ReportObjectPoses);
-
- std::optional<IceUtil::Time> discardUpdatesUntil;
- bool discardAll = false;
- {
- std::scoped_lock lock(robotHeadMutex);
- if (robotHead.checkHeadVelocity)
- {
- if (robotHead.isMoving())
- {
- robotHead.movementStarts(robotHead.discardIntervalAfterMoveMS);
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << robotHead.discardUpdatesUntil;
- discardAll = true;
- }
- else if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
- {
- discardAll = true;
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << robotHead.discardUpdatesUntil;
- }
- else
- {
- discardUpdatesUntil = robotHead.discardUpdatesUntil;
- }
- }
- }
- if (debugObserver)
- {
- StringVariantBaseMap map;
- map["Discarding All Updates"] = new Variant(discardAll ? 1.f : 0.f);
- if (discardAll)
- {
- map["Proportion Updated Poses"] = new Variant(0.f);
- }
- debugObserver->setDebugChannel(getName(), map);
- }
-
- if (discardAll)
- {
- return;
- }
-
- {
- std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone(data.robot);
-
- if (data.robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = data.robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
-
- // This stays empty on the first report.
- objpose::ObjectPoseSeq previousPoses;
- if (auto it = data.objectPoses.find(providerName); it != data.objectPoses.end())
- {
- previousPoses = it->second;
- }
-
- // Build new poses.
- objpose::ObjectPoseSeq newObjectPoses;
- int numUpdated = 0;
- for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
- {
- IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
-
- // Check whether we have an old pose for this object.
- std::optional<objpose::ObjectPose> previousPose;
- for (const objpose::ObjectPose& prev : previousPoses)
- {
- if (prev.objectID == fromIce(provided.objectID))
- {
- previousPose = prev;
- }
- }
-
- if (discardUpdatesUntil && timestamp < *discardUpdatesUntil)
- {
- if (previousPose)
- {
- // Keep the old one
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- // Discard the new pose.
- // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
- // << "timestamp " << timestamp << " < " << robotHead.discardUpdatesUntil;
- }
- }
- else if (previousPose && timestamp == previousPose->timestamp)
- {
- // Keep the old one.
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- numUpdated++;
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- newPose.fromProvidedPose(provided, data.robot);
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(newPose.objectID))
- {
- newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
- }
- }
- if (!provided.localOOBB)
- {
- // Try to load oobb from disk.
- newPose.localOOBB = data.getObjectOOBB(newPose.objectID);
- }
- }
- }
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Discarding All Updates", new Variant(discardAll ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(numUpdated) / providedPoses.size()) }
- });
- }
-
- data.objectPoses[providerName] = newObjectPoses;
- handleProviderUpdate(providerName);
-
- TIMING_END_STREAM(ReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "ReportObjectPoses [ms]", new Variant(ReportObjectPoses.toMilliSecondsDouble()) },
- });
- }
- }
- }
-
-
- void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- if (data.providers.count(providerName) == 0)
- {
- data.providers[providerName] = objpose::ProviderInfo();
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectCount", Variant(int(data.objectPoses.at(providerName).size())), "Number of provided object poses.");
- }
-
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPoses(now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPoses() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPoses() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPosesByProvider(providerName, now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
-
- objpose::observer::RequestObjectsOutput ObjectPoseObserver::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- {
- std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- objpose::observer::RequestObjectsOutput output;
-
- auto updateProxy = [&](const std::string & providerName)
- {
- if (proxies.count(providerName) == 0)
- {
- if (auto it = data.providers.find(providerName); it != data.providers.end())
- {
- proxies[providerName] = it->second.proxy;
- }
- else
- {
- ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- proxies[providerName] = nullptr;
- }
- }
- };
-
- if (input.provider.size() > 0)
- {
- providerRequests[input.provider] = input.request;
- updateProxy(input.provider);
- }
- else
- {
- std::scoped_lock lock(dataMutex);
- for (const auto& objectID : input.request.objectIDs)
- {
- bool found = true;
- for (const auto& [providerName, info] : data.providers)
- {
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- {
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
- }
- }
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- }
-
- for (const auto& [providerName, request] : providerRequests)
- {
- if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- {
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- int successful = 0;
- for (const auto& [objectID, result] : providerOutput.results)
- {
- objpose::observer::ObjectRequestResult& res = output.results[objectID];
- res.providerName = providerName;
- res.result = result;
- successful += int(result.success);
- }
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- }
- }
- return output;
- }
-
- objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers;
- }
-
- Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return simox::alg::get_keys(data.providers);
- }
-
- objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.getProviderInfo(providerName);
- }
-
- bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers.count(providerName) > 0;
- }
-
-
- objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.attachObjectToRobotNode(input);
- }
-
- objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachObjectFromRobotNode(input);
- }
-
- objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachAllObjectsFromRobotNodes();
- }
-
-
- objpose::AgentFramesSeq ObjectPoseObserver::getAttachableFrames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { data.robot };
- for (VirtualRobot::RobotPtr agent : agents)
- {
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- }
-
- objpose::SignalHeadMovementOutput
- ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(robotHeadMutex);
- return robotHead.signalHeadMovement(input);
- }
-
-
- void ObjectPoseObserver::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- while (visu.updateTask && !visu.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(visuMutex);
-
- if (visu.enabled)
- {
- TIMING_START(Visu);
-
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
- ObjectFinder objectFinder;
- float minConfidence = -1;
- {
- std::scoped_lock lock(dataMutex);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- data.updateObjectPoses(now);
- objectPoses = data.objectPoses;
- objectFinder = data.objectFinder;
- if (data.decay.enabled)
- {
- minConfidence = data.decay.removeObjectsBelowConfidence;
- }
- }
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, minConfidence, objectFinder);
- arviz.commit(layers);
-
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
- }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
- {
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
- }
-
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- tab.visu.setup(this->visu);
- tab.decay.setup(this->data.decay);
- tab.robotHead.setup(this->robotHead);
-
- VBoxLayout root = {tab.visu.group, tab.decay.group, tab.robotHead.group, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- {
- std::scoped_lock lock(visuMutex);
- tab.visu.update(this->visu);
- }
- {
- std::scoped_lock lock(robotHeadMutex);
- tab.robotHead.update(this->robotHead);
- }
- {
- std::scoped_lock lock(dataMutex);
- tab.decay.update(this->data.decay);
- }
- }
-
- void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-}
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
deleted file mode 100644
index 8fb3369ceaed1f4880037c741bd74f8e2eb5625e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ /dev/null
@@ -1,192 +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/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @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 <mutex>
-
-#include <ArmarXCore/observers/Observer.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-#include <RobotAPI/components/ObjectPoseObserver/detail/Data.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Decay.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Visu.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
-
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
-
-namespace armarx
-{
- /**
- * @class ObjectPoseObserverPropertyDefinitions
- * @brief Property definitions of `ObjectPoseObserver`.
- */
- class ObjectPoseObserverPropertyDefinitions :
- public ObserverPropertyDefinitions
- {
- public:
- ObjectPoseObserverPropertyDefinitions(std::string prefix);
- };
-
-
-
- /**
- * @defgroup Component-ObjectPoseObserver ObjectPoseObserver
- * @ingroup RobotAPI-Components
- * A description of the component ObjectPoseObserver.
- *
- * @class ObjectPoseObserver
- * @ingroup Component-ObjectPoseObserver
- * @brief Brief description of class ObjectPoseObserver.
- *
- * Detailed description of class ObjectPoseObserver.
- */
- class ObjectPoseObserver :
- virtual public Observer
- , virtual public objpose::ObjectPoseObserverInterface
- , virtual public armarx::RobotStateComponentPluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- {
- class Data;
-
- public:
- using RobotState = armarx::RobotStateComponentPluginUser;
-
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
-
-
- // ObjectPoseTopic interface
- public:
- void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
-
- // ObjectPoseObserverInterface interface
- public:
-
- // OBJECT POSES
-
- objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
-
- // PROVIDER INFORMATION
-
- bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
- Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
- objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
-
-
- // REQUESTING
-
- objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
- // ATTACHING
-
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(ICE_CURRENT_ARG) override;
-
- objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
-
- // HEAD MOVEMENT SIGNALS
-
- objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
-
-
-
- // Remote GUI
- void createRemoteGuiTab();
- void RemoteGui_update() override;
-
-
- protected:
-
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- void onInitObserver() override;
- void onConnectObserver() override;
-
- void onDisconnectComponent() override;
- void onExitObserver() override;
-
-
- private:
-
- void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
-
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
- void handleProviderUpdate(const std::string& providerName);
-
- objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
-
-
-
- // Visualization
-
- void visualizeRun();
-
-
- private:
-
- DebugObserverInterfacePrx debugObserver;
-
- objpose::observer::Data data;
- std::mutex dataMutex;
-
- objpose::observer::RobotHeadMovement robotHead;
- std::mutex robotHeadMutex;
-
- objpose::observer::Visu visu;
- std::mutex visuMutex;
-
-
- struct Calibration
- {
- std::string robotNode = "Neck_2_Pitch";
- float offset = 0.0f;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
- };
- Calibration calibration;
-
-
- struct RemoteGuiTab : RemoteGui::Client::Tab
- {
- objpose::observer::Visu::RemoteGui visu;
- objpose::observer::Decay::RemoteGui decay;
- objpose::observer::RobotHeadMovement::RemoteGui robotHead;
- };
- RemoteGuiTab tab;
-
- };
-
-}
-
-#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
deleted file mode 100644
index be89f745d995ff82431d4b2fb285181a607730dc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
+++ /dev/null
@@ -1,359 +0,0 @@
-#include "Data.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <sstream>
-
-
-namespace armarx::objpose::observer
-{
-
- void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- decay.defineProperties(defs, prefix + "decay.");
- }
-
- ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
- {
- bool synchronized = false;
- ObjectPoseSeq result;
-
- for (auto& [providerName, objectPoses] : objectPoses)
- {
- // Update data.
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- for (const ObjectPose& objectPose : objectPoses)
- {
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- }
- return result;
- }
-
-
- ObjectPoseSeq Data::getObjectPosesByProvider(
- const std::string& providerName,
- IceUtil::Time now)
- {
- bool synchronized = false;
-
- // Update data.
- ObjectPoseSeq& objectPoses = this->objectPoses.at(providerName);
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- ObjectPoseSeq result;
- for (const ObjectPose& objectPose : objectPoses)
- {
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- return result;
- }
-
-
- void Data::updateObjectPoses(IceUtil::Time now)
- {
- bool synchronized = false;
- for (auto& [providerName, objectPoses] : objectPoses)
- {
- updateObjectPoses(objectPoses, now, robot, synchronized);
- }
- }
-
-
- void Data::updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- for (ObjectPose& pose : objectPoses)
- {
- updateObjectPose(pose, now, agent, agentSynchronized);
- }
- }
-
-
- void Data::updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- updateAttachement(objectPose, agent, agentSynchronized);
-
- if (decay.enabled)
- {
- decay.updateConfidence(objectPose, now);
- }
- }
-
-
- void Data::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
- {
- if (!objectPose.attachment)
- {
- // Fetch attachment info from internal data structure.
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- // Store it in the objectPose.
- objectPose.attachment = it->second;
- }
- else
- {
- // No attachment, nothing to do.
- return;
- }
- }
- ARMARX_CHECK(objectPose.attachment);
-
- if (!synchronized) // Synchronize only once.
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
- synchronized = true;
- }
- objectPose.updateAttached(agent);
- }
-
-
- ObjectPose* Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
- {
- ObjectPose* pose = nullptr;
- if (!providerName.empty())
- {
- pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
- }
- else
- {
- for (auto& [_, poses] : objectPoses)
- {
- pose = findObjectPoseByID(poses, objectID);
- if (pose)
- {
- break;
- }
- }
- }
- return pose;
- }
-
-
- std::optional<simox::OrientedBoxf> Data::getObjectOOBB(const ObjectID& id)
- {
- return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- try
- {
- return objectInfo->loadOOBB();
- }
- catch (const std::ios_base::failure& e)
- {
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- return std::nullopt;
- }
- }
- else
- {
- return std::nullopt;
- }
- });
- }
-
- ProviderInfo Data::getProviderInfo(const std::string& providerName)
- {
- try
- {
- return providers.at(providerName);
- }
- catch (const std::out_of_range&)
- {
- std::stringstream ss;
- ss << "No provider with name '" << providerName << "' available.\n";
- ss << "Available are:\n";
- for (const auto& [name, _] : providers)
- {
- ss << "- '" << name << "'\n";
- }
- throw std::out_of_range(ss.str());
- }
- }
-
-
-
- AttachObjectToRobotNodeOutput
- Data::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input)
- {
- AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
-
- ObjectID objectID = armarx::fromIce(input.objectID);
-
- if (input.agentName != "" && input.agentName != this->robot->getName())
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
- << "\n(You can leave the agent name empty if there is only one agent.)\n"
- << "\nKnown agents: " << std::vector<std::string> {this->robot->getName()};
- return output;
- }
- VirtualRobot::RobotPtr agent = this->robot;
-
- if (!agent->hasRobotNode(input.frameName))
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
- return output;
- }
- std::string frameName = input.frameName;
-
-
- // Find object pose provider name can be empty.
- ObjectPose* currentObjectPose = this->findObjectPose(objectID, input.providerName);
- if (!currentObjectPose)
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
- return output;
- }
-
- ObjectAttachmentInfo info;
- info.agentName = agent->getName();
- info.frameName = frameName;
-
- if (input.poseInFrame)
- {
- info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
- }
- else
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
-
- armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
- if (frameName == armarx::GlobalFrame)
- {
- info.poseInFrame = framed.toGlobalEigen(this->robot);
- }
- else
- {
- framed.changeFrame(this->robot, info.frameName);
- info.poseInFrame = framed.toEigen();
- }
- }
- this->attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
-
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << currentObjectPose->providerName << "' "
- << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
-
- output.success = true;
- output.attachment = new data::ObjectAttachmentInfo();
- output.attachment->frameName = info.frameName;
- output.attachment->agentName = info.agentName;
- output.attachment->poseInFrame = new Pose(info.poseInFrame);
-
- return output;
- }
-
- DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input)
- {
- ObjectID objectID = armarx::fromIce(input.objectID);
- std::string providerName = input.providerName;
-
- std::optional<ObjectAttachmentInfo> attachment;
- {
- // Remove from latest pose (if it was cached).
- ObjectPose* objectPose = this->findObjectPose(objectID, input.providerName);
- if (objectPose)
- {
- objectPose->attachment = std::nullopt;
- }
-
- if (providerName.empty() && objectPose)
- {
- providerName = objectPose->providerName;
- }
- // Remove from attachment map.
- if (input.providerName.size() > 0)
- {
- auto it = this->attachments.find(std::make_pair(input.providerName, objectID));
- if (it != this->attachments.end())
- {
- attachment = it->second;
- this->attachments.erase(it);
- }
- }
- else
- {
- // Search for entry with matching object ID.
- for (auto it = this->attachments.begin(); it != this->attachments.end(); ++it)
- {
- const ObjectID& id = it->first.second;
- if (id == objectID)
- {
- attachment = it->second;
- this->attachments.erase(it);
- break;
- }
- }
- }
- }
-
- DetachObjectFromRobotNodeOutput output;
- output.wasAttached = bool(attachment);
- if (attachment)
- {
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
- }
- else
- {
- ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
- << "from robot node, but it was not attached.";
- }
-
- return output;
- }
-
- DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes()
- {
- DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = int(this->attachments.size());
-
- // Clear attachment map.
- this->attachments.clear();
-
- // Remove from poses (if it was cached).
- for (auto& [prov, poses] : this->objectPoses)
- {
- for (auto& pose : poses)
- {
- pose.attachment = std::nullopt;
- }
- }
-
- ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
-
- return output;
- }
-
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
deleted file mode 100644
index e825a95340f71e4357392433ae16f8c3fec1ee30..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-#include <optional>
-
-#include <SimoxUtility/caching/CacheMap.h>
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include "Decay.h"
-
-
-namespace armarx::objpose::observer
-{
-
- /**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
- */
- class Data : public armarx::Logging
- {
- public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
-
- ObjectPose* findObjectPose(const ObjectID& objectID, const std::string& providerName = "");
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
-
- ProviderInfo getProviderInfo(const std::string& providerName);
-
-
- AttachObjectToRobotNodeOutput attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input);
- DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input);
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
-
-
- void updateObjectPoses(IceUtil::Time now);
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
-
- /**
- * @brief If the object is attached to a robot node, update it according to the current robot state.
- *
- * If there is no attachement info in `objectPose` itself, the internal data
- * structure `attachments` is queried. If an attachment is found there,
- * it is written into the given `objectPose` (thus, it is "cached" in the
- * info `objectPose`).
- *
- * @param synchronized Indicates whether the agent is already synchronized to the current time.
- */
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
- bool& synchronized) const;
-
-
- public:
-
- RobotStateComponentInterfacePrx robotStateComponent;
- VirtualRobot::RobotPtr robot;
-
- ProviderInfoMap providers;
-
- std::map<std::string, ObjectPoseSeq> objectPoses;
-
- std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments;
-
-
- ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
- /// Decay model.
- Decay decay;
-
- };
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
index 738482cc1c41cb2c8a040ec0aa53206e88c2d427..9eb2a0dac0aadc72946c163c9d79148f799e5aaf 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -1,76 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-namespace armarx::plugins
-{
- class ObjectPoseClientPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
-
- template<class...Ts>
- std::optional<ObjectInfo> findObject(Ts&& ...ts) const
- {
- return _finder.findObject(std::forward<Ts>(ts)...);
- }
-
- template<class...Ts>
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(Ts&& ...ts) const
- {
- return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
- }
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(const std::optional<ObjectInfo>& ts) const
- {
- return _finder.loadManipulationObject(ts);
- }
-
- const ObjectFinder& setObjectFinderPath(const std::string& path);
- const ObjectFinder& getObjectFinder() const;
-
- private:
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- ObjectFinder _finder;
- };
-}
-
-namespace armarx
-{
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseClientPluginUser :
- virtual public ManagedIceObject
- {
- public:
- /// Allow usage like: ObjectPoseClient::getObjects()
- using ObjectPoseClient = ObjectPoseClientPluginUser;
-
- ObjectPoseClientPluginUser();
-
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
- objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
-
- objpose::ObjectPoseSeq getObjectPoses();
-
- plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
- const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
-
- const ObjectFinder& getObjectFinder() const;
- private:
- armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
index 332064711b688d0e81969f3d1cc2ced472d817b1..55f6366e85ef20f50833f0889c6cf4b88c3614d2 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
@@ -1,60 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
-
-
-namespace armarx::plugins
-{
-
- class ObjectPoseProviderPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
- void postOnConnectComponent() override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
-
- private:
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- };
-
-}
-
-
-namespace armarx
-{
-
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseProviderPluginUser :
- virtual public ManagedIceObject
- , virtual public objpose::ObjectPoseProvider
- {
- public:
-
- ObjectPoseProviderPluginUser();
-
- /// Implement to process object requests (empty default implementation).
- objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
- objpose::ObjectPoseTopicPrx objectPoseTopic;
-
- private:
-
- armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
-
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
deleted file mode 100644
index 878921745f8b8cfd10cc67cbcc594d2199c84ea3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ObjectPoseObserver)
-
-armarx_add_test(ObjectPoseObserverTest ObjectPoseObserverTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
deleted file mode 100644
index 903d51bfb830771b2b70b0ef4bdacc414ecee3ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
+++ /dev/null
@@ -1,68 +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/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @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
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseObserver
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <iostream>
-
-using namespace armarx;
-
-
-BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
-{
- Eigen::Vector3f pos(-100, -200, -300);
- Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
- Eigen::Vector3f extents(40, 50, 60);
-
- armarx::objpose::Box box;
- box.position = new Vector3(pos);
- box.orientation = new Quaternion(ori);
- box.extents = new Vector3(extents);
-
-
- const float prec = 1e-3;
-
- simox::OrientedBoxf oobb;
- armarx::objpose::fromIce(box, oobb);
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
-
- armarx::objpose::Box boxOut;
- armarx::objpose::toIce(boxOut, oobb);
-
- Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
- Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
- Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
-
- ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
- ARMARX_CHECK(oriOut.isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
-}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
index fae15ee0afc1b1577bf808383a75faaa2d814ec3..1395fc02f564792a7ffe7e0963264f5c0ac6fb9e 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
@@ -2,10 +2,10 @@ armarx_component_set_name("ObjectPoseProviderExample")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
- # RobotAPICore RobotAPIInterfaces
- # RobotAPIComponentPlugins # for ArViz and other plugins
- RobotAPIArmarXObjects ObjectPoseObserver
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # RobotAPI
+ RobotAPI::ArmarXObjects
)
set(SOURCES
@@ -18,16 +18,6 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(ObjectPoseProviderExample PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-# add unit tests
-add_subdirectory(test)
#generate the application
armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index 590bad8d596e06666405a3a631b78a790263cd10..f759697348f61927af7df5c9297e2081b0c1db91 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -30,16 +30,13 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
namespace armarx
{
- ObjectPoseProviderExamplePropertyDefinitions::ObjectPoseProviderExamplePropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ObjectPoseProviderExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index aac11a5f1c9fd6d8a920a85166495e29346e6772..7ed814eb06c219f3280f1d5d4e9473ef56344fb0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -31,26 +31,14 @@
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
// Include the ProviderPlugin
-#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h>
-#include <RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
namespace armarx
{
- /**
- * @class ObjectPoseProviderExamplePropertyDefinitions
- * @brief Property definitions of `ObjectPoseProviderExample`.
- */
- class ObjectPoseProviderExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- ObjectPoseProviderExamplePropertyDefinitions(std::string prefix);
- };
-
-
/**
* @defgroup Component-ObjectPoseProviderExample ObjectPoseProviderExample
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
deleted file mode 100644
index e65a2f31bc9150164b5e5ed6fc0e93227f5176ce..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ${ARMARX_COMPONENT_LIB_NAME})
-
-armarx_add_test(ObjectPoseProviderExampleTest ObjectPoseProviderExampleTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index 148c9c6bde8dc1ff41cefe0d91df429bd0790081..526448e132f6e282b3d3f14fd39132fe32017ebb 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -1,8 +1,5 @@
# memory servers
-add_subdirectory(server/ExampleMemory)
-add_subdirectory(server/GeneralPurposeMemory)
-add_subdirectory(server/RobotStateMemory)
-add_subdirectory(server/SkillsMemory)
+add_subdirectory(server)
# memory server addons
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..83fd40e2dee27f8459c028474b01f178218f752f
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_subdirectory(ExampleMemory)
+add_subdirectory(GeneralPurposeMemory)
+add_subdirectory(ObjectMemory)
+add_subdirectory(RobotSensorMemory)
+add_subdirectory(SkillsMemory)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index a3c7f7bc8c31e0c6c8ff2af96ebd36412521ee00..8bd030bb8883e73910b182d7aad66feda0dab96f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -60,16 +60,16 @@ namespace armarx
void ExampleMemory::onInitComponent()
{
- workingmemory.name() = p.memoryName;
+ workingMemory.name() = p.memoryName;
// Usually, the memory server will specify a number of core segments with a specific aron type.
- workingmemory.addCoreSegment("ExampleData", armem::example::ExampleData::toInitialAronType());
+ workingMemory.addCoreSegment("ExampleData", armem::example::ExampleData::toInitialAronType());
// For illustration purposes, we add more segments (without types).
bool trim = true;
p.core.defaultCoreSegments = simox::alg::split(p.core._defaultSegmentsStr, ",", trim);
p.core._defaultSegmentsStr.clear();
- workingmemory.addCoreSegments(p.core.defaultCoreSegments);
+ workingMemory.addCoreSegments(p.core.defaultCoreSegments);
}
void ExampleMemory::onConnectComponent()
@@ -121,8 +121,8 @@ namespace armarx
using namespace armarx::RemoteGui::Client;
{
- std::scoped_lock lock(workingmemoryMutex);
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingmemory);
+ std::scoped_lock lock(workingMemoryMutex);
+ tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory);
}
VBoxLayout root = {tab.memoryGroup, VSpacer()};
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index bb1988874e0d7a57f8f6e0511e8137643ca9dfec..b269993f79bd5c5d39c590e1359cb5790fd9b4dc 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -36,14 +36,6 @@
using armarx::armem::example::ExampleData;
namespace armem = armarx::armem;
-/*
-BOOST_AUTO_TEST_CASE(test_ExampleData)
-{
- ExampleData data;
- BOOST_CHECK(true);
-}
-*/
-
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
@@ -55,5 +47,5 @@ BOOST_AUTO_TEST_CASE(test_ExampleData_type)
armem::wm::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
armem::wm::ProviderSegment& prov = core.addProviderSegment("Provider");
- BOOST_CHECK_EQUAL(core.aronType, prov.aronType);
+ BOOST_CHECK_EQUAL(core.aronType(), prov.aronType());
}
diff --git a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
index 8247ab6f9d1ff22baecefb6146a14ea6866c8d93..3b17f4fc879dfb49fd1b816f8e4b0e450e3f926f 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
@@ -50,7 +50,7 @@ namespace armarx
void GeneralPurposeMemory::onInitComponent()
{
- workingmemory.name() = memoryName;
+ workingMemory.name() = memoryName;
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
index 28ade93141eef153f7ed6dd8972cc0adda2b9f3b..1c90c244bf872fdc4931e0b9067a92c189d9b694 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
@@ -2,12 +2,16 @@ armarx_component_set_name("ObjectMemory")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarXGui
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ # RobotAPI
+ RobotAPI::ComponentPlugins
+ RobotAPI::armem_objects
- ${IVT_LIBRARIES}
+ # This project
+ ${PROJECT_NAME}Interfaces
)
set(SOURCES
@@ -19,8 +23,11 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#generate the application
-armarx_generate_and_add_component_executable()
-
+# add unit tests
+# add_subdirectory(test)
+# generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server::obj
+)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 93977f9d9a04181f9b22c24d0c6639571d591412..18ccb6875d1611e05cf20a8fa714dd293df995e6 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -15,78 +15,146 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "ObjectMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/algorithm/string.h>
+namespace armarx::armem::server::obj
+{
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+ const std::string ObjectMemory::defaultMemoryName = "Object";
-namespace armarx
-{
- ObjectMemory::ObjectMemory()
- {
- }
armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+
+ // Offer
+ defs->topic(debugObserver);
+
+ // Subscribe
+ defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
+
+ // Use
+ // defs->component(kinematicUnitObserver); // Optional dependency.
+ defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
+ "Name of the kinematic unit observer.");
+
+ const std::string prefix = "mem.";
+
+ workingMemory.name() = defaultMemoryName;
+ defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
+ classSegment.defineProperties(defs, prefix + "cls.");
+ instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+
return defs;
}
+ ObjectMemory::ObjectMemory() :
+ server::ComponentPluginUser(),
+ instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ classSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
+ {
+ }
std::string ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
-
void ObjectMemory::onInitComponent()
{
- memory.name() = memoryName;
+ workingMemory.name() = defaultMemoryName;
+
+ instance::SegmentAdapter::init();
+
+ try
+ {
+ classSegment.init();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init class segment for unknown reason.";
+ }
}
-
void ObjectMemory::onConnectComponent()
{
+ // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
+ // So we need to always make sure to guard a call to addRobot
+ const std::string robotKey = "robot";
+ VirtualRobot::RobotPtr robot = RobotState::hasRobot(robotKey)
+ ? RobotState::getRobot(robotKey)
+ : RobotState::addRobot(robotKey, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ robotStateComponent = RobotState::getRobotStateComponent();
+
+ getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+
+ instance::SegmentAdapter::connect(
+ robotStateComponent, robot,
+ kinematicUnitObserver,
+ ArVizComponentPluginUser::arviz,
+ debugObserver
+ );
+ classSegment.connect(
+ ArVizComponentPluginUser::arviz
+ );
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
-
void ObjectMemory::onDisconnectComponent()
{
}
-
void ObjectMemory::onExitComponent()
{
}
-
- // WRITING
- armem::data::AddSegmentsResult ObjectMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ void ObjectMemory::createRemoteGuiTab()
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
- return result;
+ using namespace armarx::RemoteGui::Client;
+
+ tab.instance.setup(*this);
+ tab.clazz.setup(classSegment);
+
+ HBoxLayout segments =
+ {
+ tab.instance.group,
+ tab.clazz.group
+ };
+ VBoxLayout root =
+ {
+ segments,
+ VSpacer()
+ };
+ RemoteGui_createTab(Component::getName(), root, &tab);
}
-
- armem::data::CommitResult ObjectMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ void ObjectMemory::RemoteGui_update()
{
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
- return result;
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ tab.instance.update(*this);
+ tab.clazz.update(classSegment);
}
-
- // READING
- // Inherited from Plugin
-
-
}
+
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index ca194dfbf6c2f0c3b527a2170aa1308f883780ae..65bd9c472828faa2b8b7f58679c4922c1aef389d 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -15,25 +15,37 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
+#include <memory>
+#include <mutex>
-#include <ArmarXCore/core/Component.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-namespace armarx
+
+namespace armarx::armem::server::obj
{
+
/**
* @defgroup Component-ObjectMemory ObjectMemory
* @ingroup RobotAPI-Components
@@ -46,41 +58,66 @@ namespace armarx
* Detailed description of class ObjectMemory.
*/
class ObjectMemory :
- virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ virtual public Component
+
+ , virtual public armarx::armem::server::ObjectMemoryInterface
+ , virtual public armarx::armem::server::ComponentPluginUser
+ , virtual public armarx::armem::server::obj::instance::SegmentAdapter
+
+ , virtual public armarx::RobotStateComponentPluginUser
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
{
public:
+
+ using RobotState = armarx::RobotStateComponentPluginUser;
+
+ static const std::string defaultMemoryName;
+
+
+ public:
+
ObjectMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- // WritingInterface interface
+
public:
- armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- protected:
- /// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
-
- /// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ // Remote GUI
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
private:
- std::string memoryName = "ObjectMemory";
- bool addCoreSegmentOnUsage = true;
+
+ DebugObserverInterfacePrx debugObserver;
+ RobotStateComponentInterfacePrx robotStateComponent;
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+
+ clazz::Segment classSegment;
+
+
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ instance::SegmentAdapter::RemoteGui instance;
+ clazz::Segment::RemoteGui clazz;
+ };
+ RemoteGuiTab tab;
+
};
+
}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5f5ecdf18b6c4abf1aa08213ee23c920491cf01b
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ObjectMemory)
+
+armarx_add_test(ObjectMemoryTest ObjectMemoryTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
similarity index 71%
rename from source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
rename to source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
index d173c24a7a110c93d94a95d3df6b7c1d1a6d5e78..40329162efb831da65f1dcd1671dc458e5d6d1b0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
@@ -13,25 +13,28 @@
* 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::ObjectPoseProviderExample
+ * @package RobotAPI::ArmarXObjects::ObjectMemory
* @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
*/
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseProviderExample
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectMemory
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h>
+#include <RobotAPI/components/ObjectMemory/ObjectMemory.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
-{
- armarx::ObjectPoseProviderExample instance;
+using namespace armarx;
- BOOST_CHECK_EQUAL(true, true);
+
+BOOST_AUTO_TEST_CASE(test_ObjectMemory)
+{
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 2ab7912eea596a199a1df4270e728eec362dc2f7..3298e26721c98aceec2034c5b585ab2de44d4c25 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -89,7 +89,7 @@ namespace armarx
ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
- workingmemory.name() = workingMemoryName;
+ workingMemory.name() = workingMemoryName;
}
@@ -125,7 +125,7 @@ namespace armarx
void RobotStateMemory::setupRobotUnitSegment()
{
ARMARX_INFO << "Adding core segment " << robotUnitCoreSegmentName;
- workingmemory.addCoreSegments({robotUnitCoreSegmentName});
+ workingMemory.addCoreSegments({robotUnitCoreSegmentName});
ARMARX_INFO << "Adding provider segment " << robotUnitCoreSegmentName << "/" << robotUnitProviderSegmentName;
armem::data::AddSegmentInput input;
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 4efdcbff7b43ca8a168ba17d0970b1b6a5781d10..295e2c6aee2010162c6919af97141f28989b6040 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -50,7 +50,7 @@ namespace armarx
void SkillsMemory::onInitComponent()
{
- workingmemory.name() = memoryName;
+ workingMemory.name() = memoryName;
}
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
index 3e94dc5e5c3e13bd866c92c428f5d619317806ac..819331b5f27f58c036bb867e8a17dda8a0c8b977 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
@@ -15,39 +15,43 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
- <layout class="QHBoxLayout" name="ltmControlWidgetLayout">
- <property name="spacing">
- <number>6</number>
- </property>
+ <layout class="QHBoxLayout" name="topRowLayout">
<item>
- <spacer name="horizontalSpacer_2">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
+ <layout class="QHBoxLayout" name="updateWidgetLayout">
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
- </layout>
- </item>
- <item>
- <layout class="QHBoxLayout" name="updateWidgetLayout">
<item>
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
+ <layout class="QHBoxLayout" name="ltmControlWidgetLayout">
+ <property name="spacing">
+ <number>6</number>
</property>
- </spacer>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
</layout>
</item>
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
index f313b6951c2f201251787ec6a6e6b4b5b0026e4c..83d1d8743a3b6ed0ebfeca5c704c96334b22108a 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
@@ -41,9 +41,9 @@ namespace armarx
widget.setupUi(getWidget());
viewer = std::make_unique<MemoryViewer>(
- widget.ltmControlWidgetLayout,
widget.updateWidgetLayout,
+ widget.ltmControlWidgetLayout,
widget.memoryGroupBox,
widget.treesLayout,
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
index f1ccdd08058d73791e7a832a8dcb8ee645a3c75b..855c88fb33c089de09965fd69b34c0537ec8f956 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
@@ -27,7 +27,7 @@ set(GUI_UIS
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
SimpleConfigDialog
- ObjectPoseObserver
+ RobotAPI::armem_objects
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 80404797fa4176aff345a9acc6946024ff04574b..dffe7711debc728d596f5ecd7cd851df2527e120 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -99,14 +99,14 @@ namespace armarx
return "MemoryX.ObjectPoseGui";
}
- static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseObserver";
+ static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseStorage";
QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::objpose::ObjectPoseObserverInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "ObjectPoseObserver"});
+ configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "*"});
}
return qobject_cast<QDialog*>(configDialog);
}
@@ -115,26 +115,26 @@ namespace armarx
{
if (configDialog)
{
- objectPoseObserverName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
+ ObjectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
}
}
void ObjectPoseGuiWidgetController::onInitComponent()
{
- if (!objectPoseObserverName.empty())
+ if (!ObjectPoseStorageName.empty())
{
- usingProxy(objectPoseObserverName);
+ usingProxy(ObjectPoseStorageName);
}
}
void ObjectPoseGuiWidgetController::onConnectComponent()
{
- if (!objectPoseObserverName.empty())
+ if (!ObjectPoseStorageName.empty())
{
- getProxy(objectPoseObserver, objectPoseObserverName);
+ getProxy(ObjectPoseStorage, ObjectPoseStorageName);
}
- this->attachableFrames = objectPoseObserver->getAttachableFrames();
+ this->attachableFrames = ObjectPoseStorage->getAttachableFrames();
std::sort(attachableFrames.begin(), attachableFrames.end(), [](const auto & lhs, const auto & rhs)
{
return lhs.agent < rhs.agent;
@@ -147,7 +147,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::onDisconnectComponent()
{
- objectPoseObserver = nullptr;
+ ObjectPoseStorage = nullptr;
}
void ObjectPoseGuiWidgetController::updateTab()
@@ -166,7 +166,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::updateObjectsTab()
{
- if (!objectPoseObserver)
+ if (!ObjectPoseStorage)
{
// Probably disconnected.
ARMARX_VERBOSE << "No object pose observer.";
@@ -175,7 +175,7 @@ namespace armarx
IceUtil::Time start = IceUtil::Time::now();
ARMARX_VERBOSE << "Getting object poses...";
- const objpose::data::ObjectPoseSeq objectPosesIce = objectPoseObserver->getObjectPoses();
+ const objpose::data::ObjectPoseSeq objectPosesIce = ObjectPoseStorage->getObjectPoses();
ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
<< "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
@@ -262,7 +262,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::updateRequestTab()
{
- if (!objectPoseObserver)
+ if (!ObjectPoseStorage)
{
// Probably disconnected.
ARMARX_VERBOSE << "No object pose observer.";
@@ -270,7 +270,7 @@ namespace armarx
}
IceUtil::Time start = IceUtil::Time::now();
- objpose::ProviderInfoMap availableProvidersInfo = objectPoseObserver->getAvailableProvidersInfo();
+ objpose::ProviderInfoMap availableProvidersInfo = ObjectPoseStorage->getAvailableProvidersInfo();
ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
<< "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
@@ -393,8 +393,17 @@ namespace armarx
input.agentName = agentName;
input.frameName = frameName;
- objpose::AttachObjectToRobotNodeOutput output = objectPoseObserver->attachObjectToRobotNode(input);
- ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ try
+ {
+ objpose::AttachObjectToRobotNodeOutput output = ObjectPoseStorage->attachObjectToRobotNode(input);
+ ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to attach object '" << input.objectID << "' to robot node '"
+ << input.frameName << "' of agent '" << input.agentName << "'."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
@@ -405,8 +414,16 @@ namespace armarx
input.providerName = providerName.toStdString();
input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
- objpose::DetachObjectFromRobotNodeOutput output = objectPoseObserver->detachObjectFromRobotNode(input);
- ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ try
+ {
+ objpose::DetachObjectFromRobotNodeOutput output = ObjectPoseStorage->detachObjectFromRobotNode(input);
+ ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to detach object '" << input.objectID << "' from a robot node."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::requestSelectedObjects()
@@ -446,7 +463,7 @@ namespace armarx
ARMARX_INFO << "Requesting " << request.request.objectIDs.size() << " objects for "
<< request.request.relativeTimeoutMS << " ms.";
- objpose::observer::RequestObjectsOutput output = objectPoseObserver->requestObjects(request);
+ objpose::observer::RequestObjectsOutput output = ObjectPoseStorage->requestObjects(request);
int successful = 0;
for (const auto& [id, result] : output.results)
{
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
index 84a0e6e964ffdd2cead4f144627f478f41c94d10..5e9910bd8185cb13adbd54435758ee871dbd8460 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -29,7 +29,7 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
namespace armarx
@@ -115,8 +115,8 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
- std::string objectPoseObserverName;
- armarx::objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+ std::string ObjectPoseStorageName;
+ armarx::objpose::ObjectPoseStorageInterfacePrx ObjectPoseStorage;
objpose::AgentFramesSeq attachableFrames;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 3db350c9e2d32e79214795948a5c42670f7b5eb6..b2913fb4c0f39035f94b284a52ac7f32295f73b8 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -36,7 +36,7 @@ set(SLICE_FILES
ArmarXObjects/ArmarXObjectsTypes.ice
objectpose/object_pose_types.ice
- objectpose/ObjectPoseObserver.ice
+ objectpose/ObjectPoseStorageInterface.ice
objectpose/ObjectPoseProvider.ice
units/MultiHandUnitInterface.ice
@@ -122,6 +122,9 @@ set(SLICE_FILES
armem/server/ReadingMemoryInterface.ice
armem/server/WritingMemoryInterface.ice
+ # Special Servers
+ armem/server/ObjectMemoryInterface.ice
+
armem/mns.ice
armem/mns/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..32e120ae8fdabfbb7b49545a86623f281b6ee159
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
@@ -0,0 +1,54 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Rainer Kartmann
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
+
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+
+ interface ObjectInstanceSegmentInterface extends
+ armarx::objpose::ObjectPoseStorageInterface
+ {
+
+ };
+
+ interface ObjectMemoryInterface extends
+ MemoryInterface
+ , ObjectInstanceSegmentInterface
+ {
+
+ };
+
+ };
+ };
+};
+
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
similarity index 89%
rename from source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
rename to source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
index 068cba470558e175be65190fb8666c80d6cc2d5a..3097f6c7596fd3fdf82aedd9d974712081cf503c 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
@@ -27,6 +27,8 @@
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
#include <RobotAPI/interface/objectpose/object_pose_types.ice>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.ice>
@@ -87,12 +89,27 @@ module armarx
{
string providerName;
armarx::data::ObjectID objectID;
+
+ /**
+ * @brief If true, the object will stay at the position before
+ * detaching until it is provided again.
+ */
+ bool commitAttachedPose = true;
};
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
bool wasAttached;
};
+
+ struct DetachAllObjectsFromRobotNodesInput
+ {
+ /**
+ * @brief If true, the objects will stay at the position before
+ * detaching until they are provided again.
+ */
+ bool commitAttachedPose = true;
+ }
struct DetachAllObjectsFromRobotNodesOutput
{
/// Number of objects that have been detached.
@@ -135,7 +152,8 @@ module armarx
long discardUpdatesUntilMilliSeconds = -1;
};
- interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
+ interface ObjectPoseStorageInterface extends
+ ObjectPoseTopic
{
// Object poses
@@ -160,7 +178,7 @@ module armarx
/// Detach an attached object from a robot node.
DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
/// Detach all objects from robot nodes.
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(DetachAllObjectsFromRobotNodesInput input);
AgentFramesSeq getAttachableFrames();
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 66618485d73ea91819fca88d6951875fba892bcd..385da35e387d04b21aaea7e4845b073f9c977ee4 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -85,7 +85,7 @@ module armarx
class ObjectAttachmentInfo;
- /// An object pose as stored by the ObjectPoseObserver.
+ /// An object pose as stored by the ObjectPoseStorage.
struct ObjectPose
{
/// Name of the providing component.
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 57878bbbdea5f9724bc16002ffaf570aa7659298..31fd00c7edcc840eb0a629580f3b28058698bae9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -4,7 +4,11 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
- RobotAPICore
+ # ArmarXGui
+ RemoteGui
+ # RobotAPI
+ RobotAPI::Core
+ aroncommon
)
set(LIB_FILES
@@ -15,7 +19,12 @@ set(LIB_FILES
json_conversions.cpp
ice_conversions.cpp
- aron_conversions.cpp
+ aron_conversions/armarx.cpp
+ aron_conversions/objpose.cpp
+
+ plugins/ObjectPoseProviderPlugin.cpp
+ plugins/ObjectPoseClientPlugin.cpp
+ plugins/RequestedObjects.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -27,17 +36,29 @@ set(LIB_HEADERS
json_conversions.h
ice_conversions.h
+
aron_conversions.h
+ aron_conversions/armarx.h
+ aron_conversions/objpose.h
+
+ plugins/ObjectPoseProviderPlugin.h
+ plugins/ObjectPoseClientPlugin.h
+ plugins/RequestedObjects.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
+ aron/ObjectID.xml
+ aron/ObjectNames.xml
aron/ObjectPose.xml
+ aron/ObjectType.xml
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index ed8d14d913785406841f24602ffdb17496d4a2ef..ebcf8c9766a5ca1d8204a1aac7c0f317c2173371 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -24,6 +24,11 @@ namespace armarx
packageDataDir.clear();
}
+ std::string ObjectFinder::getPackageName() const
+ {
+ return packageName;
+ }
+
void ObjectFinder::init() const
{
if (packageDataDir.empty())
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index f6e9831593f4a8714eb4f3c461016bce063cadaa..6e50a9d9cbeb1c82b7b928b2289f4b74799d70c7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -32,6 +32,8 @@ namespace armarx
void setPath(const std::string& path);
+ std::string getPackageName() const;
+
std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
std::optional<ObjectInfo> findObject(const ObjectID& id) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 8c70a93aac07dae51aa5b82014038e009f2a8cd1..348a5b25419813a22717f71c92d0c3484042361c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -25,6 +25,11 @@ namespace armarx
{
}
+ void ObjectInfo::setLogError(bool enabled)
+ {
+ this->_logError = enabled;
+ }
+
std::string ObjectInfo::package() const
{
return _packageName;
@@ -100,7 +105,10 @@ namespace armarx
}
catch (const std::exception& e)
{
- ARMARX_ERROR << e.what();
+ if (_logError)
+ {
+ ARMARX_ERROR << e.what();
+ }
return std::nullopt;
}
@@ -113,10 +121,10 @@ namespace armarx
simox::AxisAlignedBoundingBox aabb(min, max);
static const float prec = 1e-4f;
- ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose();
+ ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose() << "\n" << id();
return aabb;
}
@@ -130,7 +138,10 @@ namespace armarx
}
catch (const std::exception& e)
{
- ARMARX_ERROR << e.what();
+ if (_logError)
+ {
+ ARMARX_ERROR << e.what();
+ }
return std::nullopt;
}
@@ -147,9 +158,21 @@ namespace armarx
ori.col(2) * extents(2));
static const float prec = 1e-3f;
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec) << oobb.center().transpose() << "\n" << pos.transpose();
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori;
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+ ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori << "\n" << id();
+ // If the object is too large, the above precision will trigger a false positive.
+ if (extents.squaredNorm() < 1e5f * 1e5f)
+ {
+ ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec)
+ << VAROUT(oobb.center().transpose())
+ << "\n" << VAROUT(pos.transpose())
+ << "\n" << VAROUT(extents.norm())
+ << "\n" << VAROUT(id());
+ ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec)
+ << VAROUT(oobb.dimensions().transpose())
+ << "\n" << VAROUT(extents.transpose())
+ << "\n" << VAROUT(extents.norm())
+ << "\n" << VAROUT(id());
+ }
return oobb;
}
@@ -176,12 +199,18 @@ namespace armarx
}
catch (const nlohmann::json::exception& e)
{
- ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ if (_logError)
+ {
+ ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ }
return std::nullopt;
}
catch (const std::exception& e)
{
- ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ if (_logError)
+ {
+ ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ }
return std::nullopt;
}
@@ -201,12 +230,18 @@ namespace armarx
if (!fs::is_regular_file(simoxXML().absolutePath))
{
- ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ if (_logError)
+ {
+ ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ }
result = false;
}
if (!fs::is_regular_file(wavefrontObj().absolutePath))
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ if (_logError)
+ {
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ }
result = false;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 10dbd5a08fd04c710cec3c41030357cc70c2243d..634944e4cc52379977f784c370f48af1e84ed28d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -50,6 +50,9 @@ namespace armarx
virtual ~ObjectInfo() = default;
+ void setLogError(bool enabled);
+
+
std::string package() const;
std::string dataset() const;
@@ -114,6 +117,8 @@ namespace armarx
ObjectID _id;
+ bool _logError = true;
+
};
std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 36379f97d0d3d047058dbb9a9d57e9517a0caa71..556ac04be8c43a686e9d8f0e839fdb5bb13f7daa 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -20,12 +20,12 @@ namespace armarx::objpose
{
std::string frameName;
std::string agentName;
- Eigen::Matrix4f poseInFrame;
+ Eigen::Matrix4f poseInFrame = Eigen::Matrix4f::Identity();
};
/**
- * @brief An object pose as stored by the ObjectPoseObserver.
+ * @brief An object pose as stored by the ObjectPoseStorage.
*/
struct ObjectPose
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dac70d6259875d8bc393180b415c2e4e8bb13451
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml
@@ -0,0 +1,22 @@
+<!--
+The ARON DTO of armarx::ObjectID.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::ObjectID">
+ <ObjectChild key='dataset'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='className'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='instanceName'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml
new file mode 100644
index 0000000000000000000000000000000000000000..eb99bea9cd5a6cef00552cb17239ed8b715f5b8d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml
@@ -0,0 +1,23 @@
+<!--
+Recognized and spoken names of a known object.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::ObjectNames">
+ <ObjectChild key="recognizedNames">
+ <List>
+ <String/>
+ </List>
+ </ObjectChild>
+ <ObjectChild key="spokenNames">
+ <List>
+ <String/>
+ </List>
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index 441ec9b60ac7cd6cee5be2be62cf3e391b117862..e9701dc24c9a891449c9025a514322eaa615b52f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -1,30 +1,22 @@
-<!--This class contains the data structure for ObjectPose -->
+<!--
+ARON DTO of armarx::objpose::ObjectPose.
+-->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
</CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ </AronIncludes>
<GenerateTypes>
- <IntEnum name="armarx::objpose::aron::ObjectTypes">
- <EnumValue key="ANY_OBJECT" value="0" />
- <EnumValue key="KNOWN_OBJECT" value="1" />
- <EnumValue key="UNKNOWN_OBJECT" value="2" />
- </IntEnum>
-
- <Object name="armarx::objpose::aron::ObjectID">
- <ObjectChild key='dataset'>
- <string />
- </ObjectChild>
- <ObjectChild key='className'>
- <string />
- </ObjectChild>
- <ObjectChild key='instanceName'>
- <string />
- </ObjectChild>
- </Object>
-
- <Object name="armarx::objpose::aron::ObjectAttachmentInfo">
+ <Object name="armarx::objpose::arondto::ObjectAttachmentInfo">
<ObjectChild key='frameName'>
<string />
</ObjectChild>
@@ -36,27 +28,18 @@
</ObjectChild>
</Object>
- <Object name="armarx::objpose::aron::OrientedBoundingBox">
- <ObjectChild key='centerPose'>
- <Pose />
- </ObjectChild>
- <ObjectChild key='extends'>
- <Position />
- </ObjectChild>
- </Object>
-
- <Object name='armarx::objpose::aron::ObjectPose'>
+ <Object name='armarx::objpose::arondto::ObjectPose'>
<ObjectChild key='providerName'>
<string />
</ObjectChild>
<ObjectChild key='objectType'>
- <armarx::objpose::aron::ObjectTypes />
+ <armarx::objpose::arondto::ObjectType />
</ObjectChild>
<ObjectChild key='objectID'>
- <armarx::objpose::aron::ObjectID />
+ <armarx::arondto::ObjectID />
</ObjectChild>
<ObjectChild key='objectPoseRobot'>
@@ -78,15 +61,18 @@
<ObjectChild key='robotConfig'>
<Dict>
<Float />
- </Dict>
+ </Dict>
</ObjectChild>
<ObjectChild key='robotPose'>
<Pose />
</ObjectChild>
+ <ObjectChild key='attachmentValid'>
+ <bool />
+ </ObjectChild>
<ObjectChild key='attachment'>
- <armarx::objpose::aron::ObjectAttachmentInfo />
+ <armarx::objpose::arondto::ObjectAttachmentInfo />
</ObjectChild>
<ObjectChild key='confidence'>
@@ -96,9 +82,12 @@
<ObjectChild key='timestamp'>
<Time />
</ObjectChild>
-
+
+ <ObjectChild key='localOOBBValid'>
+ <bool />
+ </ObjectChild>
<ObjectChild key='localOOBB'>
- <armarx::objpose::aron::OrientedBoundingBox />
+ <simox::arondto::OrientedBox />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0a40adf4027fca325d01ac02e13deb1cf0911d5d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
@@ -0,0 +1,16 @@
+<!--
+ARON DTO of armarx::objpose::ObjectTypeEnum.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <IntEnum name="armarx::objpose::arondto::ObjectType">
+ <EnumValue key="AnyObject" value="0" />
+ <EnumValue key="KnownObject" value="1" />
+ <EnumValue key="UnknownObject" value="2" />
+ </IntEnum>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
deleted file mode 100644
index 71398d59dc8d00018bcc0d4fb5c1c7cfb6010465..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-#include "aron_conversions.h"
-
-// STL
-#include <stdexcept>
-#include <string>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// Simox
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-// RobotAPI
-// TODO: ice dependency! header should be removed.
-#include <RobotAPI/interface/objectpose/object_pose_types.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
-
-namespace armarx::objpose
-{
-
- ObjectID fromAron(const aron::ObjectID& id)
- {
- return ObjectID(id.dataset, id.className, id.instanceName);
- }
-
- ObjectTypeEnum fromAron(const aron::ObjectTypes& objectType)
- {
- using AronObjectType = aron::ObjectTypes::__ImplEnum;
-
- ObjectTypeEnum e{};
-
- switch (objectType.value)
- {
- case AronObjectType::ANY_OBJECT:
- e = ObjectTypeEnum::AnyObject;
- break;
- case AronObjectType::KNOWN_OBJECT:
- e = ObjectTypeEnum::KnownObject;
- break;
- case AronObjectType::UNKNOWN_OBJECT:
- e = ObjectTypeEnum::UnknownObject;
- break;
- }
-
- return e;
- }
-
- simox::OrientedBoxf fromAron(const aron::OrientedBoundingBox& obb)
- {
- return simox::OrientedBoxf(obb.centerPose, obb.extends);
- }
-
- ObjectAttachmentInfo fromAron(const aron::ObjectAttachmentInfo& info)
- {
- return
- {
- .frameName = info.frameName,
- .agentName = info.agentName,
- .poseInFrame = info.poseInFrame
- };
- }
-
-
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose)
- {
- objectPose.providerName = aronObjectPose.providerName;
-
- objectPose.objectType = fromAron(aronObjectPose.objectType);
-
- objectPose.objectID = fromAron(aronObjectPose.objectID);
-
- objectPose.objectPoseRobot = aronObjectPose.objectPoseRobot;
- objectPose.objectPoseGlobal = aronObjectPose.objectPoseGlobal;
- objectPose.objectPoseOriginal = aronObjectPose.objectPoseOriginal;
- objectPose.objectPoseOriginalFrame = aronObjectPose.objectPoseOriginalFrame;
-
- objectPose.robotConfig = aronObjectPose.robotConfig;
- objectPose.robotPose = aronObjectPose.robotPose;
-
- objectPose.attachment = fromAron(aronObjectPose.attachment);
-
- objectPose.confidence = aronObjectPose.confidence;
-
- objectPose.timestamp = IceUtil::Time::microSeconds(aronObjectPose.timestamp);
-
- objectPose.localOOBB = fromAron(aronObjectPose.localOOBB);
-
- }
-
-
- aron::ObjectID toAron(const ObjectID& id)
- {
- aron::ObjectID aronId;
-
- aronId.className = id.className();
- aronId.dataset = id.dataset();
- aronId.instanceName = id.instanceName();
-
- return aronId;
- }
-
- aron::ObjectTypes toAron(const ObjectTypeEnum& objectType)
- {
- aron::ObjectTypes ot{};
-
- switch (objectType)
- {
- case ObjectTypeEnum::AnyObject:
- ot = aron::ObjectTypes::ANY_OBJECT;
- break;
- case ObjectTypeEnum::KnownObject:
- ot = aron::ObjectTypes::KNOWN_OBJECT;
- break;
- case ObjectTypeEnum::UnknownObject:
- ot = aron::ObjectTypes::UNKNOWN_OBJECT;
- break;
- }
-
- return ot;
- }
-
- aron::ObjectAttachmentInfo toAron(const ObjectAttachmentInfo& info)
- {
- aron::ObjectAttachmentInfo aronInfo;
-
- aronInfo.agentName = info.agentName;
- aronInfo.frameName = info.frameName;
- aronInfo.poseInFrame = info.poseInFrame;
-
- return aronInfo;
- }
-
- aron::OrientedBoundingBox toAron(const simox::OrientedBoxf& box)
- {
- aron::OrientedBoundingBox aronBox;
- aronBox.centerPose = box.transformation();
- aronBox.extends = box.dimensions();
- return aronBox;
- }
-
- template <typename T>
- auto toAron(const std::optional<T>& p)
- {
- if (p)
- {
- return toAron(p.value());
- }
-
- // TODO(fabian.reister): handle optional properly
- // current fallback: default c'tor
- return decltype(toAron(p.value()))();
- }
-
-
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose)
- {
- aronObjectPose.providerName = objectPose.providerName;
-
- aronObjectPose.objectType = toAron(objectPose.objectType);
-
- aronObjectPose.objectID = toAron(objectPose.objectID);
-
- aronObjectPose.objectPoseRobot = objectPose.objectPoseRobot;
- aronObjectPose.objectPoseGlobal = objectPose.objectPoseGlobal;
- aronObjectPose.objectPoseOriginal = objectPose.objectPoseOriginal;
- aronObjectPose.objectPoseOriginalFrame = objectPose.objectPoseOriginalFrame;
-
- aronObjectPose.robotConfig = objectPose.robotConfig;
- aronObjectPose.robotPose = objectPose.robotPose;
-
- aronObjectPose.attachment = toAron(objectPose.attachment);
-
- aronObjectPose.confidence = objectPose.confidence;
-
- aronObjectPose.timestamp = objectPose.timestamp.toMicroSeconds();
-
- aronObjectPose.localOOBB = toAron(objectPose.localOOBB);
- }
-
-} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
index 50e7fc9412d1a30ac166386a29723547ef04cbda..f9862b6bbd1bc1f3d61771e92ef77b35dde1df83 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
@@ -1,16 +1,4 @@
#pragma once
-
-namespace armarx::objpose
-{
- struct ObjectPose;
-
- namespace aron
- {
- struct ObjectPose;
- }
-
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose);
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose);
-
-} // namespace armarx::objpose
\ No newline at end of file
+#include "aron_conversions/armarx.h"
+#include "aron_conversions/objpose.h"
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd081a06376a3ada55ea163fc20af229c3cc92b2
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
@@ -0,0 +1,27 @@
+#include "armarx.h"
+
+
+void armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
+{
+ bo = ObjectID(dto.dataset, dto.className, dto.instanceName);
+}
+void armarx::toAron(arondto::ObjectID& dto, const ObjectID& bo)
+{
+ dto.dataset = bo.dataset();
+ dto.className = bo.className();
+ dto.instanceName = bo.instanceName();
+}
+
+
+void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackageFileLocation& bo)
+{
+ bo.package = dto.package;
+ bo.relativePath = dto.path;
+ bo.absolutePath = "";
+}
+
+void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackageFileLocation& bo)
+{
+ dto.package = bo.package;
+ dto.path = bo.relativePath;
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
new file mode 100644
index 0000000000000000000000000000000000000000..08c60f45a3e27d1372f77d620a3e5efffd5c7400
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h> // For PackageFileLocation
+#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
+
+
+namespace armarx
+{
+ void fromAron(const arondto::PackagePath& dto, PackageFileLocation& bo);
+ void toAron(arondto::PackagePath& dto, const PackageFileLocation& bo);
+
+ void fromAron(const arondto::ObjectID& dto, ObjectID& bo);
+ void toAron(arondto::ObjectID& dto, const ObjectID& bo);
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2de8a3da0096d0c2edf7d103c944edbb681eb56d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -0,0 +1,138 @@
+#include "objpose.h"
+
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
+{
+ bo.frameName = dto.frameName;
+ bo.agentName = dto.agentName;
+ bo.poseInFrame = dto.poseInFrame;
+}
+void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
+{
+ dto.frameName = bo.frameName;
+ dto.agentName = bo.agentName;
+ dto.poseInFrame = bo.poseInFrame;
+}
+
+void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ObjectType::AnyObject:
+ bo = ObjectTypeEnum::AnyObject;
+ return;
+ case arondto::ObjectType::KnownObject:
+ bo = ObjectTypeEnum::KnownObject;
+ return;
+ case arondto::ObjectType::UnknownObject:
+ bo = ObjectTypeEnum::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+}
+void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo)
+{
+ switch (bo)
+ {
+ case ObjectTypeEnum::AnyObject:
+ dto.value = arondto::ObjectType::AnyObject;
+ return;
+ case ObjectTypeEnum::KnownObject:
+ dto.value = arondto::ObjectType::KnownObject;
+ return;
+ case ObjectTypeEnum::UnknownObject:
+ dto.value = arondto::ObjectType::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+}
+
+
+void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
+{
+ bo.providerName = dto.providerName;
+
+ fromAron(dto.objectType, bo.objectType);
+ fromAron(dto.objectID, bo.objectID);
+
+ bo.objectPoseRobot = dto.objectPoseRobot;
+ bo.objectPoseGlobal = dto.objectPoseGlobal;
+ bo.objectPoseOriginal = dto.objectPoseOriginal;
+ bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+
+ bo.robotConfig = dto.robotConfig;
+ bo.robotPose = dto.robotPose;
+
+ if (dto.attachmentValid)
+ {
+ bo.attachment = ObjectAttachmentInfo();
+ fromAron(dto.attachment, *bo.attachment);
+ }
+ else
+ {
+ bo.attachment = std::nullopt;
+ }
+
+ bo.confidence = dto.confidence;
+
+ bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+
+ if (dto.localOOBBValid)
+ {
+ bo.localOOBB = simox::OrientedBoxf();
+ fromAron(dto.localOOBB, *bo.localOOBB);
+ }
+ else
+ {
+ bo.localOOBB = std::nullopt;
+ }
+}
+
+
+void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
+{
+ dto.providerName = bo.providerName;
+
+ toAron(dto.objectType, bo.objectType);
+ toAron(dto.objectID, bo.objectID);
+
+ dto.objectPoseRobot = bo.objectPoseRobot;
+ dto.objectPoseGlobal = bo.objectPoseGlobal;
+ dto.objectPoseOriginal = bo.objectPoseOriginal;
+ dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+
+ dto.robotConfig = bo.robotConfig;
+ dto.robotPose = bo.robotPose;
+
+ if (bo.attachment)
+ {
+ dto.attachmentValid = true;
+ toAron(dto.attachment, *bo.attachment);
+ }
+ else
+ {
+ dto.attachmentValid = false;
+ toAron(dto.attachment, ObjectAttachmentInfo());
+ }
+
+ dto.confidence = bo.confidence;
+
+ dto.timestamp = bo.timestamp.toMicroSeconds();
+
+ if (bo.localOOBB)
+ {
+ dto.localOOBBValid = true;
+ toAron(dto.localOOBB, *bo.localOOBB);
+ }
+ else
+ {
+ dto.localOOBBValid = false;
+ toAron(dto.localOOBB, simox::OrientedBoxf());
+ }
+}
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
new file mode 100644
index 0000000000000000000000000000000000000000..afe31e500b7ba9a39b76dc33b0eeda0916cc849b
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+
+
+namespace armarx::objpose
+{
+ void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
+ void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
+
+ void fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo);
+ void toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo);
+
+ void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
+ void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
similarity index 73%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 95b046d87274b5562b43d1237cf2ffff9144f0aa..b12572855775c7c9f3f11f4f2d571da7ff36a14f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -8,8 +8,8 @@ namespace armarx::plugins
{
properties->defineOptionalProperty<std::string>(
makePropertyName(PROPERTY_NAME),
- "ObjectPoseObserver",
- "Name of the object pose observer.");
+ "ObjectMemory",
+ "Name of the object memory.");
}
}
@@ -20,12 +20,12 @@ namespace armarx::plugins
void ObjectPoseClientPlugin::preOnConnectComponent()
{
- parent<ObjectPoseClientPluginUser>().objectPoseObserver = createObjectPoseObserver();
+ parent<ObjectPoseClientPluginUser>().objectPoseStorage = createObjectPoseStorage();
}
- objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPlugin::createObjectPoseObserver()
+ objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPlugin::createObjectPoseStorage()
{
- return parent<Component>().getProxyFromProperty<objpose::ObjectPoseObserverInterfacePrx>(makePropertyName(PROPERTY_NAME));
+ return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(makePropertyName(PROPERTY_NAME));
}
const ObjectFinder& ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
@@ -47,19 +47,19 @@ namespace armarx
addPlugin(plugin);
}
- objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPluginUser::createObjectPoseObserver()
+ objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPluginUser::createObjectPoseStorage()
{
- return plugin->createObjectPoseObserver();
+ return plugin->createObjectPoseStorage();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
{
- if (!objectPoseObserver)
+ if (!objectPoseStorage)
{
ARMARX_WARNING << "No object pose observer.";
return {};
}
- return objpose::fromIce(objectPoseObserver->getObjectPoses());
+ return objpose::fromIce(objectPoseStorage->getObjectPoses());
}
plugins::ObjectPoseClientPlugin& ObjectPoseClientPluginUser::getObjectPoseClientPlugin()
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..996f699acd35dd50afa095f3bdaa66d9d2a7e061
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -0,0 +1,85 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::plugins
+{
+ class ObjectPoseClientPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+ objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
+
+ template<class...Ts>
+ std::optional<ObjectInfo> findObject(Ts&& ...ts) const
+ {
+ return _finder.findObject(std::forward<Ts>(ts)...);
+ }
+
+ template<class...Ts>
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(Ts&& ...ts) const
+ {
+ return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
+ }
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(const std::optional<ObjectInfo>& ts) const
+ {
+ return _finder.loadManipulationObject(ts);
+ }
+
+ const ObjectFinder& setObjectFinderPath(const std::string& path);
+ const ObjectFinder& getObjectFinder() const;
+
+
+ private:
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
+
+ ObjectFinder _finder;
+
+ };
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseClientPluginUser :
+ virtual public ManagedIceObject
+ {
+ public:
+
+ /// Allow usage like: ObjectPoseClient::getObjects()
+ using ObjectPoseClient = ObjectPoseClientPluginUser;
+
+ ObjectPoseClientPluginUser();
+
+ objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
+ objpose::ObjectPoseStorageInterfacePrx objectPoseStorage;
+
+ objpose::ObjectPoseSeq getObjectPoses();
+
+ plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
+ const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
+
+ const ObjectFinder& getObjectFinder() const;
+
+
+ private:
+
+ armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f7c87ef560cad2e4a0b20257387f52b8bd6835d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
+
+
+namespace armarx::plugins
+{
+
+ class ObjectPoseProviderPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+ void postOnConnectComponent() override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+
+ private:
+
+ static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+
+ };
+
+}
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseProviderPluginUser :
+ virtual public ManagedIceObject
+ , virtual public objpose::ObjectPoseProvider
+ {
+ public:
+
+ ObjectPoseProviderPluginUser();
+
+ /// Implement to process object requests (empty default implementation).
+ objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+ objpose::ObjectPoseTopicPrx objectPoseTopic;
+
+
+ private:
+
+ armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 06c12b18a610b8b05998daea8bc2100cb6839078..1778ba42d2c67c1d6967485ee5fbcaa05241488a 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -19,6 +19,7 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
+add_subdirectory(armem_objects)
add_subdirectory(armem_robot_localization)
add_subdirectory(armem_robot_mapping)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 8e61ccd8332b8ef0c8cc4fc891dd2548a68a5682..a4ca9b08ad6d3b0937294c5a9050adb6680abfcc 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -44,5 +44,7 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(RobotAPI::ComponentPlugins ALIAS RobotAPIComponentPlugins)
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 9e8250ffc08f91c8f515f81c803da0830a6dee6d..be30284443c02aa0c4a90225855df5b2d753f6aa 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -11,23 +11,19 @@ set(LIBS
${LIBBSONCXX_LIBRARIES}
)
-set(ARON_FILES
- core/aron/MemoryID.xml
-)
-
set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
core/SuccessHeader.cpp
core/Time.cpp
-
core/ice_conversions.cpp
+ core/aron_conversions.cpp
core/base/detail/MemoryItem.cpp
core/base/detail/MaxHistorySize.cpp
core/base/detail/MemoryContainerBase.cpp
core/base/detail/EntityContainerBase.cpp
- core/base/detail/TypedEntityContainerBase.cpp
+ core/base/detail/AronTyped.cpp
core/base/CoreSegmentBase.cpp
core/base/EntityBase.cpp
@@ -37,15 +33,14 @@ set(LIB_FILES
core/base/ProviderSegmentBase.cpp
core/workingmemory/ice_conversions.cpp
- core/workingmemory/detail/MemoryContainer.cpp
- core/workingmemory/detail/EntityContainer.cpp
- core/workingmemory/detail/TypedEntityContainer.cpp
+ core/workingmemory/detail/CopyWithoutData.cpp
core/workingmemory/CoreSegment.cpp
core/workingmemory/Entity.cpp
core/workingmemory/EntityInstance.cpp
core/workingmemory/EntitySnapshot.cpp
core/workingmemory/Memory.cpp
core/workingmemory/ProviderSegment.cpp
+ core/workingmemory/Visitor.cpp
core/workingmemory/ice_conversions.cpp
core/longtermmemory/CoreSegment.cpp
@@ -55,6 +50,7 @@ set(LIB_FILES
core/longtermmemory/Memory.cpp
core/longtermmemory/ProviderSegment.cpp
+ core/diskmemory/TypeIO.cpp
core/diskmemory/CoreSegment.cpp
core/diskmemory/Entity.cpp
core/diskmemory/EntityInstance.cpp
@@ -111,6 +107,9 @@ set(LIB_HEADERS
core/MemoryID.h
core/SuccessHeader.h
core/Time.h
+ core/aron_conversions.h
+ core/ice_conversions.h
+ core/ice_conversions_templates.h
core/error.h
core/error/ArMemError.h
@@ -119,7 +118,7 @@ set(LIB_HEADERS
core/base/detail/MaxHistorySize.h
core/base/detail/MemoryContainerBase.h
core/base/detail/EntityContainerBase.h
- core/base/detail/TypedEntityContainerBase.h
+ core/base/detail/AronTyped.h
core/base/CoreSegmentBase.h
core/base/EntityBase.h
@@ -128,15 +127,14 @@ set(LIB_HEADERS
core/base/MemoryBase.h
core/base/ProviderSegmentBase.h
- core/workingmemory/detail/MemoryContainer.h
- core/workingmemory/detail/EntityContainer.h
- core/workingmemory/detail/TypedEntityContainer.h
+ core/workingmemory/detail/CopyWithoutData.h
core/workingmemory/CoreSegment.h
core/workingmemory/Entity.h
core/workingmemory/EntityInstance.h
core/workingmemory/EntitySnapshot.h
core/workingmemory/Memory.h
core/workingmemory/ProviderSegment.h
+ core/workingmemory/Visitor.h
core/workingmemory/ice_conversions.h
core/longtermmemory/CoreSegment.h
@@ -147,7 +145,7 @@ set(LIB_HEADERS
core/longtermmemory/ProviderSegment.h
core/longtermmemory/mongodb/MongoDBConnectionManager.h
- core/diskmemory/detail/TypedEntityContainer.h
+ core/diskmemory/TypeIO.h
core/diskmemory/CoreSegment.h
core/diskmemory/Entity.h
core/diskmemory/EntityInstance.h
@@ -155,8 +153,8 @@ set(LIB_HEADERS
core/diskmemory/Memory.h
core/diskmemory/ProviderSegment.h
- core/ice_conversions.h
core/ice_conversions_templates.h
+ core/ice_conversions.h
client.h
client/ComponentPlugin.h
@@ -213,8 +211,7 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${LIB_NAME}
ARON_FILES
- ${ARON_FILES}
- #ENABLE_DEBUG_INFO
+ aron/MemoryID.xml
)
diff --git a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
similarity index 55%
rename from source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
rename to source/RobotAPI/libraries/armem/aron/MemoryID.xml
index 11d0fee395edca2c597dc1ea9de53dfbd7f9eae9..996259c63c7b66028bd8b853714fa3147fd64ed6 100644
--- a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
+++ b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
@@ -1,23 +1,23 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::armem::aron::MemoryID'>
- <ObjectChild key='memoryName'>
+ <Object name="armarx::armem::arondto::MemoryID">
+ <ObjectChild key="memoryName">
<string />
</ObjectChild>
- <ObjectChild key='coreSegmentName'>
+ <ObjectChild key="coreSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='providerSegmentName'>
+ <ObjectChild key="providerSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='entityName'>
+ <ObjectChild key="entityName">
<string />
</ObjectChild>
- <ObjectChild key='timestamp'>
- <long />
+ <ObjectChild key="timestamp">
+ <Time />
</ObjectChild>
- <ObjectChild key='instanceIndex'>
+ <ObjectChild key="instanceIndex">
<int />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5e6d3e0d10cb7b3f1ed51bcc8f665cb48ec3cbf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -0,0 +1,22 @@
+#include "aron_conversions.h"
+
+
+void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
+{
+ bo.memoryName = dto.memoryName;
+ bo.coreSegmentName = dto.coreSegmentName;
+ bo.providerSegmentName = dto.providerSegmentName;
+ bo.entityName = dto.entityName;
+ bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+ bo.instanceIndex = dto.instanceIndex;
+}
+
+void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
+{
+ dto.memoryName = bo.memoryName;
+ dto.coreSegmentName = bo.coreSegmentName;
+ dto.providerSegmentName = bo.providerSegmentName;
+ dto.entityName = bo.entityName;
+ dto.timestamp = bo.timestamp.toMicroSeconds();
+ dto.instanceIndex = bo.instanceIndex;
+}
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d105fd9761f3ff1d8eaa58fabf4aa257719478b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
+
+namespace armarx::armem
+{
+ void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
+ void toAron(arondto::MemoryID& dto, const MemoryID& bo);
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 8e99598c18ab63291b723e7ec4c1ecdae0047d4f..d28d09c74f3f324fab3d9f31dc37548a567ea95e 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -4,8 +4,9 @@
#include <string>
#include "ProviderSegmentBase.h"
+#include "detail/AronTyped.h"
+#include "detail/EntityContainerBase.h"
#include "detail/MaxHistorySize.h"
-#include "detail/TypedEntityContainerBase.h"
namespace armarx::armem::base
@@ -16,27 +17,51 @@ namespace armarx::armem::base
*/
template <class _ProviderSegmentT, class _Derived>
class CoreSegmentBase :
- virtual public detail::TypedEntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>,
- virtual public detail::MaxHistorySize
+ public detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>,
+ public detail::MaxHistorySize,
+ public detail::AronTyped
{
- using Base = detail::TypedEntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>;
+ using Base = detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>;
public:
+
+ using typename Base::DerivedT;
+ using typename Base::ContainerT;
+
using ProviderSegmentT = _ProviderSegmentT;
using EntityT = typename ProviderSegmentT::EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
- CoreSegmentBase& operator=(const CoreSegmentBase& other)
+
+ public:
+
+ CoreSegmentBase()
+ {
+ }
+ CoreSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ CoreSegmentBase(MemoryID().withCoreSegmentName(name), aronType)
+ {
+ }
+ CoreSegmentBase(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ CoreSegmentBase(parentID.withProviderSegmentName(name), aronType)
+ {
+ }
+ CoreSegmentBase(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ Base(id),
+ AronTyped(aronType)
{
- other._copySelf(*this);
- return *this;
}
- using Base::id;
+ CoreSegmentBase(const CoreSegmentBase& other) = default;
+ CoreSegmentBase(CoreSegmentBase&& other) = default;
+ CoreSegmentBase& operator=(const CoreSegmentBase& other) = default;
+ CoreSegmentBase& operator=(CoreSegmentBase&& other) = default;
+
+
inline const std::string& name() const
{
- return id.coreSegmentName;
+ return this->id().coreSegmentName;
}
inline std::string& name()
{
@@ -44,31 +69,30 @@ namespace armarx::armem::base
}
- using Base::container;
- inline const std::map<std::string, _ProviderSegmentT>& providerSegments() const
+ inline const std::map<std::string, ProviderSegmentT>& providerSegments() const
{
- return container;
+ return this->_container;
}
- inline std::map<std::string, _ProviderSegmentT>& providerSegments()
+ inline std::map<std::string, ProviderSegmentT>& providerSegments()
{
- return const_cast<std::map<std::string, _ProviderSegmentT>&>(const_cast<const CoreSegmentBase*>(this)->providerSegments());
+ return const_cast<std::map<std::string, ProviderSegmentT>&>(const_cast<const CoreSegmentBase*>(this)->providerSegments());
}
bool hasProviderSegment(const std::string& name) const
{
- return container.count(name) > 0;
+ return this->_container.count(name) > 0;
}
- _ProviderSegmentT& getProviderSegment(const std::string& name)
+ ProviderSegmentT& getProviderSegment(const std::string& name)
{
- return const_cast<_ProviderSegmentT&>(const_cast<const CoreSegmentBase*>(this)->getProviderSegment(name));
+ return const_cast<ProviderSegmentT&>(const_cast<const CoreSegmentBase*>(this)->getProviderSegment(name));
}
- const _ProviderSegmentT& getProviderSegment(const std::string& name) const
+ const ProviderSegmentT& getProviderSegment(const std::string& name) const
{
- auto it = this->container.find(name);
- if (it != container.end())
+ auto it = this->_container.find(name);
+ if (it != this->_container.end())
{
return it->second;
}
@@ -78,19 +102,39 @@ namespace armarx::armem::base
}
}
- using Base::_checkContainerName;
+ using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.coreSegmentName, this->getKeyString());
+ this->_checkContainerName(id.coreSegmentName, this->getKeyString());
return getProviderSegment(id.providerSegmentName).getEntity(id);
}
+ const EntityT* findEntity(const MemoryID& id) const override
+ {
+ this->_checkContainerName(id.coreSegmentName, this->getKeyString());
+ if (id.hasProviderSegmentName())
+ {
+ return getProviderSegment(id.providerSegmentName).findEntity(id);
+ }
+ else
+ {
+ for (const auto& [_, providerSegment] : this->_container)
+ {
+ if (auto entity = providerSegment.findEntity(id))
+ {
+ return entity;
+ }
+ }
+ return nullptr;
+ }
+ }
+
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.coreSegmentName, this->name());
+ this->_checkContainerName(update.entityID.coreSegmentName, this->name());
- auto it = this->container.find(update.entityID.providerSegmentName);
- if (it != container.end())
+ auto it = this->_container.find(update.entityID.providerSegmentName);
+ if (it != this->_container.end())
{
return it->second.update(update);
}
@@ -106,20 +150,20 @@ namespace armarx::armem::base
* @param providerSegmentType The provider type. If nullptr, the core segment type is used.
* @return The added provider segment.
*/
- _ProviderSegmentT& addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType = nullptr)
+ ProviderSegmentT& addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType = nullptr)
{
- aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType;
- return addProviderSegment(_ProviderSegmentT(name, type));
+ aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType();
+ return addProviderSegment(ProviderSegmentT(name, type));
}
/// Copy and insert a provider segment.
- _ProviderSegmentT& addProviderSegment(const _ProviderSegmentT& providerSegment)
+ ProviderSegmentT& addProviderSegment(const ProviderSegmentT& providerSegment)
{
return addProviderSegment(ProviderSegment(providerSegment));
}
/// Move and insert a provider segment.
- _ProviderSegmentT& addProviderSegment(_ProviderSegmentT&& providerSegment)
+ ProviderSegmentT& addProviderSegment(ProviderSegmentT&& providerSegment)
{
if (hasProviderSegment(providerSegment.name()))
{
@@ -127,9 +171,9 @@ namespace armarx::armem::base
providerSegment.getLevelName(), providerSegment.name(), getLevelName(), this->getKeyString());
}
- auto it = container.emplace(providerSegment.name(), std::move(providerSegment)).first;
- it->second.id.setCoreSegmentID(id);
- it->second.setMaxHistorySize(maxHistorySize);
+ auto it = this->_container.emplace(providerSegment.name(), std::move(providerSegment)).first;
+ it->second.id().setCoreSegmentID(this->id());
+ it->second.setMaxHistorySize(_maxHistorySize);
return it->second;
}
@@ -142,21 +186,20 @@ namespace armarx::armem::base
void setMaxHistorySize(long maxSize) override
{
MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, seg] : container)
+ for (auto& [name, seg] : this->_container)
{
seg.setMaxHistorySize(maxSize);
}
}
- using Base::size;
virtual bool equalsDeep(const CoreSegmentBase& other) const
{
//std::cout << "CoreSegment::equalsDeep" << std::endl;
- if (size() != other.size())
+ if (this->size() != other.size())
{
return false;
}
- for (const auto& [key, provider] : container)
+ for (const auto& [key, provider] : this->_container)
{
if (not other.hasProviderSegment(key))
{
@@ -180,7 +223,14 @@ namespace armarx::armem::base
return this->name();
}
- public:
+
+ protected:
+
+ virtual void _copySelf(DerivedT& other) const override
+ {
+ Base::_copySelf(other);
+ other.aronType() = _aronType;
+ }
};
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 2f7788696e46a52f5252547575205a3a37fcd4f0..39dcc12c92902bad4c7a2e0c9cb7385de8e0a40c 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,10 +5,13 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include "../../core/Time.h"
#include "../../core/MemoryID.h"
#include "EntitySnapshotBase.h"
+#include "detail/MaxHistorySize.h"
#include "detail/MemoryContainerBase.h"
@@ -36,29 +39,49 @@ namespace armarx::armem::base
*/
template <class _EntitySnapshotT, class _Derived>
class EntityBase :
- virtual public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>
+ public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
+ public detail::MaxHistorySize
{
using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
public:
+
+ using typename Base::DerivedT;
+ using typename Base::ContainerT;
+
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
- EntityBase& operator=(const EntityBase& other)
+
+ public:
+
+ EntityBase()
+ {
+ }
+ EntityBase(const std::string& name, const MemoryID& parentID = {}) :
+ EntityBase(parentID.withEntityName(name))
{
- other._copySelf(*this);
- return *this;
}
+ EntityBase(const MemoryID& id) :
+ Base(id)
+ {
+ }
+
+
+ EntityBase(const EntityBase& other) = default;
+ EntityBase(EntityBase&& other) = default;
+ EntityBase& operator=(const EntityBase& other) = default;
+ EntityBase& operator=(EntityBase&& other) = default;
+
- using Base::size;
virtual bool equalsDeep(const EntityBase& other) const
{
//std::cout << "Entity::equalsDeep" << std::endl;
- if (size() != other.size())
+ if (this->size() != other.size())
{
return false;
}
- for (const auto& [key, snapshot] : container)
+ for (const auto& [key, snapshot] : this->_container)
{
if (not other.hasSnapshot(key))
{
@@ -73,10 +96,9 @@ namespace armarx::armem::base
}
- using Base::id;
inline const std::string& name() const
{
- return id.entityName;
+ return this->id().entityName;
}
inline std::string& name()
{
@@ -84,14 +106,13 @@ namespace armarx::armem::base
}
- using Base::container;
- inline const std::map<Time, _EntitySnapshotT>& history() const
+ inline const std::map<Time, EntitySnapshotT>& history() const
{
- return container;
+ return this->_container;
}
- inline std::map<Time, _EntitySnapshotT>& history()
+ inline std::map<Time, EntitySnapshotT>& history()
{
- return const_cast<std::map<Time, _EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
+ return const_cast<std::map<Time, EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
}
@@ -100,7 +121,7 @@ namespace armarx::armem::base
*/
bool hasSnapshot(Time time) const
{
- return container.count(time) > 0;
+ return this->_container.count(time) > 0;
}
/**
@@ -117,7 +138,7 @@ namespace armarx::armem::base
*/
std::vector<Time> getTimestamps() const
{
- return simox::alg::get_keys(container);
+ return simox::alg::get_keys(this->_container);
}
@@ -129,30 +150,30 @@ namespace armarx::armem::base
* @throws `armem::error::MissingEntry` If there is no such entry.
* @throws `armem::error::MissingData` If the entry has no data.
*/
- _EntitySnapshotT& getSnapshot(Time time)
+ EntitySnapshotT& getSnapshot(Time time)
{
- return const_cast<_EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
}
- const _EntitySnapshotT& getSnapshot(Time time) const
+ const EntitySnapshotT& getSnapshot(Time time) const
{
- auto it = container.find(time);
- if (it != container.end())
+ auto it = this->_container.find(time);
+ if (it != this->_container.end())
{
return it->second;
}
else
{
- throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id.str());
+ throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
}
}
- _EntitySnapshotT& getSnapshot(const MemoryID& id)
+ EntitySnapshotT& getSnapshot(const MemoryID& id)
{
- return const_cast<_EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(id));
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(id));
}
- const _EntitySnapshotT& getSnapshot(const MemoryID& id) const
+ const EntitySnapshotT& getSnapshot(const MemoryID& id) const
{
checkEntityName(id.entityName);
return getSnapshot(id.timestamp);
@@ -164,12 +185,12 @@ namespace armarx::armem::base
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
* @throw `armem::error::MissingData` If the latest snapshot has no data.
*/
- _EntitySnapshotT& getLatestSnapshot()
+ EntitySnapshotT& getLatestSnapshot()
{
- return const_cast<_EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
}
- const _EntitySnapshotT& getLatestSnapshot() const
+ const EntitySnapshotT& getLatestSnapshot() const
{
return getLatestItem().second;
}
@@ -183,12 +204,12 @@ namespace armarx::armem::base
virtual MemoryID update(const EntityUpdate& update)
{
checkEntityName(update.entityID.entityName);
- id = update.entityID;
+ this->id() = update.entityID;
- _EntitySnapshotT* snapshot;
+ EntitySnapshotT* snapshot;
- auto it = container.find(update.timeCreated);
- if (it == container.end())
+ auto it = this->_container.find(update.timeCreated);
+ if (it == this->_container.end())
{
// Insert into history.
snapshot = &addSnapshot(update.timeCreated);
@@ -201,7 +222,7 @@ namespace armarx::armem::base
// Update entry.
snapshot->update(update);
- return snapshot->id;
+ return snapshot->id();
}
/**
@@ -209,21 +230,21 @@ namespace armarx::armem::base
* @param snapshot The snapshot.
* @return The stored snapshot.
*/
- _EntitySnapshotT& addSnapshot(const _EntitySnapshotT& snapshot)
+ EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
{
- return addSnapshot(_EntitySnapshotT(snapshot));
+ return addSnapshot(EntitySnapshotT(snapshot));
}
- _EntitySnapshotT& addSnapshot(_EntitySnapshotT&& snapshot)
+ EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
{
- auto it = container.emplace(snapshot.time(), std::move(snapshot)).first;
- it->second.id.setEntityID(id);
+ auto it = this->_container.emplace(snapshot.time(), std::move(snapshot)).first;
+ it->second.id().setEntityID(this->id());
return it->second;
}
- _EntitySnapshotT& addSnapshot(const Time& timestamp)
+ EntitySnapshotT& addSnapshot(const Time& timestamp)
{
- return addSnapshot(_EntitySnapshotT(timestamp));
+ return addSnapshot(EntitySnapshotT(timestamp));
}
@@ -232,57 +253,34 @@ namespace armarx::armem::base
*
* The current history is truncated if necessary.
*/
- void setMaxHistorySize(long maxSize)
+ void setMaxHistorySize(long maxSize) override
{
- this->maxHistorySize = maxSize;
+ MaxHistorySize::setMaxHistorySize(maxSize);
truncateHistoryToSize();
}
std::string getKeyString() const override
{
- return id.entityName;
+ return this->id().entityName;
}
std::string getLevelName() const override
{
return "entity";
}
- virtual _Derived copy() const override
- {
- _Derived d;
- this->_copySelf(d);
- return d;
- }
-
- virtual _Derived copyEmpty() const override
- {
- _Derived d;
- this->_copySelfEmpty(d);
- return d;
- }
-
- protected:
- virtual void _copySelf(_Derived& other) const override
- {
- Base::_copySelf(other);
- }
-
- virtual void _copySelfEmpty(_Derived& other) const override
- {
- Base::_copySelfEmpty(other);
- }
private:
+
/// If maximum size is set, ensure `history`'s is not higher.
void truncateHistoryToSize()
{
- if (maxHistorySize >= 0)
+ if (_maxHistorySize >= 0)
{
- while (container.size() > size_t(maxHistorySize))
+ while (this->_container.size() > size_t(_maxHistorySize))
{
- container.erase(container.begin());
+ this->_container.erase(this->_container.begin());
}
- ARMARX_CHECK_LESS_EQUAL(container.size(), maxHistorySize);
+ ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
}
}
@@ -299,24 +297,15 @@ namespace armarx::armem::base
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- const typename std::map<Time, _EntitySnapshotT>::value_type& getLatestItem() const
+ const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
{
- if (container.empty())
+ if (this->_container.empty())
{
throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
}
- return *container.rbegin();
+ return *this->_container.rbegin();
}
- public:
- /**
- * @brief Maximum size of `history`
- *
- * If negative, the size of `history` is not limited.
- */
- long maxHistorySize = -1;
- // ToDo: Add max age;
- // ToDo in future: keep/remove predicate
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index 872521f2a18e94e3b9c4f8affc22322677c18ffc..aab70193435c7eec54b9e2127e7c57528ebba842 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -11,35 +11,39 @@ namespace armarx::armem::base
/**
* @brief Data of a single entity instance.
*/
- template <class _Derived>
+ template <class _DerivedT>
class EntityInstanceBase :
- virtual public detail::MemoryItem
+ public detail::MemoryItem
{
using Base = detail::MemoryItem;
public:
- EntityInstanceBase& operator=(const EntityInstanceBase& other)
+
+ using DerivedT = _DerivedT;
+
+
+ public:
+
+ EntityInstanceBase()
{
- //other._copySelf(*this);
- return *this;
}
-
- using Base::id;
- inline int& index()
+ EntityInstanceBase(int index, const MemoryID& parentID = {}) :
+ EntityInstanceBase(parentID.withInstanceIndex(index))
{
- return id.instanceIndex;
}
- inline int index() const
+ EntityInstanceBase(const MemoryID& id) :
+ Base(id)
{
- return id.instanceIndex;
}
- // Copying
- virtual _Derived copy() const
+
+ inline int& index()
{
- _Derived d;
- this->_copySelf(d);
- return d;
+ return id().instanceIndex;
+ }
+ inline int index() const
+ {
+ return id().instanceIndex;
}
/**
@@ -49,7 +53,16 @@ namespace armarx::armem::base
*/
virtual void update(const EntityUpdate& update, int index) = 0;
- virtual bool equalsDeep(const _Derived& other) const = 0;
+
+ virtual bool equalsDeep(const DerivedT& other) const = 0;
+
+ virtual DerivedT copy() const
+ {
+ DerivedT d;
+ this->_copySelf(d);
+ return d;
+ }
+
std::string getLevelName() const override
{
@@ -61,11 +74,14 @@ namespace armarx::armem::base
return std::to_string(index());
}
+
protected:
- virtual void _copySelf(_Derived& other) const
+
+ virtual void _copySelf(DerivedT& other) const
{
Base::_copySelf(other);
}
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 1f74f36d06e61e77b3b255c865145d2d0135a84d..defdf1c049fa5274b0d112f42d78037730d9eff5 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -17,29 +17,47 @@ namespace armarx::armem::base
*/
template <class _EntityInstanceT, class _Derived>
class EntitySnapshotBase :
- virtual public detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>
+ public detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>
{
using Base = detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>;
public:
+
+ using typename Base::DerivedT;
+ using typename Base::ContainerT;
+
using EntityInstanceT = _EntityInstanceT;
- EntitySnapshotBase& operator=(const EntitySnapshotBase& other)
+
+ public:
+
+ EntitySnapshotBase()
+ {
+ }
+ EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
+ EntitySnapshotBase(parentID.withTimestamp(time))
+ {
+ }
+ EntitySnapshotBase(const MemoryID& id) :
+ Base(id)
{
- other._copySelf(*this);
- return *this;
}
- using Base::size;
+ EntitySnapshotBase(const EntitySnapshotBase& other) = default;
+ EntitySnapshotBase(EntitySnapshotBase&& other) = default;
+ EntitySnapshotBase& operator=(const EntitySnapshotBase& other) = default;
+ EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
+
+
virtual bool equalsDeep(const EntitySnapshotBase& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
- if (size() != other.size())
+ if (this->size() != other.size())
{
return false;
}
int i = 0;
- for (const auto& instance : container)
+ for (const auto& instance : this->_container)
{
if (not instance.equalsDeep(other.getInstance(i)))
{
@@ -50,26 +68,24 @@ namespace armarx::armem::base
return true;
}
- using Base::id;
inline Time& time()
{
- return id.timestamp;
+ return this->id().timestamp;
}
inline const Time& time() const
{
- return id.timestamp;
+ return this->id().timestamp;
}
- using Base::container;
- inline const std::vector<_EntityInstanceT>& instances() const
+ inline const std::vector<EntityInstanceT>& instances() const
{
- return container;
+ return this->_container;
}
- inline std::vector<_EntityInstanceT>& instances()
+ inline std::vector<EntityInstanceT>& instances()
{
- return const_cast<std::vector<_EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
+ return const_cast<std::vector<EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
}
@@ -77,14 +93,14 @@ namespace armarx::armem::base
{
if (parentID)
{
- id = *parentID;
+ this->id() = *parentID;
}
time() = update.timeCreated;
- container.clear();
+ this->_container.clear();
for (int i = 0; i < int(update.instancesData.size()); ++i)
{
- _EntityInstanceT& data = container.emplace_back(i, id);
+ EntityInstanceT& data = this->_container.emplace_back(i, this->id());
data.update(update, i);
}
}
@@ -93,7 +109,7 @@ namespace armarx::armem::base
bool hasInstance(int index) const
{
size_t si = size_t(index);
- return index >= 0 && si < container.size();
+ return index >= 0 && si < this->_container.size();
}
/**
@@ -102,21 +118,20 @@ namespace armarx::armem::base
* @return The instance.
* @throw `armem::error::MissingEntry` If the given index is invalid.
*/
- _EntityInstanceT& getInstance(int index)
+ EntityInstanceT& getInstance(int index)
{
- return const_cast<_EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
+ return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
}
- const _EntityInstanceT& getInstance(int index) const
+ const EntityInstanceT& getInstance(int index) const
{
if (hasInstance(index))
{
- size_t si = size_t(index);
- return container[si];
+ return this->_container[static_cast<size_t>(index)];
}
else
{
- throw armem::error::MissingEntry(_EntityInstanceT().getLevelName(), std::to_string(index), getLevelName(), toDateTimeMilliSeconds(time()));
+ throw armem::error::MissingEntry(EntityInstanceT().getLevelName(), std::to_string(index), getLevelName(), toDateTimeMilliSeconds(time()));
}
}
@@ -127,12 +142,12 @@ namespace armarx::armem::base
* @throw `armem::error::MissingEntry` If the given index is invalid.
* @throw `armem::error::InvalidMemoryID` If memory ID does not have an instance index.
*/
- _EntityInstanceT& getInstance(const MemoryID& id)
+ EntityInstanceT& getInstance(const MemoryID& id)
{
- return const_cast<_EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(id));
+ return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(id));
}
- const _EntityInstanceT& getInstance(const MemoryID& id) const
+ const EntityInstanceT& getInstance(const MemoryID& id) const
{
if (!id.hasInstanceIndex())
{
@@ -148,26 +163,26 @@ namespace armarx::armem::base
* @return The stored instance.
* @throw `armem::error::InvalidArgument` If the given index is invalid. Must be equal to container.size() or -1 (meaning push_back)
*/
- _EntityInstanceT& addInstance(const _EntityInstanceT& instance)
+ EntityInstanceT& addInstance(const EntityInstanceT& instance)
{
- return addInstance(_EntityInstanceT(instance));
+ return addInstance(EntityInstanceT(instance));
}
- _EntityInstanceT& addInstance(_EntityInstanceT&& instance)
+ EntityInstanceT& addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 && (size_t) instance.index() < container.size())
+ if (instance.index() > 0 && static_cast<size_t>(instance.index()) < this->_container.size())
{
throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
"Cannot add an EntityInstance because its index already exists.");
}
- if (instance.index() > 0 && (size_t) instance.index() > container.size())
+ if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
{
throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
"Cannot add an EntityInstance because its index is too big.");
}
- int new_index = container.size();
- auto& it = container.emplace_back(std::move(instance));
+ int new_index = this->_container.size();
+ auto& it = this->_container.emplace_back(std::move(instance));
it.index() = new_index;
return it;
}
@@ -182,7 +197,6 @@ namespace armarx::armem::base
return "entity snapshot";
}
- public:
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index bb7d5a8b9f307d7caec701c7be454c434fde891b..53b398a284bc8ebb1151d5266f81ec7a43010cea 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -3,9 +3,12 @@
#include <map>
#include <string>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include "CoreSegmentBase.h"
#include "detail/EntityContainerBase.h"
+
namespace armarx::armem::base
{
@@ -14,27 +17,45 @@ namespace armarx::armem::base
*/
template <class _CoreSegmentT, class _Derived>
class MemoryBase :
- virtual public detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>
+ public detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>
{
using Base = detail::EntityContainerBase<_CoreSegmentT, typename _CoreSegmentT::ProviderSegmentT::EntityT, _Derived>;
public:
+
+ using typename Base::DerivedT;
+ using typename Base::ContainerT;
+
using CoreSegmentT = _CoreSegmentT;
using ProviderSegmentT = typename CoreSegmentT::ProviderSegmentT;
using EntityT = typename ProviderSegmentT::EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
- MemoryBase& operator=(const MemoryBase& other)
+
+ public:
+
+ MemoryBase()
{
- other._copySelf(*this);
- return *this;
}
+ MemoryBase(const std::string& name) :
+ MemoryBase(MemoryID().withMemoryName(name))
+ {
+ }
+ MemoryBase(const MemoryID& id) :
+ Base(id)
+ {
+ }
+
+ MemoryBase(const MemoryBase& other) = default;
+ MemoryBase(MemoryBase&& other) = default;
+ MemoryBase& operator=(const MemoryBase& other) = default;
+ MemoryBase& operator=(MemoryBase&& other) = default;
+
- using Base::id;
inline const std::string& name() const
{
- return id.memoryName;
+ return this->id().memoryName;
}
inline std::string& name()
{
@@ -42,72 +63,91 @@ namespace armarx::armem::base
}
- using Base::container;
- inline const std::map<std::string, _CoreSegmentT>& coreSegments() const
+ inline const std::map<std::string, CoreSegmentT>& coreSegments() const
{
- return container;
+ return this->_container;
}
- inline std::map<std::string, _CoreSegmentT>& coreSegments()
+ inline std::map<std::string, CoreSegmentT>& coreSegments()
{
- return const_cast<std::map<std::string, _CoreSegmentT>&>(const_cast<const MemoryBase*>(this)->coreSegments());
+ return const_cast<std::map<std::string, CoreSegmentT>&>(const_cast<const MemoryBase*>(this)->coreSegments());
}
bool hasCoreSegment(const std::string& name) const
{
- return container.count(name) > 0;
+ return this->_container.count(name) > 0;
}
- _CoreSegmentT& getCoreSegment(const std::string& name)
+ CoreSegmentT& getCoreSegment(const std::string& name)
{
- return const_cast<_CoreSegmentT&>(const_cast<const MemoryBase*>(this)->getCoreSegment(name));
+ return const_cast<CoreSegmentT&>(const_cast<const MemoryBase*>(this)->getCoreSegment(name));
}
- const _CoreSegmentT& getCoreSegment(const std::string& name) const
+ const CoreSegmentT& getCoreSegment(const std::string& name) const
{
- auto it = this->container.find(name);
- if (it != container.end())
+ auto it = this->_container.find(name);
+ if (it != this->_container.end())
{
return it->second;
}
else
{
- throw armem::error::MissingEntry(_CoreSegmentT().getLevelName(), name, getLevelName(), this->name());
+ throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), name, getLevelName(), this->name());
}
}
- using Base::_checkContainerName;
+ using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.memoryName, this->name());
+ this->_checkContainerName(id.memoryName, this->name());
return getCoreSegment(id.coreSegmentName).getEntity(id);
}
+ const EntityT* findEntity(const MemoryID& id) const override
+ {
+ this->_checkContainerName(id.memoryName, this->name());
+ if (id.hasCoreSegmentName())
+ {
+ return getCoreSegment(id.providerSegmentName).findEntity(id);
+ }
+ else
+ {
+ for (const auto& [_, coreSegment] : this->_container)
+ {
+ if (auto entity = coreSegment.findEntity(id))
+ {
+ return entity;
+ }
+ }
+ return nullptr;
+ }
+ }
+
/**
* @brief Add an empty core segment with the given name.
* @param name The core segment name.
* @param coreSegmentType The core segment type (optional).
* @return The added core segment.
*/
- _CoreSegmentT& addCoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr)
+ CoreSegmentT& addCoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr)
{
- return addCoreSegment(_CoreSegmentT(name, coreSegmentType));
+ return addCoreSegment(CoreSegmentT(name, coreSegmentType));
}
/// Copy and insert a core segment.
- _CoreSegmentT& addCoreSegment(const _CoreSegmentT& coreSegment)
+ CoreSegmentT& addCoreSegment(const CoreSegmentT& coreSegment)
{
- return addCoreSegment(_CoreSegmentT(coreSegment));
+ return addCoreSegment(CoreSegmentT(coreSegment));
}
/// Move and insert a core segment.
- _CoreSegmentT& addCoreSegment(_CoreSegmentT&& coreSegment)
+ CoreSegmentT& addCoreSegment(CoreSegmentT&& coreSegment)
{
- if (container.count(coreSegment.name()) > 0)
+ if (this->_container.count(coreSegment.name()) > 0)
{
throw armem::error::ContainerEntryAlreadyExists(coreSegment.getLevelName(), coreSegment.name(),
this->getLevelName(), this->name());
}
- auto it = container.emplace(coreSegment.name(), std::move(coreSegment)).first;
- it->second.id.setMemoryID(id);
+ auto it = this->_container.emplace(coreSegment.name(), std::move(coreSegment)).first;
+ it->second.id().setMemoryID(this->id());
return it->second;
}
@@ -116,9 +156,9 @@ namespace armarx::armem::base
* @param The core segment names.
* @return The core segments. The contained pointers are never null.
*/
- std::vector<_CoreSegmentT*> addCoreSegments(const std::vector<std::string>& names)
+ std::vector<CoreSegmentT*> addCoreSegments(const std::vector<std::string>& names)
{
- std::vector<_CoreSegmentT*> segments;
+ std::vector<CoreSegmentT*> segments;
for (const auto& name : names)
{
try
@@ -135,28 +175,27 @@ namespace armarx::armem::base
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.memoryName, this->name());
+ this->_checkContainerName(update.entityID.memoryName, this->name());
- auto it = this->container.find(update.entityID.coreSegmentName);
- if (it != container.end())
+ auto it = this->_container.find(update.entityID.coreSegmentName);
+ if (it != this->_container.end())
{
return it->second.update(update);
}
else
{
- throw armem::error::MissingEntry(_CoreSegmentT().getLevelName(), update.entityID.coreSegmentName, getLevelName(), this->name());
+ throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), update.entityID.coreSegmentName, getLevelName(), this->name());
}
}
- using Base::size;
virtual bool equalsDeep(const MemoryBase& other) const
{
//std::cout << "Memory::equalsDeep" << std::endl;
- if (size() != other.size())
+ if (this->size() != other.size())
{
return false;
}
- for (const auto& [key, core] : container)
+ for (const auto& [key, core] : this->_container)
{
if (not other.hasCoreSegment(key))
{
@@ -179,7 +218,5 @@ namespace armarx::armem::base
{
return this->name();
}
-
- public:
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 25ed0e585379b549cdc4798fcc55cb02ca40d7a4..20b298af557452854fb39b442023e75f6f0a2f2e 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -4,7 +4,8 @@
#include <string>
#include "EntityBase.h"
-#include "detail/TypedEntityContainerBase.h"
+#include "detail/AronTyped.h"
+#include "detail/EntityContainerBase.h"
#include "detail/MaxHistorySize.h"
@@ -16,26 +17,51 @@ namespace armarx::armem::base
*/
template <class _EntityT, class _Derived>
class ProviderSegmentBase :
- virtual public detail::TypedEntityContainerBase<_EntityT, _EntityT, _Derived>,
- virtual public detail::MaxHistorySize
+ public detail::EntityContainerBase<_EntityT, _EntityT, _Derived>,
+ public detail::MaxHistorySize,
+ public detail::AronTyped
{
- using Base = detail::TypedEntityContainerBase<_EntityT, _EntityT, _Derived>;
+ using Base = detail::EntityContainerBase<_EntityT, _EntityT, _Derived>;
public:
+
+ using typename Base::DerivedT;
+ using typename Base::ContainerT;
+
using EntityT = _EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
- ProviderSegmentBase& operator=(const ProviderSegmentBase& other)
+
+ public:
+
+ ProviderSegmentBase()
+ {
+ }
+
+ ProviderSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ ProviderSegmentBase(MemoryID().withProviderSegmentName(name), aronType)
+ {
+ }
+ ProviderSegmentBase(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ ProviderSegmentBase(parentID.withProviderSegmentName(name), aronType)
{
- other._copySelf(*this);
- return *this;
}
+ ProviderSegmentBase(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ Base(id),
+ AronTyped(aronType)
+ {
+ }
+
+ ProviderSegmentBase(const ProviderSegmentBase& other) = default;
+ ProviderSegmentBase(ProviderSegmentBase&& other) = default;
+ ProviderSegmentBase& operator=(const ProviderSegmentBase& other) = default;
+ ProviderSegmentBase& operator=(ProviderSegmentBase&& other) = default;
+
- using Base::id;
inline const std::string& name() const
{
- return id.providerSegmentName;
+ return this->id().providerSegmentName;
}
inline std::string& name()
{
@@ -43,38 +69,37 @@ namespace armarx::armem::base
}
- using Base::container;
- inline const std::map<std::string, _EntityT>& entities() const
+ inline const std::map<std::string, EntityT>& entities() const
{
- return container;
+ return this->_container;
}
- inline std::map<std::string, _EntityT>& entities()
+ inline std::map<std::string, EntityT>& entities()
{
- return const_cast<std::map<std::string, _EntityT>&>(const_cast<const ProviderSegmentBase*>(this)->entities());
+ return const_cast<std::map<std::string, EntityT>&>(const_cast<const ProviderSegmentBase*>(this)->entities());
}
bool hasEntity(const std::string& name) const
{
- return container.count(name) > 0;
+ return this->_container.count(name) > 0;
}
- using Base::_checkContainerName;
- const _EntityT& getEntity(const MemoryID& id) const override
+ using Base::getEntity;
+ const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.providerSegmentName, this->getKeyString());
+ this->_checkContainerName(id.providerSegmentName, this->getKeyString());
return getEntity(id.entityName);
}
- _EntityT& getEntity(const std::string& name)
+ EntityT& getEntity(const std::string& name)
{
- return const_cast<_EntityT&>(const_cast<const ProviderSegmentBase*>(this)->getEntity(name));
+ return const_cast<EntityT&>(const_cast<const ProviderSegmentBase*>(this)->getEntity(name));
}
- const _EntityT& getEntity(const std::string& name) const
+ const EntityT& getEntity(const std::string& name) const
{
- auto it = this->container.find(name);
- if (it != container.end())
+ auto it = this->_container.find(name);
+ if (it != this->_container.end())
{
return it->second;
}
@@ -84,6 +109,20 @@ namespace armarx::armem::base
}
}
+ const EntityT* findEntity(const MemoryID& id) const override
+ {
+ this->_checkContainerName(id.providerSegmentName, this->getKeyString());
+ auto it = this->_container.find(id.entityName);
+ if (it != this->_container.end())
+ {
+ return &it->second;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
/**
* @brief Updates an entity's history.
*
@@ -91,15 +130,15 @@ namespace armarx::armem::base
*/
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.providerSegmentName, this->name());
+ this->_checkContainerName(update.entityID.providerSegmentName, this->name());
- _EntityT* entity;
- auto it = this->container.find(update.entityID.providerSegmentName);
- if (it == container.end())
+ EntityT* entity;
+ auto it = this->_container.find(update.entityID.providerSegmentName);
+ if (it == this->_container.end())
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
- entity->setMaxHistorySize(maxHistorySize);
+ entity->setMaxHistorySize(_maxHistorySize);
}
else
{
@@ -110,20 +149,20 @@ namespace armarx::armem::base
}
/// Add an empty entity with the given name.
- _EntityT& addEntity(const std::string& name)
+ EntityT& addEntity(const std::string& name)
{
- return addEntity(_EntityT(name));
+ return addEntity(EntityT(name));
}
/// Copy and insert an entity.
- _EntityT& addEntity(const _EntityT& entity)
+ EntityT& addEntity(const EntityT& entity)
{
- return addEntity(_EntityT(entity));
+ return addEntity(EntityT(entity));
}
/// Move and insert an entity.
- _EntityT& addEntity(_EntityT&& entity)
+ EntityT& addEntity(EntityT&& entity)
{
- auto it = container.emplace(entity.name(), std::move(entity)).first;
- it->second.id.setProviderSegmentID(id);
+ auto it = this->_container.emplace(entity.name(), std::move(entity)).first;
+ it->second.id().setProviderSegmentID(this->id());
return it->second;
}
@@ -136,22 +175,21 @@ namespace armarx::armem::base
void setMaxHistorySize(long maxSize) override
{
MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, entity] : container)
+ for (auto& [name, entity] : this->_container)
{
entity.setMaxHistorySize(maxSize);
}
}
- using Base::size;
virtual bool equalsDeep(const ProviderSegmentBase& other) const
{
//std::cout << "ProviderSegment::equalsDeep" << std::endl;
- if (size() != other.size())
+ if (this->size() != other.size())
{
return false;
}
- for (const auto& [key, value] : container)
+ for (const auto& [key, value] : this->_container)
{
if (not other.hasEntity(key))
{
@@ -176,7 +214,14 @@ namespace armarx::armem::base
return this->name();
}
- public:
+
+ protected:
+
+ virtual void _copySelf(DerivedT& other) const override
+ {
+ Base::_copySelf(other);
+ other.aronType() = _aronType;
+ }
};
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec7ca0402ad128dc6d342aa28279929c376df557
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
@@ -0,0 +1,26 @@
+#include "AronTyped.h"
+
+namespace armarx::armem::base::detail
+{
+
+ AronTyped::AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType) :
+ _aronType(aronType)
+ {}
+
+ bool AronTyped::hasAronType() const
+ {
+ return _aronType != nullptr;
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr& AronTyped::aronType()
+ {
+ return _aronType;
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr AronTyped::aronType() const
+ {
+ return _aronType;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb0ad088fb4fb27ca39cf09f034ea7eb3b202cfe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+
+
+namespace armarx::armem::base::detail
+{
+
+ /**
+ * @brief Something with a specific ARON type.
+ */
+ class AronTyped
+ {
+ public:
+
+ AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+
+
+ bool hasAronType() const;
+ aron::typenavigator::ObjectNavigatorPtr& aronType();
+ aron::typenavigator::ObjectNavigatorPtr aronType() const;
+
+
+ protected:
+
+ /// The expected Aron type. May be nullptr, in which case no type information is available.
+ aron::typenavigator::ObjectNavigatorPtr _aronType;
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
index 0f28deeefef83a48e1ebea520bce14b800afd050..09733b648d096c7e33179c2903f3dbe241dca8d2 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -19,20 +19,25 @@ namespace armarx::armem::base::detail
*/
template <class _ValueT, class _EntityT, class _Derived>
class EntityContainerBase :
- virtual public MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>
+ public MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>
{
using Base = MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>;
public:
- using Derived = _Derived;
- using EntitySnapshotT = typename _EntityT::EntitySnapshotT;
+
+ using DerivedT = _Derived;
+ using ValueT = _ValueT;
+
+ using EntityT = _EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
- EntityContainerBase& operator=(const EntityContainerBase& other)
- {
- other._copySelf(*this);
- return *this;
- }
+
+ public:
+
+ using Base::MemoryContainerBase;
+ using Base::operator=;
+
/**
* @brief Store all updates in `commit`.
@@ -62,11 +67,26 @@ namespace armarx::armem::base::detail
* @return The entity.
* @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
*/
- virtual _EntityT& getEntity(const MemoryID& id)
+ virtual EntityT& getEntity(const MemoryID& id)
{
- return const_cast<_EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
+ return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
}
- virtual const _EntityT& getEntity(const MemoryID& id) const = 0;
+ virtual const EntityT& getEntity(const MemoryID& id) const = 0;
+
+ /**
+ * @brief Find an entity.
+ *
+ * Search for the entity with the given ID and return a pointer to the
+ * first match. If `id` is underspecified (e.g. no provider segment name),
+ * search all children until the first match is found.
+ *
+ * If no matching entity is found, return `nullptr`.
+ *
+ * @param id The entities ID.
+ * @return A pointer to the first matching entity or `nullptr` if none was found.
+ */
+ virtual const EntityT* findEntity(const MemoryID& id) const = 0;
+
/**
* @brief Retrieve an entity snapshot.
@@ -84,7 +104,7 @@ namespace armarx::armem::base::detail
virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
{
- const _EntityT& entity = getEntity(id);
+ const EntityT& entity = getEntity(id);
if (id.hasTimestamp())
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
index 4fe12f7e2ec9038456d8701f4022f33387ea4cfa..2c3b0554c65523826db0a7709599ed8e5140e0a1 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
@@ -9,11 +9,11 @@ namespace armarx::armem::base::detail
void MaxHistorySize::setMaxHistorySize(long maxSize)
{
- this->maxHistorySize = maxSize;
+ this->_maxHistorySize = maxSize;
}
long MaxHistorySize::getMaxHistorySize() const
{
- return maxHistorySize;
+ return _maxHistorySize;
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
index edbc12fa6ba514513ee61c5c2e2cdb3148c0ad07..69539df5c4063b8d27cd751b79ee82eb90ff5b7e 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
@@ -23,9 +23,12 @@ namespace armarx::armem::base::detail
/**
* @brief Maximum size of entity histories.
+ *
+ * If negative, the size of `history` is not limited.
+ *
* @see Entity::maxHstorySize
*/
- long maxHistorySize = -1;
+ long _maxHistorySize = -1;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index e29be04a3fc7aae3fb4301280eb8fc358a2fa6ef..b2155c70dbf645ebe0e2783fd3189bf9b58800af 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -1,8 +1,6 @@
#pragma once
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../../error.h"
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include "MemoryItem.h"
@@ -14,101 +12,124 @@ namespace armarx::armem::base::detail
* @class Provides default implmentations of `MemoryContainer`, as well as
* iterators (which requires a template).
*/
- template <class _ContainerT, class _Derived>
+ template <class _ContainerT, class _DerivedT>
class MemoryContainerBase :
- virtual public MemoryItem
+ public MemoryItem
{
using Base = MemoryItem;
public:
- using Derived = _Derived;
+
+ using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
+
public:
+
+
MemoryContainerBase()
{}
- MemoryContainerBase(const MemoryContainerBase<ContainerT, Derived>& o) :
- MemoryItem(o.id),
- container(o.container)
- {}
-
- MemoryContainerBase& operator=(const MemoryContainerBase& other)
+ MemoryContainerBase(const MemoryID& id) :
+ MemoryItem(id)
{
- other._copySelf(*this);
- return *this;
}
+ MemoryContainerBase(const MemoryContainerBase& other) = default;
+ MemoryContainerBase(MemoryContainerBase&& other) = default;
+ MemoryContainerBase& operator=(const MemoryContainerBase& other) = default;
+ MemoryContainerBase& operator=(MemoryContainerBase&& other) = default;
+
+
// Container methods
+
virtual bool empty() const
{
- return container.empty();
+ return _container.empty();
}
virtual std::size_t size() const
{
- return container.size();
+ return _container.size();
}
virtual void clear()
{
- return container.clear();
+ return _container.clear();
}
typename ContainerT::const_iterator begin() const
{
- return container.begin();
+ return _container.begin();
}
typename ContainerT::iterator begin()
{
- return container.begin();
+ return _container.begin();
}
typename ContainerT::const_iterator end() const
{
- return container.end();
+ return _container.end();
}
typename ContainerT::iterator end()
{
- return container.end();
+ return _container.end();
+ }
+
+ const ContainerT& container() const
+ {
+ return _container;
+ }
+ ContainerT& container()
+ {
+ return _container;
}
// Copying
- virtual Derived copy() const
+
+ virtual DerivedT copy() const
{
- Derived d;
+ DerivedT d;
this->_copySelf(d);
return d;
}
/// Make a copy not containing any elements.
- virtual Derived copyEmpty() const
+ virtual DerivedT copyEmpty() const
{
- Derived d;
+ DerivedT d;
this->_copySelfEmpty(d);
return d;
}
+
protected:
- /// @throw `armem::error::ContainerNameMismatch` Of `expectedName != actualName`.
- void _checkContainerName(const std::string& expectedName, const std::string& actualName) const
+
+ /**
+ * @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
+ */
+ void _checkContainerName(const std::string& gottenName, const std::string& actualName,
+ bool emptyOk = true) const
{
- if (expectedName != actualName)
+ if (!((emptyOk && gottenName.empty()) || gottenName == actualName))
{
- throw armem::error::ContainerNameMismatch(expectedName, this->getLevelName(), actualName);
+ throw armem::error::ContainerNameMismatch(gottenName, this->getLevelName(), actualName);
}
}
- virtual void _copySelf(Derived& other) const
+ virtual void _copySelf(DerivedT& other) const
{
Base::_copySelf(other);
- other.container = container;
+ other.container() = _container;
}
- virtual void _copySelfEmpty(Derived& other) const
+ virtual void _copySelfEmpty(DerivedT& other) const
{
Base::_copySelf(other);
}
- public:
- ContainerT container;
+
+ protected:
+
+ ContainerT _container;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
index 602817fe5b587e971147f1eb9581d6279825da71..a6a9f73e17443091503c09472bc3cee7259644ac 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
@@ -1 +1,40 @@
#include "MemoryItem.h"
+
+
+namespace armarx::armem::base::detail
+{
+
+ MemoryItem::MemoryItem()
+ {
+ }
+
+ MemoryItem::MemoryItem(const MemoryID& id) :
+ _id(id)
+ {
+ }
+
+ /*
+ MemoryItem::MemoryItem(const MemoryItem& other) :
+ _id(other.id())
+ {}
+ */
+
+
+ MemoryItem::~MemoryItem()
+ {
+ }
+
+ /*
+ MemoryItem& MemoryItem::operator=(const MemoryItem& other)
+ {
+ other._copySelf(*this);
+ return *this;
+ }
+ */
+
+ void MemoryItem::_copySelf(MemoryItem& other) const
+ {
+ other.id() = id();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
index 9749e275a8c2f56bf25eb5ec02852cee139e551c..45226e6327520b7bbddc198364c16c2b8229352c 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
@@ -14,40 +14,50 @@ namespace armarx::armem::base::detail
class MemoryItem
{
public:
- MemoryItem()
- {
- }
- MemoryItem(const MemoryItem& other) :
- id(other.id)
- {}
+ MemoryItem();
+ MemoryItem(const MemoryID& id);
+
+ MemoryItem(const MemoryItem& other) = default;
+ MemoryItem(MemoryItem&& other) = default;
+ MemoryItem& operator=(const MemoryItem& other) = default;
+ MemoryItem& operator=(MemoryItem&& other) = default;
+
+ virtual ~MemoryItem();
+
- MemoryItem(const MemoryID& id) :
- id(id)
+ //MemoryItem& operator=(const MemoryItem& other);
+
+
+ inline MemoryID& id()
{
+ return _id;
}
-
- MemoryItem& operator=(const MemoryItem& other)
+ inline const MemoryID& id() const
{
- other._copySelf(*this);
- return *this;
+ return _id;
}
+
// Introspection
+
/// Get a string version of `*this`' key.
virtual std::string getKeyString() const = 0;
/// Get a readable name of this level for messages, errors etc.
virtual std::string getLevelName() const = 0;
+
protected:
- virtual void _copySelf(MemoryItem& other) const
- {
- other.id = id;
- }
- public:
- MemoryID id;
+ /// Copy `*this` to `other`.
+ virtual void _copySelf(MemoryItem& other) const;
+
+
+ protected:
+
+ MemoryID _id;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.cpp b/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.cpp
deleted file mode 100644
index cd42021d8a566ecca4a5e4662a560660d08b3630..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "TypedEntityContainerBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.h
deleted file mode 100644
index f4c511da0bf3b49375980d5afe4d98f03ef84384..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/base/detail/TypedEntityContainerBase.h
+++ /dev/null
@@ -1,84 +0,0 @@
-#pragma once
-
-#include "EntityContainerBase.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-namespace armarx::armem::base::detail
-{
-
- /**
- * @brief An entity container with a specific (Aron) type.
- */
- template <class _ValueT, class _EntityT, class _Derived>
- class TypedEntityContainerBase :
- virtual public EntityContainerBase<_ValueT, _EntityT, _Derived>
- {
- using Base = EntityContainerBase<_ValueT, _EntityT, _Derived>;
-
- public:
- using Derived = _Derived;
- using EntitySnapshotT = typename _EntityT::EntitySnapshotT;
- using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
-
- TypedEntityContainerBase(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- aronType(aronType)
- {}
-
- TypedEntityContainerBase(const TypedEntityContainerBase<_ValueT, _EntityT, _Derived>& other) :
- MemoryItem(other),
- MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>(other),
- aronType(other.aronType)
- {}
-
- TypedEntityContainerBase(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- MemoryItem(id),
- aronType(aronType)
- {}
-
- TypedEntityContainerBase& operator=(const TypedEntityContainerBase& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- bool hasAronType() const
- {
- return aronType != nullptr;
- }
-
- virtual _Derived copy() const override
- {
- _Derived d;
- this->_copySelf(d);
- return d;
- }
-
- virtual _Derived copyEmpty() const override
- {
- _Derived d;
- this->_copySelfEmpty(d);
- return d;
- }
-
- protected:
- virtual void _copySelf(_Derived& other) const override
- {
- Base::_copySelf(other);
- other.aronType = aronType;
- }
-
- virtual void _copySelfEmpty(_Derived& other) const override
- {
- Base::_copySelfEmpty(other);
- other.aronType = aronType;
- }
-
- public:
- /// The expected Aron type. May be nullptr, in which case no type information is available.
- aron::typenavigator::ObjectNavigatorPtr aronType;
-
- };
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
index 369dab76068c26474936b684df1dcc79311859e3..6a26be9c7915a54b081e5d03b4f67cb6ae9eeaee 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -2,47 +2,12 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "error.h"
+#include "TypeIO.h"
namespace armarx::armem::d_ltm
{
- CoreSegment::CoreSegment()
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(MemoryID().withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(parentID.withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(id, aronType)
- {
- }
-
- CoreSegment::CoreSegment(const CoreSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, ProviderSegment>, CoreSegment>(other),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(other)
- {
- }
-
- CoreSegment& CoreSegment::operator=(const CoreSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
std::filesystem::path CoreSegment::_fullPath() const
{
if (path)
@@ -54,15 +19,15 @@ namespace armarx::armem::d_ltm
std::filesystem::path CoreSegment::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName / id.coreSegmentName;
+ return path / id().memoryName / id().coreSegmentName;
}
wm::CoreSegment CoreSegment::convert() const
{
wm::CoreSegment m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
- m.addProviderSegment(s.convert(aronType));
+ m.addProviderSegment(s.convert(_aronType));
}
return m;
}
@@ -79,7 +44,7 @@ namespace armarx::armem::d_ltm
ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
}
- container.clear();
+ _container.clear();
path = p_ptr;
if (!std::filesystem::exists(p))
@@ -93,13 +58,16 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = container.emplace(std::make_pair(k, id.withProviderSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
wms.first->second.reload(p_ptr);
}
if (d.is_regular_file())
{
- readAronType(d.path());
+ if (auto type = TypeIO::readAronType(d.path()))
+ {
+ _aronType = type;
+ }
}
}
}
@@ -108,18 +76,18 @@ namespace armarx::armem::d_ltm
void CoreSegment::append(const wm::CoreSegment& m)
{
std::filesystem::create_directories(_fullPath());
- writeAronType(_fullPath());
+ TypeIO::writeAronType(_aronType, _fullPath());
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m)
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
else
{
std::filesystem::create_directory(_fullPath() / k);
- auto wms = container.emplace(std::make_pair(k, id.withProviderSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
index f26c01c67ed90ab5467b32e4ed755b3ddee2e1cf..76bb297e76d268ce8517449686ffae1cdb3ed2cd 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
@@ -3,12 +3,10 @@
#include <filesystem>
#include "../base/CoreSegmentBase.h"
-#include "detail/TypedEntityContainer.h"
+#include "../workingmemory/CoreSegment.h"
#include "ProviderSegment.h"
-#include "../workingmemory/CoreSegment.h"
-
namespace armarx::armem::d_ltm
{
@@ -17,19 +15,15 @@ namespace armarx::armem::d_ltm
* @brief Data of a core segment containing multiple provider segments.
*/
class CoreSegment :
- virtual public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- virtual public detail::TypedEntityContainer<ProviderSegment, CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
- CoreSegment();
- CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const CoreSegment& other);
- CoreSegment& operator=(const CoreSegment& other);
+ using Base::CoreSegmentBase;
+ using Base::operator=;
+
// Conversion
wm::CoreSegment convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
index 3847f0af9235f903f1a819e05b3bbccee1ecfa0e..a616914ede5c279d11daea7afcfe81a681b0c460 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
@@ -2,33 +2,6 @@
namespace armarx::armem::d_ltm
{
-
- Entity::Entity()
- {
- }
-
- Entity::Entity(const std::string& name, const MemoryID& parentID) :
- base::detail::MemoryItem(parentID.withEntityName(name))
- {
- }
-
- Entity::Entity(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- Entity::Entity(const Entity& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<Time, EntitySnapshot>, Entity>(other)
- {
- }
-
- Entity& Entity::operator=(const Entity& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
std::filesystem::path Entity::_fullPath() const
{
if (path)
@@ -40,13 +13,13 @@ namespace armarx::armem::d_ltm
std::filesystem::path Entity::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName / id.coreSegmentName / id.providerSegmentName / id.entityName;
+ return path / id().memoryName / id().coreSegmentName / id().providerSegmentName / id().entityName;
}
wm::Entity Entity::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
wm::Entity m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addSnapshot(s.convert(expectedStructure));
}
@@ -65,7 +38,7 @@ namespace armarx::armem::d_ltm
ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
}
- container.clear();
+ _container.clear();
path = p_ptr;
if (!std::filesystem::exists(p))
@@ -80,7 +53,7 @@ namespace armarx::armem::d_ltm
{
std::string k = d.path().filename();
armem::Time t = armem::Time::microSeconds(std::stol(k));
- auto wms = container.emplace(std::make_pair(t, id.withTimestamp(t)));
+ auto wms = _container.emplace(std::make_pair(t, id().withTimestamp(t)));
wms.first->second.reload(p_ptr);
}
}
@@ -90,16 +63,16 @@ namespace armarx::armem::d_ltm
void Entity::append(const wm::Entity& m)
{
std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.setTo(s);
}
else
{
std::filesystem::create_directory(_fullPath() / std::to_string(k.toMicroSeconds()));
- auto wms = container.emplace(std::make_pair(k, id.withTimestamp(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
wms.first->second.path = path;
wms.first->second.setTo(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
index 756f3eb69dd3499333532187ee207fd4cec4e102..35022275570c93378cbe0c767aad1776da594838 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
@@ -3,10 +3,10 @@
#include <filesystem>
#include "../base/EntityBase.h"
+#include "../workingmemory/Entity.h"
#include "EntitySnapshot.h"
-#include "../workingmemory/Entity.h"
namespace armarx::armem::d_ltm
@@ -31,19 +31,15 @@ namespace armarx::armem::d_ltm
* each containing a single `AronData` object of a specific `AronType`.
*/
class Entity :
- virtual public base::EntityBase<EntitySnapshot, Entity>
+ public base::EntityBase<EntitySnapshot, Entity>
{
using Base = base::EntityBase<EntitySnapshot, Entity>;
public:
- Entity();
- Entity(const std::string& name, const MemoryID& parentID = {});
- Entity(const MemoryID& id);
-
- /// Copy the history from `other` to this.
- Entity(const Entity& other);
- /// Copy the history from `other` to this.
- Entity& operator=(const Entity& other);
+
+ using Base::EntityBase;
+ using Base::operator=;
+
// Conversion
wm::Entity convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
index 43831ad04c5faf634ab6699b17622e0be4014d37..49bfe419d8a8cf1052fad7e47a0ecba9e355c529 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
@@ -13,35 +13,19 @@
namespace armarx::armem::d_ltm
{
- EntityInstance::EntityInstance()
- {
- }
EntityInstance::EntityInstance(const EntityInstance& other) :
- base::detail::MemoryItem(other.id)
+ Base(other),
+ path(other.path)
{
}
- EntityInstance::EntityInstance(int index, const MemoryID& parentID) :
- EntityInstance(parentID.withInstanceIndex(index))
+ bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
+ return id() == other.id();
}
- EntityInstance::EntityInstance(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
- EntityInstance& EntityInstance::operator=(const EntityInstance& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- bool EntityInstance::equalsDeep(const EntityInstance& other) const
- {
- return id == other.id;
- }
void EntityInstance::update(const EntityUpdate& update, int index)
{
@@ -73,7 +57,7 @@ namespace armarx::armem::d_ltm
std::filesystem::path EntityInstance::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName / id.coreSegmentName / id.providerSegmentName / id.entityName / std::to_string(id.timestamp.toMicroSeconds()) / std::to_string(id.instanceIndex);
+ return path / id().memoryName / id().coreSegmentName / id().providerSegmentName / id().entityName / std::to_string(id().timestamp.toMicroSeconds()) / std::to_string(id().instanceIndex);
}
wm::EntityInstance EntityInstance::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
@@ -143,7 +127,7 @@ namespace armarx::armem::d_ltm
wm::EntityInstance EntityInstance::unwrapData(const aron::datanavigator::DictNavigatorPtr& dataWrapped) const
{
- wm::EntityInstance e(id);
+ wm::EntityInstance e(id());
wm::EntityInstanceMetadata& metadata = e.metadata();
if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
@@ -193,7 +177,7 @@ namespace armarx::armem::d_ltm
dataWrapped->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
- confidence->setValue(metadata.confidence);
+ confidence->setValue(static_cast<double>(metadata.confidence));
dataWrapped->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
return dataWrapped;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
index 8e7a0fed18959090f05196ba32565f507e19faf4..9ee48c55b33dca02c88f57de1ac30426f8d5ae37 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
@@ -3,26 +3,28 @@
#include <filesystem>
#include "../base/EntityInstanceBase.h"
-
#include "../workingmemory/EntityInstance.h"
+
namespace armarx::armem::d_ltm
{
/**
* @brief Data of a single entity instance.
*/
class EntityInstance :
- virtual public base::EntityInstanceBase<EntityInstance>
+ public base::EntityInstanceBase<EntityInstance>
{
using Base = base::EntityInstanceBase<EntityInstance>;
public:
- EntityInstance();
- EntityInstance(const EntityInstance&);
- EntityInstance(int index, const MemoryID& parentID = {});
- EntityInstance(const MemoryID& id);
- EntityInstance& operator=(const EntityInstance& other);
+ using Base::EntityInstanceBase;
+
+ EntityInstance(const EntityInstance& other);
+ EntityInstance(EntityInstance&& other) = default;
+ EntityInstance& operator=(const EntityInstance& other) = default;
+ EntityInstance& operator=(EntityInstance&& other) = default;
+
/**
* @brief Fill `*this` with the update's values.
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
index 1f855d1a4ad583ccbedfb4c97e24b8fdb527a388..199e4f2c9b9eed9d9b099e773d96ba0a03fd7d16 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
@@ -8,33 +8,6 @@
namespace armarx::armem::d_ltm
{
-
- EntitySnapshot::EntitySnapshot()
- {
- }
-
- EntitySnapshot::EntitySnapshot(Time time, const MemoryID& parentID) :
- EntitySnapshot(parentID.withTimestamp(time))
- {
- }
-
- EntitySnapshot::EntitySnapshot(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- EntitySnapshot::EntitySnapshot(const EntitySnapshot& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::vector<EntityInstance>, EntitySnapshot>(other)
- {
- }
-
- EntitySnapshot& EntitySnapshot::operator=(const EntitySnapshot& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
std::filesystem::path EntitySnapshot::_fullPath() const
{
if (path)
@@ -46,13 +19,15 @@ namespace armarx::armem::d_ltm
std::filesystem::path EntitySnapshot::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName / id.coreSegmentName / id.providerSegmentName / id.entityName / std::to_string(id.timestamp.toMicroSeconds());
+ return path / id().memoryName / id().coreSegmentName / id().providerSegmentName
+ / id().entityName
+ / std::to_string(id().timestamp.toMicroSeconds());
}
wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
wm::EntitySnapshot m;
- for (const auto& s : container)
+ for (const auto& s : _container)
{
m.addInstance(s.convert(expectedStructure));
}
@@ -72,7 +47,7 @@ namespace armarx::armem::d_ltm
}
else
{
- container.clear();
+ _container.clear();
path = p_ptr;
// todo
@@ -81,7 +56,7 @@ namespace armarx::armem::d_ltm
std::filesystem::path d = p / std::to_string(i);
if (std::filesystem::is_directory(d))
{
- auto wms = container.emplace_back(id.withInstanceIndex(i));
+ auto wms = _container.emplace_back(id().withInstanceIndex(i));
wms.reload(p_ptr);
}
else
@@ -97,14 +72,14 @@ namespace armarx::armem::d_ltm
std::filesystem::create_directories(_fullPath());
// We remove the contente here and reset it with new values
- container.clear();
+ _container.clear();
- unsigned int i = 0;
- for (const auto& s : m.container)
+ int i = 0;
+ for (const auto& s : m.instances())
{
std::filesystem::create_directory(_fullPath() / std::to_string(i));
- auto wms = container.emplace_back(id.withInstanceIndex(i++));
+ auto wms = _container.emplace_back(id().withInstanceIndex(i++));
wms.path = path;
wms.setTo(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
index 5c3cf02ff6cb452797ac0c0433bbef294d92a29f..f73702857fc6700e5cfae5f6d08b369240d08f79 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
@@ -3,11 +3,10 @@
#include <filesystem>
#include "../base/EntitySnapshotBase.h"
+#include "../workingmemory/EntitySnapshot.h"
#include "EntityInstance.h"
-#include "../workingmemory/EntitySnapshot.h"
-
namespace armarx::armem::d_ltm
{
@@ -16,20 +15,15 @@ namespace armarx::armem::d_ltm
* @brief Data of an entity at one point in time.
*/
class EntitySnapshot :
- virtual public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
{
using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
public:
- EntitySnapshot();
- EntitySnapshot(Time time, const MemoryID& parentID = {});
- EntitySnapshot(const MemoryID& id);
- /// Copy the instances from `other` to this.
- EntitySnapshot(const EntitySnapshot& other);
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
- /// Copy the instances from `other` to this.
- EntitySnapshot& operator=(const EntitySnapshot& other);
// Conversion
void reload(const std::shared_ptr<std::filesystem::path>&);
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
index 4ef34ffd13af2559ca0691c4bdcbdd950e2636ce..874d189d6a22a79c9e015d800c292f9af075cfea 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
@@ -8,34 +8,6 @@
namespace armarx::armem::d_ltm
{
-
- Memory::Memory()
- {
- }
-
- Memory::Memory(const std::string& name) :
- base::detail::MemoryItem(MemoryID().withMemoryName(name))
- {
- }
-
- Memory::Memory(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- Memory::Memory(const Memory& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, CoreSegment>, Memory>(other)
- {
- *this = other;
- }
-
- Memory& Memory::operator=(const Memory& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
std::filesystem::path Memory::_fullPath() const
{
if (path)
@@ -47,13 +19,13 @@ namespace armarx::armem::d_ltm
std::filesystem::path Memory::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName;
+ return path / id().memoryName;
}
wm::Memory Memory::convert() const
{
wm::Memory m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addCoreSegment(s.convert());
}
@@ -67,7 +39,7 @@ namespace armarx::armem::d_ltm
ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
}
- container.clear();
+ _container.clear();
path = std::make_shared<std::filesystem::path>(p.parent_path());
if (!std::filesystem::exists(p))
@@ -76,14 +48,14 @@ namespace armarx::armem::d_ltm
}
else
{
- id = MemoryID().withMemoryName(p.filename());
+ id() = MemoryID().withMemoryName(p.filename());
for (const auto& d : std::filesystem::directory_iterator(p))
{
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = container.emplace(std::make_pair(k, id.withCoreSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
wms.first->second.reload(path);
}
}
@@ -93,9 +65,9 @@ namespace armarx::armem::d_ltm
void Memory::append(const wm::Memory& m)
{
std::filesystem::create_directories(_fullPath());
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
@@ -103,7 +75,7 @@ namespace armarx::armem::d_ltm
{
std::filesystem::create_directory(_fullPath() / k);
- auto wms = container.emplace(std::make_pair(k, id.withCoreSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
index 32bd30fa69c4dff87ef9d13e599cac8442936003..7f7872f7a638cc31122f09b00a95755ede4d9606 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
@@ -3,10 +3,10 @@
#include <filesystem>
#include "../base/MemoryBase.h"
+#include "../workingmemory/Memory.h"
#include "CoreSegment.h"
-#include "../workingmemory/Memory.h"
namespace armarx::armem::d_ltm
{
@@ -15,17 +15,15 @@ namespace armarx::armem::d_ltm
* @brief Data of a memory consisting of multiple core segments.
*/
class Memory :
- virtual public base::MemoryBase<CoreSegment, Memory>
+ public base::MemoryBase<CoreSegment, Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
- Memory();
- Memory(const std::string& name);
- Memory(const MemoryID& id);
- Memory(const Memory& other);
- Memory& operator=(const Memory& other);
+ using Base::MemoryBase;
+ using Base::operator=;
+
// Conversion
wm::Memory convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
index 60a8268ee4d0bb966510caa2960fd2f59506a117..2d5ed3d4a588827137b33e69ecaec3be1a4c31fc 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
@@ -2,47 +2,11 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "error.h"
+#include "TypeIO.h"
namespace armarx::armem::d_ltm
{
-
- ProviderSegment::ProviderSegment()
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(MemoryID().withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(parentID.withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(parentID.withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(id, aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const ProviderSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, Entity>, ProviderSegment>(other),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(other)
- {
- }
-
- ProviderSegment& ProviderSegment::operator=(const ProviderSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
std::filesystem::path ProviderSegment::_fullPath() const
{
if (path)
@@ -54,17 +18,17 @@ namespace armarx::armem::d_ltm
std::filesystem::path ProviderSegment::_fullPath(const std::filesystem::path& path) const
{
- return path / id.memoryName / id.coreSegmentName / id.providerSegmentName;
+ return path / id().memoryName / id().coreSegmentName / id().providerSegmentName;
}
wm::ProviderSegment ProviderSegment::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
wm::ProviderSegment m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
if (hasAronType())
{
- m.addEntity(s.convert(aronType));
+ m.addEntity(s.convert(_aronType));
}
else
{
@@ -86,7 +50,7 @@ namespace armarx::armem::d_ltm
ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
}
- container.clear();
+ _container.clear();
path = p_ptr;
if (!std::filesystem::exists(p))
@@ -100,13 +64,16 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = container.emplace(std::make_pair(k, id.withEntityName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
wms.first->second.reload(p_ptr);
}
if (d.is_regular_file())
{
- readAronType(d.path());
+ if (auto type = TypeIO::readAronType(d.path()))
+ {
+ _aronType = type;
+ }
}
}
}
@@ -115,18 +82,18 @@ namespace armarx::armem::d_ltm
void ProviderSegment::append(const wm::ProviderSegment& m)
{
std::filesystem::create_directories(_fullPath());
- writeAronType(_fullPath());
+ TypeIO::writeAronType(_aronType, _fullPath());
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
else
{
std::filesystem::create_directory(_fullPath() / k);
- auto wms = container.emplace(std::make_pair(k, id.withEntityName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
index c3a5f1dfd2b75e49d8d20c54816a793c0ba85c4b..8ad96ea0630af0d5aba8b7270fe229c235c2a010 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
@@ -3,12 +3,10 @@
#include <filesystem>
#include "../base/ProviderSegmentBase.h"
-#include "detail/TypedEntityContainer.h"
+#include "../workingmemory/ProviderSegment.h"
#include "Entity.h"
-#include "../workingmemory/ProviderSegment.h"
-
namespace armarx::armem::d_ltm
{
@@ -17,19 +15,15 @@ namespace armarx::armem::d_ltm
* @brief Data of a provider segment containing multiple entities.
*/
class ProviderSegment :
- virtual public base::ProviderSegmentBase<Entity, ProviderSegment>,
- virtual public detail::TypedEntityContainer<Entity, ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
{
using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
public:
- ProviderSegment();
- ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const ProviderSegment& other);
- ProviderSegment& operator=(const ProviderSegment& other);
+ using Base::ProviderSegmentBase;
+ using Base::operator=;
+
// Conversion
wm::ProviderSegment convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc7f7a83706c7b9f69cc05a6e81bcf810c10a249
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.cpp
@@ -0,0 +1,71 @@
+#include "TypeIO.h"
+
+#include <iostream>
+#include <fstream>
+#include <filesystem>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
+#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+
+namespace armarx::armem::d_ltm
+{
+
+ aron::typenavigator::ObjectNavigatorPtr TypeIO::unwrapType(const aron::typenavigator::ObjectNavigatorPtr& type)
+ {
+ return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(type->getMemberType(TYPE_WRAPPER_DATA_FIELD));
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr TypeIO::wrapType(const aron::typenavigator::ObjectNavigatorPtr& type)
+ {
+ aron::typenavigator::ObjectNavigatorPtr typeWrapped(new aron::typenavigator::ObjectNavigator());
+ typeWrapped->setObjectName(type->getObjectName() + "__ltm_type_export");
+ typeWrapped->addMemberType(TYPE_WRAPPER_DATA_FIELD, type);
+
+ typeWrapped->addMemberType(TYPE_WRAPPER_TIME_STORED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
+ typeWrapped->addMemberType(TYPE_WRAPPER_TIME_CREATED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
+ typeWrapped->addMemberType(TYPE_WRAPPER_TIME_SENT_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
+ typeWrapped->addMemberType(TYPE_WRAPPER_TIME_ARRIVED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
+ typeWrapped->addMemberType(TYPE_WRAPPER_CONFIDENCE_FIELD, std::make_shared<aron::typenavigator::DoubleNavigator>());
+
+ return typeWrapped;
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr TypeIO::readAronType(const std::filesystem::__cxx11::path& filepath)
+ {
+ if (std::filesystem::is_regular_file(filepath))
+ {
+ if (filepath.filename() == (std::string(TYPE_FILENAME) + ".json"))
+ {
+ std::ifstream ifs(filepath);
+ std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+
+ aron::typeIO::reader::NlohmannJSONReader typeReader(file_content);
+ aron::typeIO::writer::NavigatorWriter navWriter;
+ aron::typeIO::Converter::ReadAndConvert(typeReader, navWriter);
+ return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(navWriter.getResult());
+ }
+ }
+ return nullptr;
+ }
+
+ void TypeIO::writeAronType(const aron::typenavigator::ObjectNavigatorPtr& type, const std::filesystem::__cxx11::path& filepath)
+ {
+ if (type)
+ {
+ std::ofstream ofs(filepath);
+
+ aron::typeIO::writer::NlohmannJSONWriter typeWriter;
+ aron::typeIO::Visitor::VisitAndSetup(typeWriter, type);
+ std::string new_file_full_content = typeWriter.getResult().dump(2);
+
+ ofs << new_file_full_content;
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h
new file mode 100644
index 0000000000000000000000000000000000000000..4311bdcd7034a08b30b7c7f6ae9ee3eab0952fac
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/TypeIO.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <filesystem>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+
+
+namespace armarx::armem::d_ltm
+{
+
+ /**
+ * @brief An entity container with a specific (Aron) type.
+ */
+ class TypeIO
+ {
+ public:
+
+ static aron::typenavigator::ObjectNavigatorPtr unwrapType(const aron::typenavigator::ObjectNavigatorPtr& type);
+ static aron::typenavigator::ObjectNavigatorPtr wrapType(const aron::typenavigator::ObjectNavigatorPtr& type);
+
+ static aron::typenavigator::ObjectNavigatorPtr readAronType(const std::filesystem::path& filepath);
+ static void writeAronType(const aron::typenavigator::ObjectNavigatorPtr& type, const std::filesystem::path& filepath);
+
+
+ private:
+
+ static const constexpr char* TYPE_FILENAME = "type";
+ static constexpr const char* TYPE_WRAPPER_DATA_FIELD = "__ARON_DATA";
+ static constexpr const char* TYPE_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
+ static constexpr const char* TYPE_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
+ static constexpr const char* TYPE_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
+ static constexpr const char* TYPE_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
+ static constexpr const char* TYPE_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/detail/TypedEntityContainer.h b/source/RobotAPI/libraries/armem/core/diskmemory/detail/TypedEntityContainer.h
deleted file mode 100644
index f17bbc077f1c3d7ef7c49ddc4d9dddb843a184e2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/diskmemory/detail/TypedEntityContainer.h
+++ /dev/null
@@ -1,100 +0,0 @@
-#pragma once
-
-#include <iostream>
-#include <fstream>
-
-#include "../../base/detail/TypedEntityContainerBase.h"
-#include "../Entity.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-namespace armarx::armem::d_ltm::detail
-{
-
- /**
- * @brief An entity container with a specific (Aron) type.
- */
- template <class _ValueT, class Derived>
- class TypedEntityContainer :
- virtual public base::detail::TypedEntityContainerBase<_ValueT, Entity, Derived>
- {
- using Base = base::detail::TypedEntityContainerBase<_ValueT, Entity, Derived>;
-
- public:
- TypedEntityContainer& operator=(const TypedEntityContainer& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- protected:
- aron::typenavigator::ObjectNavigatorPtr unwrapType(const aron::typenavigator::ObjectNavigatorPtr& t) const
- {
- return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(t->getMemberType(TYPE_WRAPPER_DATA_FIELD));
- }
-
- aron::typenavigator::ObjectNavigatorPtr wrapType(const aron::typenavigator::ObjectNavigatorPtr& t) const
- {
- aron::typenavigator::ObjectNavigatorPtr typeWrapped(new aron::typenavigator::ObjectNavigator());
- typeWrapped->setObjectName(t->getObjectName() + "__ltm_type_export");
- typeWrapped->addMemberType(TYPE_WRAPPER_DATA_FIELD, t);
-
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_STORED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_CREATED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_SENT_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_TIME_ARRIVED_FIELD, std::make_shared<aron::typenavigator::LongNavigator>());
- typeWrapped->addMemberType(TYPE_WRAPPER_CONFIDENCE_FIELD, std::make_shared<aron::typenavigator::DoubleNavigator>());
-
- return typeWrapped;
- }
-
- using Base::aronType;
- void readAronType(const std::filesystem::path& d)
- {
- if (std::filesystem::is_regular_file(d))
- {
- if (d.filename() == (std::string(TYPE_FILENAME) + ".json"))
- {
- std::ifstream ifs(d);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- aron::typeIO::reader::NlohmannJSONReader typeReader(file_content);
- aron::typeIO::writer::NavigatorWriter navWriter;
- aron::typeIO::Converter::ReadAndConvert(typeReader, navWriter);
- aronType = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(navWriter.getResult());
- }
- }
- }
-
- using Base::hasAronType;
- void writeAronType(const std::filesystem::path& d)
- {
- if (hasAronType())
- {
- std::ofstream ofs;
- ofs.open(d);
-
- aron::typeIO::writer::NlohmannJSONWriter typeWriter;
- aron::typeIO::Visitor::VisitAndSetup(typeWriter, aronType);
- std::string new_file_full_content = typeWriter.getResult().dump(2);
-
- ofs << new_file_full_content;
- ofs.close();
- }
- }
-
- private:
- static const constexpr char* TYPE_FILENAME = "type";
- static constexpr const char* TYPE_WRAPPER_DATA_FIELD = "__ARON_DATA";
- static constexpr const char* TYPE_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
- static constexpr const char* TYPE_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
- static constexpr const char* TYPE_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
- static constexpr const char* TYPE_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
- static constexpr const char* TYPE_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 2b61bfbac513a1854859abecf81697e5baff3aae..c44a40758a73ba64d8827c6f7a751b9df8fc00c3 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -35,16 +35,15 @@ namespace armarx::armem::error
}
- ContainerNameMismatch::ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
const std::string& ownTerm, const std::string& containerName) :
- ArMemError(makeMsg(expectedName, ownTerm, containerName))
+ ArMemError(makeMsg(gottenName, ownTerm, containerName))
{}
- std::string ContainerNameMismatch::makeMsg(
- const std::string& expectedName, const std::string& containerTerm, const std::string& containerName)
+ std::string ContainerNameMismatch::makeMsg(const std::string& gottenName, const std::string& containerTerm, const std::string& containerName)
{
std::stringstream ss;
- ss << "Name '" << expectedName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
+ ss << "Name '" << gottenName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
return ss.str();
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 068283a519d74cfe86d7ffb31345d4404e4e0d7f..3456c7a350835e6c67ca1d439d2fc0c44a8be507 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -45,10 +45,10 @@ namespace armarx::armem::error
{
public:
- ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
- static std::string makeMsg(const std::string& expectedName,
+ static std::string makeMsg(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
};
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index 0a9774020ba573f41a26f6be610e6464108ae194..d79acf1da2b86f94414205131173797e0354bc24 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -147,11 +147,11 @@ namespace armarx
void armem::detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
{
- toIce(ice.id, item.id);
+ toIce(ice.id, item.id());
}
void armem::detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
{
- fromIce(ice.id, item.id);
+ fromIce(ice.id, item.id());
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index b98a0d5efb654739de535284d816dfebc4a0b5fe..78f275fe1a8b1285b852431acaa67dc13963874d 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -8,45 +8,10 @@
namespace armarx::armem::ltm
{
- CoreSegment::CoreSegment()
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(MemoryID().withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(parentID.withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(id, aronType)
- {
- }
-
- CoreSegment::CoreSegment(const CoreSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, ProviderSegment>, CoreSegment>(other),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(other)
- {
- }
-
- CoreSegment& CoreSegment::operator=(const CoreSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
wm::CoreSegment CoreSegment::convert() const
{
wm::CoreSegment m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addProviderSegment(s.convert());
}
@@ -55,15 +20,15 @@ namespace armarx::armem::ltm
void CoreSegment::append(const wm::CoreSegment& m)
{
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
else
{
- auto wms = container.emplace(std::make_pair(k, id.withCoreSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
index 59bb7830d66e1b5ec2e15c904ae967a93534b8a3..67f9da6f465820534a4214cbb423e14916819daf 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -14,18 +14,15 @@ namespace armarx::armem::ltm
* @brief Data of a core segment containing multiple provider segments.
*/
class CoreSegment :
- virtual public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
- CoreSegment();
- CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const CoreSegment& other);
- CoreSegment& operator=(const CoreSegment& other);
+ using Base::CoreSegmentBase;
+ using Base::operator=;
+
// Conversion
wm::CoreSegment convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index c0de42c689c4844b50f5bb8858e776057471a484..15e232c01fb0edb2ce6e0bffec0f2f0b2c0a3110 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -3,36 +3,10 @@
namespace armarx::armem::ltm
{
- Entity::Entity()
- {
- }
-
- Entity::Entity(const std::string& name, const MemoryID& parentID) :
- base::detail::MemoryItem(parentID.withEntityName(name))
- {
- }
-
- Entity::Entity(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- Entity::Entity(const Entity& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<Time, EntitySnapshot>, Entity>(other)
- {
- }
-
- Entity& Entity::operator=(const Entity& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
wm::Entity Entity::convert() const
{
wm::Entity m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addSnapshot(s.convert());
}
@@ -41,15 +15,15 @@ namespace armarx::armem::ltm
void Entity::append(const wm::Entity& m)
{
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.setTo(s);
}
else
{
- auto wms = container.emplace(std::make_pair(k, id.withTimestamp(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
wms.first->second.setTo(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index 79d81340e35381228ffa8875c8723a349bdb4035..ecdf297c13fae4d24b070f2af13fd6c84ed93392 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -29,19 +29,15 @@ namespace armarx::armem::ltm
* each containing a single `AronData` object of a specific `AronType`.
*/
class Entity :
- virtual public base::EntityBase<EntitySnapshot, Entity>
+ public base::EntityBase<EntitySnapshot, Entity>
{
using Base = base::EntityBase<EntitySnapshot, Entity>;
public:
- Entity();
- Entity(const std::string& name, const MemoryID& parentID = {});
- Entity(const MemoryID& id);
-
- /// Copy the history from `other` to this.
- Entity(const Entity& other);
- /// Copy the history from `other` to this.
- Entity& operator=(const Entity& other);
+
+ using Base::EntityBase;
+ using Base::operator=;
+
// Conversion
wm::Entity convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
index 44bbca24d11d42d5741759fa2487c1434ce6a3b9..6332be7ea52f3c547101ae3ee196a524a36aab17 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
@@ -13,35 +13,9 @@ namespace armarx::armem::ltm
&& std::abs(confidence - other.confidence) < 1e-6f;
}
- EntityInstance::EntityInstance()
- {
- }
-
- EntityInstance::EntityInstance(const EntityInstance& other) :
- MemoryItem(other.id),
- _metadata(other._metadata)
- {
- }
-
- EntityInstance::EntityInstance(int index, const MemoryID& parentID) :
- EntityInstance(parentID.withInstanceIndex(index))
- {
- }
-
- EntityInstance::EntityInstance(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- EntityInstance& EntityInstance::operator=(const EntityInstance& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
- return id == other.id && _metadata == other.metadata();
+ return id() == other.id() && _metadata == other.metadata();
}
void EntityInstance::update(const EntityUpdate& update, int index)
@@ -65,7 +39,7 @@ namespace armarx::armem::ltm
void EntityInstance::_copySelf(EntityInstance& other) const
{
- EntityInstanceBase<EntityInstance>::_copySelf(other);
+ Base::_copySelf(other);
other._metadata = _metadata;
}
@@ -77,6 +51,6 @@ namespace armarx::armem::ltm
void EntityInstance::setTo(const wm::EntityInstance& m)
{
- ARMARX_IMPORTANT << "Longtermmemory received an entity instance: " << m.id.str();
+ ARMARX_IMPORTANT << "Longtermmemory received an entity instance: " << m.id().str();
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
index ee94fbfbbe9e79dd74916612e49a4a6eee0b90cc..f78668e5e1a4e1506c2b63c8d2721126e293d6c9 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
@@ -34,17 +34,19 @@ namespace armarx::armem::ltm
* @brief Data of a single entity instance.
*/
class EntityInstance :
- virtual public base::EntityInstanceBase<EntityInstance>
+ public base::EntityInstanceBase<EntityInstance>
{
using Base = base::EntityInstanceBase<EntityInstance>;
public:
- EntityInstance();
- EntityInstance(const EntityInstance&);
- EntityInstance(int index, const MemoryID& parentID = {});
- EntityInstance(const MemoryID& id);
- EntityInstance& operator=(const EntityInstance& other);
+ using Base::EntityInstanceBase;
+
+ EntityInstance(const EntityInstance& other) = default;
+ EntityInstance(EntityInstance&& other) = default;
+ EntityInstance& operator=(const EntityInstance& other) = default;
+ EntityInstance& operator=(EntityInstance&& other) = default;
+
EntityInstanceMetadata& metadata()
{
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 0cf13f0210b1e9ee6499417770ef21d18550e7ee..73d4932155562ffe213504ce3b7bccfc93690b51 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -8,37 +8,10 @@
namespace armarx::armem::ltm
{
-
- EntitySnapshot::EntitySnapshot()
- {
- }
-
- EntitySnapshot::EntitySnapshot(Time time, const MemoryID& parentID) :
- EntitySnapshot(parentID.withTimestamp(time))
- {
- }
-
- EntitySnapshot::EntitySnapshot(const MemoryID& id) :
- MemoryItem(id)
- {
- }
-
- EntitySnapshot::EntitySnapshot(const EntitySnapshot& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::vector<EntityInstance>, EntitySnapshot>(other)
- {
- }
-
- EntitySnapshot& EntitySnapshot::operator=(const EntitySnapshot& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
wm::EntitySnapshot EntitySnapshot::convert() const
{
wm::EntitySnapshot m;
- for (const auto& s : container)
+ for (const auto& s : _container)
{
m.addInstance(s.convert());
}
@@ -48,12 +21,12 @@ namespace armarx::armem::ltm
void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
{
// We remove the contente here and reset it with new values
- container.clear();
+ _container.clear();
- unsigned int i = 0;
- for (const auto& s : m.container)
+ int i = 0;
+ for (const auto& s : m.container())
{
- auto wms = container.emplace_back(id.withInstanceIndex(i++));
+ auto wms = _container.emplace_back(id().withInstanceIndex(i++));
wms.setTo(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
index b8c3f491dc8b87979856786abe931ab1fc7eaeb7..79c0341643c4619270e6b264454ce50dffd0a202 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -14,20 +14,15 @@ namespace armarx::armem::ltm
* @brief Data of an entity at one point in time.
*/
class EntitySnapshot :
- virtual public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
{
using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
public:
- EntitySnapshot();
- EntitySnapshot(Time time, const MemoryID& parentID = {});
- EntitySnapshot(const MemoryID& id);
- /// Copy the instances from `other` to this.
- EntitySnapshot(const EntitySnapshot& other);
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
- /// Copy the instances from `other` to this.
- EntitySnapshot& operator=(const EntitySnapshot& other);
// Conversion
wm::EntitySnapshot convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 9b5779c5970495171b93fb2c667cefd444182054..ac204816011ee0b760fa65a9fecea330edcadf97 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -9,37 +9,10 @@
namespace armarx::armem::ltm
{
- Memory::Memory()
- {
- }
-
- Memory::Memory(const std::string& name) :
- base::detail::MemoryItem(MemoryID().withMemoryName(name))
- {
- }
-
- Memory::Memory(const MemoryID& id) :
- MemoryItem(id)
- {
- }
-
- Memory::Memory(const Memory& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, CoreSegment>, Memory>(other)
- {
- *this = other;
- }
-
- Memory& Memory::operator=(const Memory& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
wm::Memory Memory::convert() const
{
wm::Memory m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addCoreSegment(s.convert());
}
@@ -54,15 +27,15 @@ namespace armarx::armem::ltm
void Memory::append(const wm::Memory& m)
{
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
else
{
- auto wms = container.emplace(std::make_pair(k, id.withCoreSegmentName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 1085de776eda6ac8e913d24c840b867a7fef969d..a381972a8c6670f10f286b5ecee82757c1be87d1 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -13,17 +13,14 @@ namespace armarx::armem::ltm
* @brief Data of a memory consisting of multiple core segments.
*/
class Memory :
- virtual public base::MemoryBase<CoreSegment, Memory>
+ public base::MemoryBase<CoreSegment, Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
- Memory();
- Memory(const std::string& name);
- Memory(const MemoryID& id);
- Memory(const Memory& other);
- Memory& operator=(const Memory& other);
+ using Base::MemoryBase;
+ using Base::operator=;
// Conversion
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index e527b2af574cc4a29a0e615069acfffd6901bb19..38f2a142989324cab5aa9a47f2f80d7e91fa9b24 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -8,45 +8,10 @@
namespace armarx::armem::ltm
{
- ProviderSegment::ProviderSegment()
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(MemoryID().withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(parentID.withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(parentID.withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(id, aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const ProviderSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, Entity>, ProviderSegment>(other),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(other)
- {
- }
-
- ProviderSegment& ProviderSegment::operator=(const ProviderSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
wm::ProviderSegment ProviderSegment::convert() const
{
wm::ProviderSegment m;
- for (const auto& [_, s] : container)
+ for (const auto& [_, s] : _container)
{
m.addEntity(s.convert());
}
@@ -55,15 +20,15 @@ namespace armarx::armem::ltm
void ProviderSegment::append(const wm::ProviderSegment& m)
{
- for (const auto& [k, s] : m.container)
+ for (const auto& [k, s] : m.container())
{
- if (const auto& it = container.find(k); it != container.end())
+ if (const auto& it = _container.find(k); it != _container.end())
{
it->second.append(s);
}
else
{
- auto wms = container.emplace(std::make_pair(k, id.withEntityName(k)));
+ auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
index 5ac77dad663e34a3b3da6d39c53629d6a9415864..4e81db8a9270f58d44845d08f298b37d59424323 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -14,18 +14,15 @@ namespace armarx::armem::ltm
* @brief Data of a provider segment containing multiple entities.
*/
class ProviderSegment :
- virtual public base::ProviderSegmentBase<Entity, ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
{
using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
public:
- ProviderSegment();
- ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const ProviderSegment& other);
- ProviderSegment& operator=(const ProviderSegment& other);
+ using Base::ProviderSegmentBase;
+ using Base::operator=;
+
// Conversion
wm::ProviderSegment convert() const;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index 3c9fe036452c21992c82bce0d9ecad30d33c0491..b4341c9da6b9bf6c1c9be6e1141ff712d9e00132 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -7,55 +7,13 @@
namespace armarx::armem::wm
{
-
- CoreSegment::CoreSegment()
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(MemoryID().withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withCoreSegmentName(name)),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(parentID.withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(id, aronType)
- {
- }
-
- CoreSegment::CoreSegment(const CoreSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, ProviderSegment>, CoreSegment>(other),
- base::detail::TypedEntityContainerBase<ProviderSegment, Entity, CoreSegment>(other)
- {
- }
-
- CoreSegment& CoreSegment::operator=(const CoreSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- CoreSegment CoreSegment::copyWithoutData() const
- {
- CoreSegment m;
- _copySelfWithoutData(m);
- return m;
- }
-
- void CoreSegment::_copySelfWithoutData(CoreSegment& o) const
+ void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
{
- detail::TypedEntityContainer<ProviderSegment, CoreSegment>::_copySelfWithoutData(o);
- for (const auto& [k, s] : o.container)
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
{
- o.addProviderSegment(s.copyWithoutData());
+ other.addProviderSegment(s.copyWithoutData());
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index d727b408bb9672afdbf842d742f27e889ea96489..1eaf327338929b1290e476951aa69bdde0d3723d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -3,7 +3,7 @@
#include "../base/CoreSegmentBase.h"
#include "ProviderSegment.h"
-#include "detail/TypedEntityContainer.h"
+#include "detail/CopyWithoutData.h"
namespace armarx::armem::wm
@@ -13,24 +13,27 @@ namespace armarx::armem::wm
* @brief Data of a core segment containing multiple provider segments.
*/
class CoreSegment :
- virtual public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- virtual public detail::TypedEntityContainer<ProviderSegment, CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::CopyWithoutData<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
- CoreSegment();
- CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- CoreSegment(const CoreSegment& other);
- CoreSegment& operator=(const CoreSegment& other);
+ using Base::CoreSegmentBase;
+
+ CoreSegment(const CoreSegment& other) = default;
+ CoreSegment(CoreSegment&& other) = default;
+ CoreSegment& operator=(const CoreSegment& other) = default;
+ CoreSegment& operator=(CoreSegment&& other) = default;
+
+ virtual ~CoreSegment() override = default;
- CoreSegment copyWithoutData() const override;
protected:
- virtual void _copySelfWithoutData(CoreSegment& o) const override;
+
+ virtual void _copySelfWithoutData(CoreSegment& other) const override;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
index 75fddfd55094b7f007cf7a0e4775adbd16437dde..022864c19d9ea98583d9d3998bec46643921cdc8 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
@@ -2,46 +2,13 @@
namespace armarx::armem::wm
{
-
- Entity::Entity()
- {
- }
-
- Entity::Entity(const std::string& name, const MemoryID& parentID) :
- base::detail::MemoryItem(parentID.withEntityName(name))
- {
- }
-
- Entity::Entity(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- Entity::Entity(const Entity& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<Time, EntitySnapshot>, Entity>(other)
- {
- }
-
- Entity& Entity::operator=(const Entity& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- Entity Entity::copyWithoutData() const
- {
- Entity m;
- _copySelfWithoutData(m);
- return m;
- }
-
- void Entity::_copySelfWithoutData(Entity& o) const
+ void Entity::_copySelfWithoutData(Entity& other) const
{
- detail::MemoryContainer<std::map<Time, EntitySnapshot>, Entity>::_copySelfWithoutData(o);
- for (const auto& [k, s] : o.container)
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
{
- o.addSnapshot(s.copyWithoutData());
+ other.addSnapshot(s.copyWithoutData());
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
index 9d213e92f0d6756e90cf9fd7090f472ce52effa5..6a31286722e0e78d53423f6aa6b517583f83ca09 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
@@ -3,7 +3,7 @@
#include "../base/EntityBase.h"
#include "EntitySnapshot.h"
-#include "detail/MemoryContainer.h"
+#include "detail/CopyWithoutData.h"
namespace armarx::armem::wm
@@ -28,25 +28,27 @@ namespace armarx::armem::wm
* each containing a single `AronData` object of a specific `AronType`.
*/
class Entity :
- virtual public base::EntityBase<EntitySnapshot, Entity>,
- virtual public detail::MemoryContainer<std::map<Time, EntitySnapshot>, Entity>
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::CopyWithoutData<Entity>
{
using Base = base::EntityBase<EntitySnapshot, Entity>;
public:
- Entity();
- Entity(const std::string& name, const MemoryID& parentID = {});
- Entity(const MemoryID& id);
- /// Copy the history from `other` to this.
- Entity(const Entity& other);
- /// Copy the history from `other` to this.
- Entity& operator=(const Entity& other);
+ using Base::EntityBase;
+
+ Entity(const Entity& other) = default;
+ Entity(Entity&& other) = default;
+ Entity& operator=(const Entity& other) = default;
+ Entity& operator=(Entity&& other) = default;
+
+ virtual ~Entity() override = default;
- Entity copyWithoutData() const override;
protected:
- virtual void _copySelfWithoutData(Entity& o) const override;
+
+ virtual void _copySelfWithoutData(Entity& other) const override;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
index ce62d0164e9adaefd6ea4d1ab3bc46d3c8038d9e..95e508bde1412a87c865b2720bd227ec58595114 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
@@ -3,6 +3,17 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+std::ostream& armarx::armem::wm::operator<<(std::ostream& os, const EntityInstanceMetadata& d)
+{
+ os << "EntityInstanceMetadata: "
+ << "\n- t_create = \t" << armem::toStringMicroSeconds(d.timeCreated) << " us"
+ << "\n- t_sent = \t" << armem::toStringMicroSeconds(d.timeSent) << " us"
+ << "\n- t_arrived = \t" << armem::toStringMicroSeconds(d.timeArrived) << " us"
+ << "\n- confidence = \t" << d.confidence << " us"
+ ;
+ return os;
+}
+
namespace armarx::armem::wm
{
@@ -14,37 +25,12 @@ namespace armarx::armem::wm
&& std::abs(confidence - other.confidence) < 1e-6f;
}
- EntityInstance::EntityInstance()
- {
- }
-
- EntityInstance::EntityInstance(const EntityInstance& other) :
- base::detail::MemoryItem(other.id),
- _metadata(other._metadata),
- _data(other._data)
- {
- }
-
- EntityInstance::EntityInstance(int index, const MemoryID& parentID) :
- EntityInstance(parentID.withInstanceIndex(index))
- {
- }
-
- EntityInstance::EntityInstance(const MemoryID& id) :
- base::detail::MemoryItem(id)
- {
- }
-
- EntityInstance& EntityInstance::operator=(const EntityInstance& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
- return id == other.id && _metadata == other.metadata() && _data->equalsDeep(other.data());
+ return id() == other.id()
+ && _metadata == other.metadata()
+ && _data->equalsDeep(other.data());
}
void EntityInstance::update(const EntityUpdate& update, int index)
@@ -68,22 +54,16 @@ namespace armarx::armem::wm
return d;
}
- EntityInstance EntityInstance::copyWithoutData() const
- {
- EntityInstance d;
- this->_copySelfWithoutData(d);
- return d;
- }
-
void EntityInstance::_copySelf(EntityInstance& other) const
{
- EntityInstanceBase<EntityInstance>::_copySelf(other);
+ Base::_copySelf(other);
other._metadata = _metadata;
other._data = _data;
}
void EntityInstance::_copySelfWithoutData(EntityInstance& other) const
{
- EntityInstanceBase<EntityInstance>::_copySelf(other);
+ Base::_copySelf(other);
+ other._metadata = _metadata;
}
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
index cbfe38200b129cdcbd32819560fda6cfb2cbdaa0..8e6eb1afe7e673baeab933397ebd16aa1781c18d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
@@ -2,6 +2,9 @@
#include "../base/EntityInstanceBase.h"
+#include "detail/CopyWithoutData.h"
+
+
namespace armarx::armem::wm
{
@@ -28,21 +31,27 @@ namespace armarx::armem::wm
}
};
+ std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
+
+
/**
* @brief Data of a single entity instance.
*/
class EntityInstance :
- virtual public base::EntityInstanceBase<EntityInstance>
+ public base::EntityInstanceBase<EntityInstance>,
+ public detail::CopyWithoutData<EntityInstance>
{
using Base = base::EntityInstanceBase<EntityInstance>;
public:
- EntityInstance();
- EntityInstance(const EntityInstance&);
- EntityInstance(int index, const MemoryID& parentID = {});
- EntityInstance(const MemoryID& id);
- EntityInstance& operator=(const EntityInstance& other);
+ using Base::EntityInstanceBase;
+
+ EntityInstance(const EntityInstance& other) = default;
+ EntityInstance(EntityInstance&& other) = default;
+ EntityInstance& operator=(const EntityInstance& other) = default;
+ EntityInstance& operator=(EntityInstance&& other) = default;
+
EntityInstanceMetadata& metadata()
{
@@ -74,18 +83,21 @@ namespace armarx::armem::wm
virtual bool equalsDeep(const EntityInstance& other) const override;
virtual EntityInstance copy() const override;
- virtual EntityInstance copyWithoutData() const;
+
protected:
+
virtual void _copySelf(EntityInstance& other) const override;
- virtual void _copySelfWithoutData(EntityInstance& other) const;
+ virtual void _copySelfWithoutData(EntityInstance& other) const override;
private:
+
/// The metadata.
EntityInstanceMetadata _metadata;
/// The data. May be nullptr.
armarx::aron::datanavigator::DictNavigatorPtr _data;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
index 124645de98c90eae526f5ee5f23c9e79027e6f52..fb68d050a0ad19e1e2627e4cb5732a23a2ce4493 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
@@ -7,47 +7,12 @@
namespace armarx::armem::wm
{
-
-
- EntitySnapshot::EntitySnapshot()
- {
- }
-
- EntitySnapshot::EntitySnapshot(Time time, const MemoryID& parentID) :
- EntitySnapshot(parentID.withTimestamp(time))
- {
- }
-
- EntitySnapshot::EntitySnapshot(const MemoryID& id) :
- MemoryItem(id)
- {
- }
-
- EntitySnapshot::EntitySnapshot(const EntitySnapshot& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::vector<EntityInstance>, EntitySnapshot>(other)
- {
- }
-
- EntitySnapshot& EntitySnapshot::operator=(const EntitySnapshot& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- EntitySnapshot EntitySnapshot::copyWithoutData() const
- {
- EntitySnapshot m;
- _copySelfWithoutData(m);
- return m;
- }
-
- void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& o) const
+ void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
{
- detail::MemoryContainer<std::vector<EntityInstance>, EntitySnapshot>::_copySelfWithoutData(o);
- for (const auto& s : o.container)
+ other.id() = _id;
+ for (const auto& s : _container)
{
- o.addInstance(s.copyWithoutData());
+ other.addInstance(s.copyWithoutData());
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
index 5f18288ba698fd70eed0539532a9be3002967590..ee0c78c5d201ed2e9fb1fe5c333efc728a9dccfc 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
@@ -3,7 +3,7 @@
#include "../base/EntitySnapshotBase.h"
#include "EntityInstance.h"
-#include "detail/MemoryContainer.h"
+#include "detail/CopyWithoutData.h"
namespace armarx::armem::wm
@@ -13,25 +13,20 @@ namespace armarx::armem::wm
* @brief Data of an entity at one point in time.
*/
class EntitySnapshot :
- virtual public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
- virtual public detail::MemoryContainer<std::vector<EntityInstance>, EntitySnapshot>
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
+ public detail::CopyWithoutData<EntitySnapshot>
{
using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
public:
- EntitySnapshot();
- EntitySnapshot(Time time, const MemoryID& parentID = {});
- EntitySnapshot(const MemoryID& id);
- /// Copy the instances from `other` to this.
- EntitySnapshot(const EntitySnapshot& other);
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
- /// Copy the instances from `other` to this.
- EntitySnapshot& operator=(const EntitySnapshot& other);
-
- EntitySnapshot copyWithoutData() const override;
protected:
- virtual void _copySelfWithoutData(EntitySnapshot& o) const override;
+
+ virtual void _copySelfWithoutData(EntitySnapshot& other) const override;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index 706d26bcc3a94a8ddb0b0da6015abdc664857fb4..b7af1a6456687b1741710d46fa157b9091295db7 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -1,54 +1,14 @@
#include "Memory.h"
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
namespace armarx::armem::wm
{
-
- Memory::Memory()
- {
- }
-
- Memory::Memory(const std::string& name) :
- base::detail::MemoryItem(MemoryID().withMemoryName(name))
- {
- }
-
- Memory::Memory(const MemoryID& id) :
- MemoryItem(id)
- {
- }
-
- Memory::Memory(const Memory& other) :
- base::detail::MemoryItem(other),
- MemoryContainerBase<std::map<std::string, CoreSegment>, Memory>(other)
- {
- *this = other;
- }
-
- Memory& Memory::operator=(const Memory& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- Memory Memory::copyWithoutData() const
- {
- Memory m;
- _copySelfWithoutData(m);
- return m;
- }
-
- void Memory::_copySelfWithoutData(Memory& o) const
+ void Memory::_copySelfWithoutData(Memory& other) const
{
- detail::EntityContainer<CoreSegment, Memory>::_copySelfWithoutData(o);
- for (const auto& [k, s] : o.container)
+ other.id() = _id;
+ for (const auto& [k, s] : _container)
{
- o.addCoreSegment(s.copyWithoutData());
+ 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 d747ded5ef9618a9c4a2c891c493e3816c15a851..e8b7cb35349837820c3dd47f2d584ef6b9760461 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -3,7 +3,8 @@
#include "../base/MemoryBase.h"
#include "CoreSegment.h"
-#include "detail/EntityContainer.h"
+#include "detail/CopyWithoutData.h"
+
namespace armarx::armem::wm
{
@@ -12,22 +13,24 @@ namespace armarx::armem::wm
* @brief Data of a memory consisting of multiple core segments.
*/
class Memory :
- virtual public base::MemoryBase<CoreSegment, Memory>,
- virtual public detail::EntityContainer<CoreSegment, Memory>
+ public base::MemoryBase<CoreSegment, Memory>,
+ public detail::CopyWithoutData<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
- Memory();
- Memory(const std::string& name);
- Memory(const MemoryID& id);
- Memory(const Memory& other);
- Memory& operator=(const Memory& other);
+ using Base::MemoryBase;
+
+ Memory(const Memory& other) = default;
+ Memory(Memory&& other) = default;
+ Memory& operator=(const Memory& other) = default;
+ Memory& operator=(Memory&& other) = default;
- Memory copyWithoutData() const override;
protected:
- virtual void _copySelfWithoutData(Memory& o) const override;
+
+ virtual void _copySelfWithoutData(Memory& other) const override;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
index dbf436b65b078053a6405892b7f89082e89598ee..517b35bfad666cdac02041694e787523c4c1e894 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
@@ -8,54 +8,13 @@
namespace armarx::armem::wm
{
- ProviderSegment::ProviderSegment()
+ void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
{
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(MemoryID().withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(MemoryID().withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(parentID.withProviderSegmentName(name)),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(parentID.withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- base::detail::MemoryItem(id),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(id, aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const ProviderSegment& other) :
- base::detail::MemoryItem(other),
- base::detail::MemoryContainerBase<std::map<std::string, Entity>, ProviderSegment>(other),
- base::detail::TypedEntityContainerBase<Entity, Entity, ProviderSegment>(other)
- {
- }
-
- ProviderSegment& ProviderSegment::operator=(const ProviderSegment& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- ProviderSegment ProviderSegment::copyWithoutData() const
- {
- ProviderSegment m;
- _copySelfWithoutData(m);
- return m;
- }
-
- void ProviderSegment::_copySelfWithoutData(ProviderSegment& o) const
- {
- detail::TypedEntityContainer<Entity, ProviderSegment>::_copySelfWithoutData(o);
- for (const auto& [k, s] : o.container)
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
{
- o.addEntity(s.copyWithoutData());
+ other.addEntity(s.copyWithoutData());
}
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
index 542ec3dc5ee0fc2dcbbface02bf11006eaa497a1..0b703617d9b1b642153da505882e71aab8aa3723 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
@@ -3,7 +3,7 @@
#include "../base/ProviderSegmentBase.h"
#include "Entity.h"
-#include "detail/TypedEntityContainer.h"
+#include "detail/CopyWithoutData.h"
namespace armarx::armem::wm
@@ -13,24 +13,28 @@ namespace armarx::armem::wm
* @brief Data of a provider segment containing multiple entities.
*/
class ProviderSegment :
- virtual public base::ProviderSegmentBase<Entity, ProviderSegment>,
- virtual public detail::TypedEntityContainer<Entity, ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::CopyWithoutData<ProviderSegment>
+
{
using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
public:
- ProviderSegment();
- ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- ProviderSegment(const ProviderSegment& other);
- ProviderSegment& operator=(const ProviderSegment& other);
+ using Base::ProviderSegmentBase;
+
+ ProviderSegment(const ProviderSegment& other) = default;
+ ProviderSegment(ProviderSegment&& other) = default;
+ ProviderSegment& operator=(const ProviderSegment& other) = default;
+ ProviderSegment& operator=(ProviderSegment&& other) = default;
+
+ virtual ~ProviderSegment() override = default;
- ProviderSegment copyWithoutData() const override;
protected:
- virtual void _copySelfWithoutData(ProviderSegment& o) const override;
+
+ virtual void _copySelfWithoutData(ProviderSegment& other) const override;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..986612d1e788bee74e572f20b89dbc78fb99ea58
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
@@ -0,0 +1,151 @@
+#include "Visitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+ Visitor::Visitor()
+ {
+ }
+
+ Visitor::~Visitor()
+ {
+ }
+
+ bool Visitor::applyTo(Memory& memory)
+ {
+ for (auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(CoreSegment& coreSegment)
+ {
+ for (auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(ProviderSegment& providerSegment)
+ {
+ for (auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(Entity& entity)
+ {
+ for (auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntitySnapshot& snapshot)
+ {
+ for (auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+
+ bool Visitor::applyTo(const Memory& memory)
+ {
+ for (const auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const CoreSegment& coreSegment)
+ {
+ for (const auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const ProviderSegment& providerSegment)
+ {
+ for (const auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const Entity& entity)
+ {
+ for (const auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntitySnapshot& snapshot)
+ {
+ for (const auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..c0a3854a242c030113eeb46409c172563660294a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
@@ -0,0 +1,152 @@
+#pragma once
+
+
+namespace armarx::armem::wm
+{
+ class Memory;
+ class CoreSegment;
+ class ProviderSegment;
+ class Entity;
+ class EntitySnapshot;
+ class EntityInstance;
+
+
+ /**
+ * @brief A visitor for the hierarchical Memory data structure.
+ */
+ class Visitor
+ {
+
+ public:
+
+ Visitor();
+ virtual ~Visitor();
+
+ bool applyTo(Memory& memory);
+ bool applyTo(CoreSegment& coreSegment);
+ bool applyTo(ProviderSegment& providerSegment);
+ bool applyTo(Entity& entity);
+ bool applyTo(EntitySnapshot& snapshot);
+ bool applyTo(EntityInstance& instance);
+
+
+ virtual bool visitEnter(Memory& memory)
+ {
+ return visitEnter(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitEnter(CoreSegment& coreSegment)
+ {
+ return visitEnter(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitEnter(ProviderSegment& providerSegment)
+ {
+ return visitEnter(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitEnter(Entity& entity)
+ {
+ return visitEnter(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitEnter(EntitySnapshot& snapshot)
+ {
+ return visitEnter(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visitExit(Memory& memory)
+ {
+ return visitExit(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitExit(CoreSegment& coreSegment)
+ {
+ return visitExit(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitExit(ProviderSegment& providerSegment)
+ {
+ return visitExit(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitExit(Entity& entity)
+ {
+ return visitExit(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitExit(EntitySnapshot& snapshot)
+ {
+ return visitExit(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visit(EntityInstance& instance)
+ {
+ return visit(const_cast<const EntityInstance&>(instance));
+ }
+
+
+
+ // Const versions
+
+ bool applyTo(const Memory& memory);
+ bool applyTo(const CoreSegment& coreSegment);
+ bool applyTo(const ProviderSegment& providerSegment);
+ bool applyTo(const Entity& entity);
+ bool applyTo(const EntitySnapshot& snapshot);
+ bool applyTo(const EntityInstance& instance);
+
+
+ virtual bool visitEnter(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitEnter(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitEnter(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitEnter(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitEnter(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visitExit(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitExit(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitExit(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitExit(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitExit(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visit(const EntityInstance& instance)
+ {
+ (void) instance;
+ return true;
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..70441033dd0fcb24a502bcd4e552764c7c2f5d3c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.cpp
@@ -0,0 +1 @@
+#include "CopyWithoutData.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h
new file mode 100644
index 0000000000000000000000000000000000000000..caabd1e6002c803aa6a1015afd7ce660abd2482f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/detail/CopyWithoutData.h
@@ -0,0 +1,30 @@
+#pragma once
+
+
+namespace armarx::armem::wm::detail
+{
+
+ /**
+ * @class Allows copying `*this` without data in the leaf
+ * data structures.
+ */
+ template <class DerivedT>
+ class CopyWithoutData
+ {
+ public:
+
+ /// Get a copy of `this` without data.
+ virtual DerivedT copyWithoutData() const
+ {
+ DerivedT t;
+ _copySelfWithoutData(t);
+ return t;
+ }
+
+
+ protected:
+
+ virtual void _copySelfWithoutData(DerivedT& other) const = 0;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.cpp
deleted file mode 100644
index 20872a805368246aca047563e0094b055568e8db..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "EntityContainer.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.h b/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.h
deleted file mode 100644
index 431d13624ab3f004020d13ba77c3fdf4a60c380d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/EntityContainer.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include "../../base/detail/EntityContainerBase.h"
-#include "MemoryContainer.h"
-
-#include "../Entity.h"
-#include "../EntitySnapshot.h"
-#include "../EntityInstance.h"
-
-
-namespace armarx::armem::wm::detail
-{
-
- /**
- * @brief A container of entities at some point in the hierarchy.
- *
- * Can be updated by multiple entity updates.
- */
- template <class _ValueT, class _Derived>
- class EntityContainer :
- virtual public base::detail::EntityContainerBase<_ValueT, Entity, _Derived>,
- virtual public MemoryContainer<std::map<std::string, _ValueT>, _Derived>
- {
- public:
- EntityContainer& operator=(const EntityContainer& other)
- {
- other._copySelf(*this);
- return *this;
- }
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.cpp
deleted file mode 100644
index 733cd6f240a52944d6c18fb60bab928242b5b122..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "MemoryContainer.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.h b/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.h
deleted file mode 100644
index a915c8714a299ab18a671c18814c77441d6b28af..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/MemoryContainer.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../../error.h"
-
-#include "../../base/detail/MemoryContainerBase.h"
-
-
-namespace armarx::armem::wm::detail
-{
-
- /**
- * @class Provides default implmentations of `MemoryContainer`, as well as
- * iterators (which requires a template).
- */
- template <class _ContainerT, class _Derived>
- class MemoryContainer :
- virtual public base::detail::MemoryContainerBase<_ContainerT, _Derived>
- {
- using Base = base::detail::MemoryContainerBase<_ContainerT, _Derived>;
-
- public:
- MemoryContainer& operator=(const MemoryContainer& other)
- {
- other._copySelf(*this);
- return *this;
- }
-
- // Copying
- virtual _Derived copyWithoutData() const
- {
- _Derived t;
- _copySelfWithoutData(t);
- return t;
- }
-
- protected:
- virtual void _copySelfWithoutData(_Derived& o) const
- {
- Base::_copySelfEmpty(o);
- }
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.cpp
deleted file mode 100644
index 7bec77f349c1e45f7a434d499a5a8b1c952b3317..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "TypedEntityContainer.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.h b/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.h
deleted file mode 100644
index f90925ff1de57a63542dd70adadff1e762224783..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/workingmemory/detail/TypedEntityContainer.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#pragma once
-
-#include "../../base/detail/TypedEntityContainerBase.h"
-#include "EntityContainer.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-namespace armarx::armem::wm::detail
-{
-
- /**
- * @brief An entity container with a specific (Aron) type.
- */
- template <class _ValueT, class _Derived>
- class TypedEntityContainer :
- virtual public base::detail::TypedEntityContainerBase<_ValueT, Entity, _Derived>,
- virtual public EntityContainer<_ValueT, _Derived>
- {
- public:
- TypedEntityContainer& operator=(const TypedEntityContainer& other)
- {
- other._copySelf(*this);
- return *this;
- }
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
index ad77c6dc16feb2f119051ef1b0d0cecc29c9b650..7dded894331809986a991351f0eb7c7675b11812 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
@@ -43,26 +43,26 @@ namespace armarx::armem
{
detail::toIceItem(ice, snapshot);
- toIce(ice.instances, snapshot.container);
+ toIce(ice.instances, snapshot.instances());
}
void fromIce(const data::EntitySnapshot& ice, wm::EntitySnapshot& snapshot)
{
detail::fromIceItem(ice, snapshot);
- fromIce(ice.instances, snapshot.container);
+ fromIce(ice.instances, snapshot.instances());
}
void toIce(data::Entity& ice, const wm::Entity& entity)
{
detail::toIceItem(ice, entity);
- toIce(ice.history, entity.container);
+ toIce(ice.history, entity.history());
}
void fromIce(const data::Entity& ice, wm::Entity& entity)
{
detail::fromIceItem(ice, entity);
- fromIce(ice.history, entity.container);
+ fromIce(ice.history, entity.history());
}
@@ -72,9 +72,9 @@ namespace armarx::armem
if (providerSegment.hasAronType())
{
- ice.aronType = providerSegment.aronType->getResult();
+ ice.aronType = providerSegment.aronType()->getResult();
}
- toIce(ice.entities, providerSegment.container);
+ toIce(ice.entities, providerSegment.entities());
}
void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment)
{
@@ -82,9 +82,9 @@ namespace armarx::armem
if (ice.aronType)
{
- providerSegment.aronType = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
+ providerSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
}
- fromIce(ice.entities, providerSegment.container);
+ fromIce(ice.entities, providerSegment.entities());
}
void toIce(data::CoreSegment& ice, const wm::CoreSegment& coreSegment)
@@ -93,9 +93,9 @@ namespace armarx::armem
if (coreSegment.hasAronType())
{
- ice.aronType = coreSegment.aronType->getResult();
+ ice.aronType = coreSegment.aronType()->getResult();
}
- toIce(ice.providerSegments, coreSegment.container);
+ toIce(ice.providerSegments, coreSegment.providerSegments());
}
void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment)
{
@@ -103,22 +103,22 @@ namespace armarx::armem
if (ice.aronType)
{
- coreSegment.aronType = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
+ coreSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
}
- fromIce(ice.providerSegments, coreSegment.container);
+ fromIce(ice.providerSegments, coreSegment.providerSegments());
}
void toIce(data::Memory& ice, const wm::Memory& memory)
{
detail::toIceItem(ice, memory);
- toIce(ice.coreSegments, memory.container);
+ toIce(ice.coreSegments, memory.coreSegments());
}
void fromIce(const data::Memory& ice, wm::Memory& memory)
{
detail::fromIceItem(ice, memory);
- fromIce(ice.coreSegments, memory.container);
+ fromIce(ice.coreSegments, memory.coreSegments());
}
}
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
index a818ca771ba76e29aa5feddd4b9ac8ac2e5377e8..4ebc9aad33392ffdf48c019882b14217638174cf 100644
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
+++ b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
@@ -76,7 +76,7 @@ namespace armarx::armem::mns::plugins
mns::MemoryNameSystemInterfacePrx memoryNameSystem = nullptr;
bool memoryNameSystemEnabled = true;
- std::string memoryNameSystemName = "ArMemMemoryNameSystem";
+ std::string memoryNameSystemName = "MemoryNameSystem";
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index c5aff2156b36fc76b06920ba82a2db44539b1f19..536c72f49d6ccb4c656f37c345dc6a4ec70b4f17 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -41,7 +41,7 @@ namespace armarx::armem::server::plugins
settings.user = longTermMemoryDatabaseUser;
settings.password = longTermMemoryDatabasePassword;
- parent.longtermmemory.reload(settings);
+ parent.longtermMemory.reload(settings);
}
@@ -58,7 +58,7 @@ namespace armarx::armem::server::plugins
data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
{
data::RegisterMemoryInput input;
- input.name = parent.workingmemory.name();
+ input.name = parent.workingMemory.name();
input.proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
ARMARX_CHECK_NOT_NULL(input.proxy);
data::RegisterMemoryResult result = parent.memoryNameSystem->registerMemory(input);
@@ -81,7 +81,7 @@ namespace armarx::armem::server::plugins
try
{
data::RemoveMemoryInput input;
- input.name = parent.workingmemory.name();
+ input.name = parent.workingMemory.name();
result = parent.memoryNameSystem->removeMemory(input);
if (result.success)
{
@@ -122,7 +122,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;
}
@@ -130,7 +130,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);
}
@@ -138,15 +138,15 @@ 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
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(workingmemoryMutex);
- std::scoped_lock lock2(longtermmemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
+ std::scoped_lock lock2(longtermMemoryMutex);
return iceMemory.store(input);
}
@@ -154,7 +154,7 @@ namespace armarx::armem::server
// LTM STORING
armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(longtermmemoryMutex);
+ std::scoped_lock lock(longtermMemoryMutex);
return iceMemory.load(input);
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index c051995c4c6790731850bc4294e9dba59155a795..fc2154f59a0d640a2b6d6e07f5df51465caa2812 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -96,11 +96,11 @@ namespace armarx::armem::server
public:
/// The actual memory.
- wm::Memory workingmemory;
- std::mutex workingmemoryMutex;
+ wm::Memory workingMemory;
+ std::mutex workingMemoryMutex;
- ltm::Memory longtermmemory;
- std::mutex longtermmemoryMutex;
+ ltm::Memory longtermMemory;
+ std::mutex longtermMemoryMutex;
/// property defaults
std::string memoryListenerDefaultName = "MemoryUpdates";
@@ -109,7 +109,7 @@ namespace armarx::armem::server
std::string longTermMemoryDatabasePasswordDefault = "";
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceMemory { &workingmemory, &longtermmemory};
+ MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
private:
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 68904e536e5a5b8d7fc6a38a92ed77655dd1d2fd..6895cf4e5b27ecec5872a38890d8111b41405fa7 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -12,7 +12,8 @@
namespace armarx::armem::server
{
- MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingmemory, ltm::Memory* longtermmemory) : workingmemory(workingmemory), longtermmemory(longtermmemory)
+ MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, ltm::Memory* longtermMemory) :
+ workingMemory(workingMemory), longtermMemory(longtermMemory)
{
}
@@ -20,7 +21,7 @@ namespace armarx::armem::server
void
MemoryToIceAdapter::setMemoryListener(client::MemoryListenerInterfacePrx memoryListener)
{
- this->memoryListener = memoryListener;
+ this->memoryListenerTopic = memoryListener;
}
@@ -29,20 +30,20 @@ namespace armarx::armem::server
MemoryToIceAdapter::addSegment(const data::AddSegmentInput& input, bool addCoreSegments)
{
ARMARX_DEBUG << "Adding segment '" << input.coreSegmentName << "/" << input.providerSegmentName << "'.";
- ARMARX_CHECK_NOT_NULL(workingmemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentResult output;
armem::wm::CoreSegment* coreSegment = nullptr;
try
{
- coreSegment = &workingmemory->getCoreSegment(input.coreSegmentName);
+ coreSegment = &workingMemory->getCoreSegment(input.coreSegmentName);
}
catch (const armem::error::MissingEntry& e)
{
if (addCoreSegments)
{
- coreSegment = &workingmemory->addCoreSegment(input.coreSegmentName);
+ coreSegment = &workingMemory->addCoreSegment(input.coreSegmentName);
}
else
{
@@ -71,7 +72,7 @@ namespace armarx::armem::server
}
armem::MemoryID segmentID;
- segmentID.memoryName = workingmemory->name();
+ segmentID.memoryName = workingMemory->name();
segmentID.coreSegmentName = input.coreSegmentName;
segmentID.providerSegmentName = input.providerSegmentName;
@@ -84,7 +85,7 @@ namespace armarx::armem::server
data::AddSegmentsResult
MemoryToIceAdapter::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- ARMARX_CHECK_NOT_NULL(workingmemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentsResult output;
for (const auto& i : input)
@@ -97,7 +98,7 @@ namespace armarx::armem::server
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
- ARMARX_CHECK_NOT_NULL(workingmemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
armem::Commit commit;
armem::fromIce(commitIce, commit, timeArrived);
@@ -121,7 +122,7 @@ namespace armarx::armem::server
MemoryToIceAdapter::commit(const armem::Commit& commit)
{
std::vector<data::MemoryID> updatedIDs;
- const bool publishUpdates = memoryListener;
+ const bool publishUpdates = memoryListenerTopic;
CommitResult commitResult;
for (const EntityUpdate& update : commit.updates)
@@ -129,7 +130,7 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- MemoryID snapshotID = workingmemory->update(update);
+ MemoryID snapshotID = workingMemory->update(update);
result.success = true;
result.snapshotID = snapshotID;
@@ -150,7 +151,7 @@ namespace armarx::armem::server
if (publishUpdates)
{
- memoryListener->memoryUpdated(updatedIDs);
+ memoryListenerTopic->memoryUpdated(updatedIDs);
}
return commitResult;
@@ -161,18 +162,18 @@ namespace armarx::armem::server
armem::query::data::Result
MemoryToIceAdapter::query(const armem::query::data::Input& input)
{
- ARMARX_CHECK_NOT_NULL(workingmemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
armem::wm::query_proc::MemoryQueryProcessor processor(
input.withData ? armem::DataMode::WithData
: armem::DataMode::NoData);
- return processor.process(input, *workingmemory);
+ return processor.process(input, *workingMemory);
}
// LTM LOADING
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
- ARMARX_CHECK_NOT_NULL(longtermmemory);
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
query::data::Result output;
output.success = true;
@@ -182,8 +183,8 @@ namespace armarx::armem::server
// LTM STORING
data::StoreResult MemoryToIceAdapter::store(const armem::data::StoreInput& input)
{
- ARMARX_CHECK_NOT_NULL(workingmemory);
- ARMARX_CHECK_NOT_NULL(longtermmemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
data::StoreResult output;
armem::query::data::Result queryResult = this->query(input.query);
@@ -191,7 +192,7 @@ namespace armarx::armem::server
{
wm::Memory m;
fromIce(queryResult.memory, m);
- longtermmemory->append(m);
+ longtermMemory->append(m);
}
else
{
@@ -202,4 +203,9 @@ 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 0e22503872001e384a9a9e62f13f280bf0ba17c4..919c5fc8dc74ec3f742ac1c33365bc2c680ac768 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -3,8 +3,9 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/longtermmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
namespace armarx::armem::server
@@ -22,9 +23,9 @@ namespace armarx::armem::server
public:
/// Construct an MemoryToIceAdapter from an existing Memory.
- MemoryToIceAdapter(wm::Memory* workingmemory = nullptr, ltm::Memory* longtermmemory = nullptr);
+ MemoryToIceAdapter(wm::Memory* workingMemory = nullptr, ltm::Memory* longtermMemory = nullptr);
- void setMemoryListener(client::MemoryListenerInterfacePrx memoryListener);
+ void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
// WRITING
@@ -41,6 +42,7 @@ namespace armarx::armem::server
// READING
query::data::Result query(const armem::query::data::Input& input);
+ client::QueryResult query(const client::QueryInput& input);
// LTM LOADING
query::data::Result load(const armem::query::data::Input& input);
@@ -50,10 +52,10 @@ namespace armarx::armem::server
public:
- wm::Memory* workingmemory;
- ltm::Memory* longtermmemory;
+ wm::Memory* workingMemory;
+ ltm::Memory* longtermMemory;
- client::MemoryListenerInterfacePrx memoryListener;
+ client::MemoryListenerInterfacePrx memoryListenerTopic;
};
diff --git a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
index 1d016e78374082167a82730796e1950d09c158ac..a835f39bef74f745b22c4ee167e4755a031fec50 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
@@ -54,10 +54,10 @@ BOOST_AUTO_TEST_CASE(test_entity)
armem::data::EntityPtr ice;
armem::toIce(ice, entity);
- BOOST_CHECK_EQUAL(ice->id.memoryName, entity.id.memoryName);
- BOOST_CHECK_EQUAL(ice->id.coreSegmentName, entity.id.coreSegmentName);
- BOOST_CHECK_EQUAL(ice->id.providerSegmentName, entity.id.providerSegmentName);
- BOOST_CHECK_EQUAL(ice->id.entityName, entity.id.entityName);
+ BOOST_CHECK_EQUAL(ice->id.memoryName, entity.id().memoryName);
+ BOOST_CHECK_EQUAL(ice->id.coreSegmentName, entity.id().coreSegmentName);
+ BOOST_CHECK_EQUAL(ice->id.providerSegmentName, entity.id().providerSegmentName);
+ BOOST_CHECK_EQUAL(ice->id.entityName, entity.id().entityName);
BOOST_CHECK_EQUAL(ice->history.size(), entity.history().size());
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
index 1f94d9adb9a2d023a3813c0e09edf8b41367ace1..aad3359b0d57cfed79d463cc27dcced44550ef00 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
@@ -137,3 +137,30 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
}
+
+BOOST_AUTO_TEST_CASE(test_MemoryID_copy_move_ctors_ops)
+{
+ const armem::MemoryID id("A/B/C/123/1"), moved("////1"); // int is not moved
+ {
+ const armem::MemoryID out(id);
+ BOOST_CHECK_EQUAL(out, id);
+ }
+ {
+ armem::MemoryID out;
+ out = id;
+ BOOST_CHECK_EQUAL(out, id);
+ }
+ {
+ armem::MemoryID in = id;
+ const armem::MemoryID out(std::move(in));
+ BOOST_CHECK_EQUAL(in, moved);
+ BOOST_CHECK_EQUAL(out, id);
+ }
+ {
+ armem::MemoryID in = id;
+ armem::MemoryID out;
+ out = std::move(in);
+ BOOST_CHECK_EQUAL(in, moved);
+ BOOST_CHECK_EQUAL(out, id);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
index b852295ff0e9957deabc8cbd901fd628f148b4ff..3c256468935e5be3a1da55e67359c8a6a079feec 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -26,17 +26,19 @@
#include <RobotAPI/Test.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/error.h"
-
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/diskmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <iostream>
+#include <SimoxUtility/meta/type_name.h>
#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+
namespace armem = armarx::armem;
namespace aron = armarx::aron;
-
BOOST_AUTO_TEST_CASE(test_time_to_string)
{
// 111111: seconds, 345: milliseconds, 789: microseconds
@@ -76,6 +78,504 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
}
+
+
+namespace ArMemMemoryTest
+{
+ struct TestMemoryItem : public armem::base::detail::MemoryItem
+ {
+ using MemoryItem::MemoryItem;
+ using MemoryItem::operator=;
+
+ std::string getKeyString() const override
+ {
+ return "";
+ }
+ std::string getLevelName() const override
+ {
+ return "";
+ }
+ };
+ struct MemoryItemCtorOpTestFixture
+ {
+ const armem::MemoryID id {"A/B/C/123/1"};
+ const armem::MemoryID moved {"////1"}; // int is not moved
+ TestMemoryItem item { id };
+
+ MemoryItemCtorOpTestFixture()
+ {
+ BOOST_CHECK_EQUAL(item.id(), id);
+ }
+ };
+}
+
+
+BOOST_FIXTURE_TEST_SUITE(MemoryItemTest, ArMemMemoryTest::MemoryItemCtorOpTestFixture)
+
+
+BOOST_AUTO_TEST_CASE(test_copy_ctor)
+{
+ const ArMemMemoryTest::TestMemoryItem out(item);
+ BOOST_CHECK_EQUAL(item.id(), id);
+ BOOST_CHECK_EQUAL(out.id(), id);
+}
+BOOST_AUTO_TEST_CASE(test_copy_op)
+{
+ ArMemMemoryTest::TestMemoryItem out;
+ out = item;
+ BOOST_CHECK_EQUAL(item.id(), id);
+ BOOST_CHECK_EQUAL(out.id(), id);
+}
+BOOST_AUTO_TEST_CASE(test_move_ctor)
+{
+ ArMemMemoryTest::TestMemoryItem in = item;
+ const ArMemMemoryTest::TestMemoryItem out(std::move(in));
+ BOOST_CHECK_EQUAL(in.id(), moved);
+ BOOST_CHECK_EQUAL(out.id(), id);
+}
+BOOST_AUTO_TEST_CASE(test_move_op)
+{
+ ArMemMemoryTest::TestMemoryItem in = item;
+ ArMemMemoryTest::TestMemoryItem out;
+ out = std::move(in);
+ BOOST_CHECK_EQUAL(in.id(), moved);
+ BOOST_CHECK_EQUAL(out.id(), id);
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
+
+
+
+namespace ArMemMemoryTest
+{
+ struct TestMemoryContainer : public armem::base::detail::MemoryContainerBase<std::vector<int>, TestMemoryContainer>
+ {
+ using MemoryContainerBase::MemoryContainerBase;
+ using MemoryContainerBase::operator=;
+
+ std::string getKeyString() const override
+ {
+ return "";
+ }
+ std::string getLevelName() const override
+ {
+ return "";
+ }
+ };
+ struct MemoryContainerCtorOpTestFixture
+ {
+ const armem::MemoryID id {"A/B/C/123/1"};
+ const armem::MemoryID moved {"////1"}; // int is not moved
+ TestMemoryContainer cont {id};
+
+ MemoryContainerCtorOpTestFixture()
+ {
+ cont.container() = std::vector<int>{ -1, 2, -3 };
+ BOOST_CHECK_EQUAL(cont.id(), id);
+ BOOST_CHECK_EQUAL(cont.size(), 3);
+ }
+ };
+
+}
+
+BOOST_FIXTURE_TEST_SUITE(MemoryContainerTest, ArMemMemoryTest::MemoryContainerCtorOpTestFixture)
+
+BOOST_AUTO_TEST_CASE(test_copy_ctor)
+{
+ const ArMemMemoryTest::TestMemoryContainer out(cont);
+ BOOST_CHECK_EQUAL(cont.id(), id);
+ BOOST_CHECK_EQUAL(cont.size(), 3);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 3);
+}
+BOOST_AUTO_TEST_CASE(test_copy_op)
+{
+ ArMemMemoryTest::TestMemoryContainer out;
+ out = cont;
+ BOOST_CHECK_EQUAL(cont.id(), id);
+ BOOST_CHECK_EQUAL(cont.size(), 3);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 3);
+}
+BOOST_AUTO_TEST_CASE(test_move_ctor)
+{
+ ArMemMemoryTest::TestMemoryContainer in = cont;
+ const ArMemMemoryTest::TestMemoryContainer out(std::move(in));
+ BOOST_CHECK_EQUAL(in.id(), moved);
+ BOOST_CHECK_EQUAL(in.size(), 0);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 3);
+}
+BOOST_AUTO_TEST_CASE(test_move_op)
+{
+ ArMemMemoryTest::TestMemoryContainer in = cont;
+ ArMemMemoryTest::TestMemoryContainer out;
+ out = std::move(in);
+ BOOST_CHECK_EQUAL(in.id(), moved);
+ BOOST_CHECK_EQUAL(in.size(), 0);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 3);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
+
+
+
+BOOST_AUTO_TEST_CASE(test_key_ctors)
+{
+ armem::wm::EntityInstance instance(10);
+ BOOST_CHECK_EQUAL(instance.index(), 10);
+
+ armem::wm::EntitySnapshot snapshot(armem::Time::milliSeconds(100));
+ BOOST_CHECK_EQUAL(snapshot.time(), armem::Time::milliSeconds(100));
+
+ armem::wm::Entity entity("entity");
+ BOOST_CHECK_EQUAL(entity.name(), "entity");
+
+ armem::wm::ProviderSegment provSeg("provSeg");
+ BOOST_CHECK_EQUAL(provSeg.name(), "provSeg");
+
+ armem::wm::CoreSegment coreSeg("coreSeg");
+ BOOST_CHECK_EQUAL(coreSeg.name(), "coreSeg");
+
+ armem::wm::Memory memory("memory");
+ BOOST_CHECK_EQUAL(memory.name(), "memory");
+}
+
+
+template <class ...Args>
+auto add_element(std::vector<Args...>& vector)
+{
+ return vector.emplace_back();
+}
+template <class ...Args>
+auto add_element(std::map<Args...>& map)
+{
+ return map.emplace();
+}
+
+
+template <class T>
+struct CustomChecks
+{
+ static void checkEqual(const T& lhs, const T& rhs)
+ {
+ (void) lhs, (void) rhs;
+ }
+ static void checkMoved(const T& moved)
+ {
+ (void) moved;
+ }
+};
+
+
+template <class T>
+void checkEqual_d_ltm(const T& lhs, const T& rhs)
+{
+ BOOST_CHECK_EQUAL(lhs.path, rhs.path);
+}
+template <class T>
+void checkMoved_d_ltm(const T& moved)
+{
+ BOOST_CHECK_EQUAL(moved.path, nullptr);
+}
+
+template <>
+struct CustomChecks<armem::wm::EntityInstance>
+{
+ static void checkEqual(const armem::wm::EntityInstance& lhs, const armem::wm::EntityInstance& rhs)
+ {
+ BOOST_CHECK_EQUAL(lhs.metadata(), rhs.metadata());
+ BOOST_CHECK_EQUAL(lhs.data(), rhs.data());
+ }
+ static void checkMoved(const armem::wm::EntityInstance& moved)
+ {
+ BOOST_CHECK_EQUAL(moved.data(), nullptr);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::EntityInstance>
+{
+ static void checkEqual(const armem::d_ltm::EntityInstance& lhs, const armem::d_ltm::EntityInstance& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::EntityInstance& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::EntitySnapshot>
+{
+ static void checkEqual(const armem::d_ltm::EntitySnapshot& lhs, const armem::d_ltm::EntitySnapshot& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::EntitySnapshot& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::Entity>
+{
+ static void checkEqual(const armem::d_ltm::Entity& lhs, const armem::d_ltm::Entity& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::Entity& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::ProviderSegment>
+{
+ static void checkEqual(const armem::d_ltm::ProviderSegment& lhs, const armem::d_ltm::ProviderSegment& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::ProviderSegment& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::CoreSegment>
+{
+ static void checkEqual(const armem::d_ltm::CoreSegment& lhs, const armem::d_ltm::CoreSegment& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::CoreSegment& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+template <>
+struct CustomChecks<armem::d_ltm::Memory>
+{
+ static void checkEqual(const armem::d_ltm::Memory& lhs, const armem::d_ltm::Memory& rhs)
+ {
+ checkEqual_d_ltm(lhs, rhs);
+ }
+ static void checkMoved(const armem::d_ltm::Memory& moved)
+ {
+ checkMoved_d_ltm(moved);
+ }
+};
+
+
+struct CopyMoveCtorsOpsTestBase
+{
+ const armem::MemoryID id {"A/B/C/123000"}; // int index would not be moved
+ const armem::MemoryID idMoved;
+
+ std::string typeName;
+
+ CopyMoveCtorsOpsTestBase(const std::string& typeName) :
+ typeName(typeName)
+ {
+ }
+ virtual ~CopyMoveCtorsOpsTestBase()
+ {
+ }
+
+
+ void test()
+ {
+ BOOST_TEST_CONTEXT("Type " << typeName)
+ {
+ reset();
+ BOOST_TEST_CONTEXT("copy ctor")
+ {
+ testCopyCtor();
+ }
+ reset();
+ BOOST_TEST_CONTEXT("copy op")
+ {
+ testCopyOp();
+ }
+ reset();
+ BOOST_TEST_CONTEXT("move ctor")
+ {
+ testMoveCtor();
+ }
+ reset();
+ BOOST_TEST_CONTEXT("move op")
+ {
+ testMoveOp();
+ }
+ }
+ }
+
+ virtual void reset()
+ {
+ }
+
+ virtual void testCopyCtor() = 0;
+ virtual void testCopyOp() = 0;
+ virtual void testMoveCtor() = 0;
+ virtual void testMoveOp() = 0;
+};
+
+
+
+template <class T>
+struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
+{
+ T in;
+
+ InstanceCopyMoveCtorsOpsTest() :
+ CopyMoveCtorsOpsTestBase(simox::meta::get_type_name<T>())
+ {
+ }
+ virtual ~InstanceCopyMoveCtorsOpsTest() override = default;
+
+ void reset() override
+ {
+ in = T {id};
+ BOOST_CHECK_EQUAL(in.id(), id);
+ }
+
+ void testCopyCtor() override
+ {
+ T out { in };
+
+ BOOST_CHECK_EQUAL(out.id(), id);
+
+ CustomChecks<T>::checkEqual(out, in);
+ }
+ void testCopyOp() override
+ {
+ T out;
+ out = in;
+
+ BOOST_CHECK_EQUAL(out.id(), id);
+
+ CustomChecks<T>::checkEqual(out, in);
+ }
+ void testMoveCtor() override
+ {
+ T out { std::move(in) };
+
+ BOOST_CHECK_EQUAL(in.id(), idMoved);
+ BOOST_CHECK_EQUAL(out.id(), id);
+
+ CustomChecks<T>::checkMoved(in);
+ }
+ void testMoveOp() override
+ {
+ T out;
+ out = std::move(in);
+
+ BOOST_CHECK_EQUAL(in.id(), idMoved);
+ BOOST_CHECK_EQUAL(out.id(), id);
+
+ CustomChecks<T>::checkMoved(in);
+ }
+};
+
+
+template <class T>
+struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
+{
+ T in;
+
+ CopyMoveCtorsOpsTest() : CopyMoveCtorsOpsTestBase(simox::meta::get_type_name<T>())
+ {
+ }
+ virtual ~CopyMoveCtorsOpsTest() override = default;
+
+ void reset() override
+ {
+ in = T {id};
+ add_element(in.container());
+
+ BOOST_CHECK_EQUAL(in.id(), id);
+ BOOST_CHECK_EQUAL(in.size(), 1);
+ }
+
+ void testCopyCtor() override
+ {
+ T out { in };
+ BOOST_CHECK_EQUAL(in.id(), id);
+ BOOST_CHECK_EQUAL(in.size(), 1);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 1);
+
+ CustomChecks<T>::checkEqual(out, in);
+ }
+ void testCopyOp() override
+ {
+ T out;
+ out = in;
+ BOOST_CHECK_EQUAL(in.id(), id);
+ BOOST_CHECK_EQUAL(in.size(), 1);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 1);
+
+ CustomChecks<T>::checkEqual(out, in);
+ }
+ void testMoveCtor() override
+ {
+ T out { std::move(in) };
+
+ BOOST_CHECK_EQUAL(in.id(), idMoved);
+ BOOST_CHECK_EQUAL(in.size(), 0);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 1);
+
+ CustomChecks<T>::checkMoved(in);
+ }
+ void testMoveOp() override
+ {
+ T out;
+ out = std::move(in);
+
+ BOOST_CHECK_EQUAL(in.id(), idMoved);
+ BOOST_CHECK_EQUAL(in.size(), 0);
+ BOOST_CHECK_EQUAL(out.id(), id);
+ BOOST_CHECK_EQUAL(out.size(), 1);
+
+ CustomChecks<T>::checkMoved(in);
+ }
+};
+
+
+
+BOOST_AUTO_TEST_CASE(test_copy_move_ctors_ops)
+{
+ {
+ InstanceCopyMoveCtorsOpsTest<armem::wm::EntityInstance>().test();
+ CopyMoveCtorsOpsTest<armem::wm::EntitySnapshot>().test();
+ CopyMoveCtorsOpsTest<armem::wm::Entity>().test();
+ CopyMoveCtorsOpsTest<armem::wm::ProviderSegment>().test();
+ CopyMoveCtorsOpsTest<armem::wm::CoreSegment>().test();
+ CopyMoveCtorsOpsTest<armem::wm::Memory>().test();
+ }
+ {
+ InstanceCopyMoveCtorsOpsTest<armem::ltm::EntityInstance>().test();
+ CopyMoveCtorsOpsTest<armem::ltm::EntitySnapshot>().test();
+ CopyMoveCtorsOpsTest<armem::ltm::Entity>().test();
+ CopyMoveCtorsOpsTest<armem::ltm::ProviderSegment>().test();
+ CopyMoveCtorsOpsTest<armem::ltm::CoreSegment>().test();
+ CopyMoveCtorsOpsTest<armem::ltm::Memory>().test();
+ }
+ {
+ InstanceCopyMoveCtorsOpsTest<armem::d_ltm::EntityInstance>().test();
+ CopyMoveCtorsOpsTest<armem::d_ltm::EntitySnapshot>().test();
+ CopyMoveCtorsOpsTest<armem::d_ltm::Entity>().test();
+ CopyMoveCtorsOpsTest<armem::d_ltm::ProviderSegment>().test();
+ CopyMoveCtorsOpsTest<armem::d_ltm::CoreSegment>().test();
+ CopyMoveCtorsOpsTest<armem::d_ltm::Memory>().test();
+ }
+}
+
+
+
BOOST_AUTO_TEST_CASE(test_segment_setup)
{
armem::EntityUpdate update;
@@ -249,7 +749,7 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
providerSegment.update(update);
// Check correctly inherited history size.
- BOOST_CHECK_EQUAL(providerSegment.getEntity("C").maxHistorySize, 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("C").getMaxHistorySize(), 2);
// Check actual history size.
BOOST_CHECK_EQUAL(providerSegment.getEntity("C").history().size(), 2);
@@ -265,3 +765,54 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
BOOST_CHECK_EQUAL(providerSegment.getEntity(name).history().size(), 3);
}
}
+
+
+
+template <class T>
+struct CopyTest
+{
+ CopyTest()
+ {
+ }
+
+ void test()
+ {
+ // At least check whether they can be called.
+ T t;
+ T t2 = t.copy();
+
+ if constexpr (!std::is_base_of_v<armem::base::EntityInstanceBase<T>, T>)
+ {
+ t2 = t.copyEmpty();
+ }
+ }
+};
+
+
+BOOST_AUTO_TEST_CASE(test_copy)
+{
+ {
+ CopyTest<armem::wm::EntityInstance>().test();
+ CopyTest<armem::wm::EntitySnapshot>().test();
+ CopyTest<armem::wm::Entity>().test();
+ CopyTest<armem::wm::ProviderSegment>().test();
+ CopyTest<armem::wm::CoreSegment>().test();
+ CopyTest<armem::wm::Memory>().test();
+ }
+ {
+ CopyTest<armem::ltm::EntityInstance>().test();
+ CopyTest<armem::ltm::EntitySnapshot>().test();
+ CopyTest<armem::ltm::Entity>().test();
+ CopyTest<armem::ltm::ProviderSegment>().test();
+ CopyTest<armem::ltm::CoreSegment>().test();
+ CopyTest<armem::ltm::Memory>().test();
+ }
+ {
+ CopyTest<armem::d_ltm::EntityInstance>().test();
+ CopyTest<armem::d_ltm::EntitySnapshot>().test();
+ CopyTest<armem::d_ltm::Entity>().test();
+ CopyTest<armem::d_ltm::ProviderSegment>().test();
+ CopyTest<armem::d_ltm::CoreSegment>().test();
+ CopyTest<armem::d_ltm::Memory>().test();
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index c0a27825ee0ee7aae1770e3a426054194bdfd8ef..d90a90aa04a3fcc2c3d891bfab8727a991522f44 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -10,7 +10,7 @@ set(LIBRARIES
# ArmarXGui
SimpleConfigDialog
# RobotAPI
- armem
+ aroneigenconverter armem
)
set(SOURCES
@@ -24,9 +24,19 @@ set(SOURCES
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
- instance/TreeDataVisitorBase.cpp
- instance/TreeDataVisitor.cpp
- instance/TreeTypedDataVisitor.cpp
+ instance/sanitize_typename.cpp
+ instance/serialize_path.cpp
+
+ instance/display_visitors/DataDisplayVisitor.cpp
+ instance/display_visitors/TypedDataDisplayVisitor.cpp
+
+ instance/tree_builders/DataTreeBuilder.cpp
+ instance/tree_builders/DataTreeBuilderBase.cpp
+ instance/tree_builders/TypedDataTreeBuilder.cpp
+
+ instance/tree_visitors/TreeDataVisitorBase.cpp
+ instance/tree_visitors/TreeDataVisitor.cpp
+ instance/tree_visitors/TreeTypedDataVisitor.cpp
memory/GroupBox.cpp
memory/TreeWidget.cpp
@@ -47,9 +57,19 @@ set(HEADERS
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
- instance/TreeDataVisitorBase.h
- instance/TreeDataVisitor.h
- instance/TreeTypedDataVisitor.h
+ instance/sanitize_typename.h
+ instance/serialize_path.h
+
+ instance/display_visitors/DataDisplayVisitor.h
+ instance/display_visitors/TypedDataDisplayVisitor.h
+
+ instance/tree_builders/DataTreeBuilder.h
+ instance/tree_builders/DataTreeBuilderBase.h
+ instance/tree_builders/TypedDataTreeBuilder.h
+
+ instance/tree_visitors/TreeDataVisitorBase.h
+ instance/tree_visitors/TreeDataVisitor.h
+ instance/tree_visitors/TreeTypedDataVisitor.h
memory/GroupBox.h
memory/TreeWidget.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 694e98f08d3c9262670683392cca3981248ba314..447b35332a60a3442b63e07a2c5e129b5b7b83d1 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -27,36 +27,43 @@
namespace armarx::armem::gui
{
MemoryViewer::MemoryViewer(
- QBoxLayout* ltmControlWidgetLayout,
QBoxLayout* updateWidgetLayout,
- QGroupBox* _memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* _instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
- QLabel* statusLabel)
+ QBoxLayout* ltmControlWidgetLayout,
+ QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
+ QLabel* statusLabel
+ )
{
Logging::setTag("MemoryViewer");
this->statusLabel = statusLabel;
this->statusLabel->clear();
- // LTM Control
- this->ltmControlWidgetLayout = ltmControlWidgetLayout;
- ltmControlWidget = new armem::gui::LTMControlWidget();
- ltmControlWidgetLayout->insertWidget(0, ltmControlWidget);
-
// Update timer
this->updateWidgetLayout = updateWidgetLayout;
updateWidget = new armem::gui::PeriodicUpdateWidget(2.0, 60);
updateWidgetLayout->insertWidget(0, updateWidget);
+ // LTM Control
+ if (ltmControlWidgetLayout)
+ {
+ this->ltmControlWidgetLayout = ltmControlWidgetLayout;
+ ltmControlWidget = new armem::gui::LTMControlWidget();
+ ltmControlWidgetLayout->addWidget(ltmControlWidget);
+ }
+
+ // Memory View
memoryGroup = new armem::gui::MemoryGroupBox();
- armarx::gui::replaceWidget(_memoryGroupBox, memoryGroup, memoryGroupBoxParentLayout);
- ARMARX_CHECK_NULL(_memoryGroupBox);
+ armarx::gui::replaceWidget(memoryGroupBox, memoryGroup, memoryGroupBoxParentLayout);
+ ARMARX_CHECK_NULL(memoryGroupBox);
+ // Instance View
this->instanceGroup = new armem::gui::InstanceGroupBox();
- armarx::gui::replaceWidget(_instanceGroupBox, instanceGroup, instanceGroupBoxParentLayout);
+ armarx::gui::replaceWidget(instanceGroupBox, instanceGroup, instanceGroupBoxParentLayout);
this->instanceGroup->setStatusLabel(statusLabel);
- ARMARX_CHECK_NULL(_instanceGroupBox);
+ ARMARX_CHECK_NULL(instanceGroupBox);
+ // Connections
//connect(this, &This::connected, this, &This::updateMemory);
connect(ltmControlWidget, &armem::gui::LTMControlWidget::store, this, &This::store);
@@ -313,7 +320,7 @@ namespace armarx::armem::gui
void MemoryViewer::loadSettings(QSettings* settings)
{
- mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "ArMemMemoryNameSystem").toString().toStdString();
+ mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem").toString().toStdString();
debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver").toString().toStdString();
}
void MemoryViewer::saveSettings(QSettings* settings)
@@ -324,13 +331,17 @@ namespace armarx::armem::gui
void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "ArMemMemoryNameSystem", "*"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "*"});
dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
+ if (mnsName.empty())
+ {
+ mnsName = "MemoryNameSystem";
+ }
debugObserverName = dialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index c2c2e6f4c7d251f23f19cb2f7275878940a603d5..19ab5000d9d662080b80e621144650fd887a0bcd 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -44,10 +44,10 @@ namespace armarx::armem::gui
public:
MemoryViewer(
- QBoxLayout* ltmControlWidgetLayout,
QBoxLayout* updateWidgetLayout,
- QGroupBox* _memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* _instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* ltmControlWidgetLayout,
+ QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
QLabel* statusLabel
);
@@ -106,15 +106,15 @@ namespace armarx::armem::gui
std::map<std::string, armem::client::Reader> memoryReaders;
std::map<std::string, std::optional<armem::wm::Memory>> memoryData;
- QLayout* ltmControlWidgetLayout;
- armem::gui::LTMControlWidget* ltmControlWidget;
+ QLayout* updateWidgetLayout = nullptr;
+ armem::gui::PeriodicUpdateWidget* updateWidget = nullptr;
- QLayout* updateWidgetLayout;
- armem::gui::PeriodicUpdateWidget* updateWidget;
+ QLayout* ltmControlWidgetLayout = nullptr;
+ armem::gui::LTMControlWidget* ltmControlWidget = nullptr;
- armem::gui::MemoryGroupBox* memoryGroup;
+ armem::gui::MemoryGroupBox* memoryGroup = nullptr;
- armem::gui::InstanceGroupBox* instanceGroup;
+ armem::gui::InstanceGroupBox* instanceGroup = nullptr;
QLabel* statusLabel = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 8092012654c0bb9b9980279dce41467ba390d928..429d2f3603967f64728e59113841b85c862b8d46 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -9,6 +9,7 @@
#include <QLayout>
#include <QMenu>
#include <QSplitter>
+#include <QTreeWidget>
#include <QVBoxLayout>
#include <SimoxUtility/algorithm/string.h>
@@ -19,9 +20,10 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/ImageView.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
namespace armarx::armem::gui::instance
@@ -98,7 +100,7 @@ namespace armarx::armem::gui::instance
instance = &memory.getEntityInstance(id);
if (useTypeInfo)
{
- aronType = memory.getCoreSegment(id.coreSegmentName).getProviderSegment(id.providerSegmentName).aronType;
+ aronType = memory.getCoreSegment(id.coreSegmentName).getProviderSegment(id.providerSegmentName).aronType();
}
}
catch (const armem::error::ArMemError& e)
@@ -124,7 +126,7 @@ namespace armarx::armem::gui::instance
{
if (currentInstance)
{
- updateInstanceID(currentInstance->id);
+ updateInstanceID(currentInstance->id());
updateMetaData(currentInstance->metadata());
updateData(currentInstance->data(), currentAronType);
updateImageView(currentInstance->data());
@@ -147,23 +149,23 @@ namespace armarx::armem::gui::instance
void InstanceView::updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
{
- armarx::gui::clearItem(treeItemData);
if (!data)
{
+ armarx::gui::clearItem(treeItemData);
QTreeWidgetItem* item = new QTreeWidgetItem({"(No data.)"});
treeItemData->addChild(item);
}
else if (useTypeInfo && aronType)
{
- TreeTypedDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*aronType, *data);
+ TypedDataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *aronType, *data);
}
else
{
- TreeDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*data);
+ DataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *data);
}
treeItemData->setExpanded(true);
}
@@ -211,7 +213,7 @@ namespace armarx::armem::gui::instance
case aron::type::Descriptor::eIVTCByteImage:
{
QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = TreeTypedDataVisitor::deserializePath(qpath);
+ aron::Path path = deserializePath(qpath);
QAction* viewAction = new QAction("Show image");
menu.addAction(viewAction);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
index 640712dffb91bbd019bc8cdb58469b5309487575..5f937f419cd3111daf1146a6640e0ea9348f6d42 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
@@ -6,7 +6,7 @@
#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
class QGroupBox;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
deleted file mode 100644
index fa51ab6fe0018927ebb03da63828a7aea598035a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "TreeTypedDataVisitor.h"
-
-#include <SimoxUtility/algorithm/string.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-
-namespace armarx::armem::gui
-{
-
- std::string TreeTypedDataVisitor::sanitizeTypeName(const std::string& typeName) const
- {
- std::string n = typeName;
- n = simox::alg::replace_all(n, "Aron", "");
- n = simox::alg::replace_all(n, "Type", "");
- return n;
- }
-
- QStringList TreeTypedDataVisitor::serializePath(const aron::Path& path)
- {
- QStringList qpath;
- qpath.append(QString::fromStdString(path.getRootIdentifier()));
- qpath.append(QString::fromStdString(path.getDelimeter()));
- for (const std::string& item : path.getPath())
- {
- qpath.append(QString::fromStdString(item));
- }
- return qpath;
- }
-
- aron::Path TreeTypedDataVisitor::deserializePath(const QStringList& qpath)
- {
- ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
- std::vector<std::string> pathItems;
- for (int i = 2; i < qpath.size(); ++i)
- {
- pathItems.push_back(qpath.at(i).toStdString());
- }
- aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
- return path;
- }
-
-
-
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f41f5d4cb447e9571d5145f26af5cbff6c9db2cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -0,0 +1,76 @@
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string DataDisplayVisitor::getValue(DataNavigator& n)
+ {
+ DataDisplayVisitor v;
+ v.applyTo(n);
+ return v.value.str();
+ }
+
+ bool DataDisplayVisitor::visitEnter(DictDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visitEnter(ListDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(BoolDataNavigator& b)
+ {
+ if (b.getValue())
+ {
+ value << "true";
+ }
+ else
+ {
+ value << "false";
+ }
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(DoubleDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(FloatDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(IntDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(LongDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(StringDataNavigator& n)
+ {
+ value << "'" << n.getValue() << "'";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(NDArrayDataNavigator& n)
+ {
+ value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
+ return false;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f48a57f0cd42a7e69dec44dd37ec489b50b440d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <sstream>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class DataDisplayVisitor : public aron::visitor::DataVisitor
+ {
+ public:
+
+ static std::string getValue(DataNavigator& n);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictDataNavigator& n) override;
+ bool visitEnter(ListDataNavigator& n) override;
+
+ bool visit(BoolDataNavigator& b) override;
+ bool visit(DoubleDataNavigator& n) override;
+ bool visit(FloatDataNavigator& n) override;
+ bool visit(IntDataNavigator& n) override;
+ bool visit(LongDataNavigator& n) override;
+ bool visit(StringDataNavigator& n) override;
+
+ bool visit(NDArrayDataNavigator& n) override;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..827d14a599af43e6067fd1b3fa18bb136dfcbed8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -0,0 +1,147 @@
+#include "TypedDataDisplayVisitor.h"
+
+#include <iomanip> // std::setprecision
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string TypedDataDisplayVisitor::getValue(TypeNavigator& type, DataNavigator& data)
+ {
+ TypedDataDisplayVisitor v;
+ bool r = v.applyTo(type, data);
+ ARMARX_CHECK(!r);
+ return v.value.str();
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ObjectTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ListTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(TupleTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(BoolTypeNavigator&, BoolDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IntTypeNavigator&, IntDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(LongTypeNavigator&, LongDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(StringTypeNavigator&, StringDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
+ {
+ armem::Time time = armem::Time::microSeconds(data.getValue());
+ armem::toDateTimeMilliSeconds(time);
+ value << armem::toDateTimeMilliSeconds(time);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ value << std::setprecision(2) << std::fixed;
+ value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ return false;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3923b7d0d74a1ee9c4b1ea7a5664ad3f83396051
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sstream>
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class TypedDataDisplayVisitor : public aron::visitor::TypedDataVisitor
+ {
+ public:
+
+ static std::string getValue(TypeNavigator& type, DataNavigator& data);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictTypeNavigator&, DictDataNavigator& data) override;
+ bool visitEnter(ObjectTypeNavigator&, DictDataNavigator& data) override;
+
+ bool visitEnter(ListTypeNavigator&, ListDataNavigator& data) override;
+ bool visitEnter(TupleTypeNavigator&, ListDataNavigator& data) override;
+
+
+ bool visit(BoolTypeNavigator&, BoolDataNavigator& data) override;
+ bool visit(DoubleTypeNavigator&, DoubleDataNavigator& data) override;
+ bool visit(FloatTypeNavigator&, FloatDataNavigator& data) override;
+ bool visit(IntTypeNavigator&, IntDataNavigator& data) override;
+ bool visit(LongTypeNavigator&, LongDataNavigator& data) override;
+ bool visit(StringTypeNavigator&, StringDataNavigator& data) override;
+ bool visit(TimeTypeNavigator&, LongDataNavigator& data) override;
+
+
+ bool visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PoseTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PositionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OrientationTypeNavigator&, NDArrayDataNavigator& data) override;
+
+
+ protected:
+
+ std::string coeffSep = " ";
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9db04814561de68c4e7390093a7a7b3659f0219c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -0,0 +1,46 @@
+#include "sanitize_typename.h"
+
+#include <sstream>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
+
+static const std::string MemoryIDTypeName = armarx::armem::arondto::MemoryID::toInitialAronType()->getName();
+
+std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
+{
+ if (typeName == MemoryIDTypeName)
+ {
+ return "MemoryID";
+ }
+
+ namespace s = simox::alg;
+ std::string n = typeName;
+ n = s::replace_all(n, "Aron", "");
+ n = s::replace_all(n, "Type", "");
+ n = s::replace_all(n, "type::", "");
+ if (s::starts_with(n, "Object<") && s::ends_with(n, ">"))
+ {
+ std::string begin = "Object<";
+ std::string end = ">";
+ n = n.substr(begin.size(), n.size() - begin.size() - end.size());
+ }
+
+ if (true)
+ {
+ const std::string del = "::";
+ size_t find = n.rfind(del);
+ if (find != n.npos)
+ {
+ find += del.size(); // include del
+ std::stringstream ss;
+ ss << n.substr(find) << " (" << n.substr(0, find - del.size()) << ")";
+ n = ss.str();
+ }
+ }
+
+ return n;
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
new file mode 100644
index 0000000000000000000000000000000000000000..80c77acf229ecb6d70b55b6ac1c5b951a0cb89bd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <string>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ std::string sanitizeTypeName(const std::string& typeName);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a01b0b4620748b581db928c4e7ddcbd7af1a3f46
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
@@ -0,0 +1,35 @@
+#include "serialize_path.h"
+
+#include <RobotAPI/libraries/aron/core/Path.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <QString>
+#include <QStringList>
+
+
+QStringList armarx::armem::gui::instance::serializePath(const aron::Path& path)
+{
+ QStringList qpath;
+ qpath.append(QString::fromStdString(path.getRootIdentifier()));
+ qpath.append(QString::fromStdString(path.getDelimeter()));
+ for (const std::string& item : path.getPath())
+ {
+ qpath.append(QString::fromStdString(item));
+ }
+ return qpath;
+}
+
+armarx::aron::Path armarx::armem::gui::instance::deserializePath(const QStringList& qpath)
+{
+ ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
+ std::vector<std::string> pathItems;
+ for (int i = 2; i < qpath.size(); ++i)
+ {
+ pathItems.push_back(qpath.at(i).toStdString());
+ }
+ aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
+ return path;
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
new file mode 100644
index 0000000000000000000000000000000000000000..26517c6d11e3ee25071bb6f5e01da3996c6553cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
@@ -0,0 +1,18 @@
+#pragma once
+
+
+namespace armarx::aron
+{
+ class Path;
+}
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ QStringList serializePath(const aron::Path& path);
+ aron::Path deserializePath(const QStringList& qpath);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07ccac8866d870b153fba28915006d3dfd46cdae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -0,0 +1,58 @@
+#include "DataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilder::DataTreeBuilder()
+ {
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto child = data.getElement(key);
+ this->update(item, key, *child);
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item)
+ {
+ this->update(item, std::to_string(key), *children.at(key));
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+
+
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ this->setRowTexts(item, key, data);
+
+ if (auto cast = dynamic_cast<aron::datanavigator::DictNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ else if (auto cast = dynamic_cast<aron::datanavigator::ListNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9aa8471e5d4720c8e747844deb45459d68b1b90
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilder();
+
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f837fe2fa6451cf8edd82793f2c6ec773560ce2c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -0,0 +1,97 @@
+#include "DataTreeBuilderBase.h"
+
+#include <QTreeWidgetItem>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilderBase::DataTreeBuilderBase()
+ {
+ }
+
+ DataTreeBuilderBase::~DataTreeBuilderBase()
+ {}
+
+ void DataTreeBuilderBase::setColumns(int key, int value, int type)
+ {
+ this->columnKey = key;
+ this->columnType = type;
+ this->columnValue = value;
+ }
+
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(const std::string& key) const
+ {
+ return new QTreeWidgetItem(QStringList{QString::fromStdString(key)});
+ }
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(size_t key) const
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem();
+ item->setData(0, Qt::UserRole, static_cast<int>(key));
+ return item;
+ }
+
+ void DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName) const
+ {
+ item->setText(columnKey, QString::fromStdString(key));
+ item->setText(columnValue, QString::fromStdString(value));
+ item->setText(columnType, QString::fromStdString(typeName));
+ }
+
+
+ void DataTreeBuilderBase::setRowTexts(
+ QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ const std::string value = armarx::aron::DataDisplayVisitor::getValue(data);
+ setRowTexts(item, key, value, data.getName());
+ }
+
+ DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
+ {
+ DictBuilder builder;
+ builder.setCompareFn([](const std::string & key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(key, item->text(0).toStdString());
+ });
+ builder.setMakeItemFn([this](const std::string & key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ DataTreeBuilderBase::ListBuilder DataTreeBuilderBase::getListBuilder() const
+ {
+ ListBuilder builder;
+ builder.setCompareFn([](size_t key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(static_cast<int>(key), item->data(0, Qt::UserRole).toInt());
+ });
+ builder.setMakeItemFn([this](size_t key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ std::vector<size_t> DataTreeBuilderBase::getIndex(size_t size) const
+ {
+ std::vector<size_t> index;
+ index.reserve(size);
+ for (size_t i = 0; i < size; ++i)
+ {
+ index.push_back(i);
+ }
+ return index;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26a422ea71c90c2b769217a274eac7887eb8f9d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+
+
+namespace armarx
+{
+ template <class ContainerT> struct TreeWidgetBuilder;
+}
+
+class QTreeWidgetItem;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilderBase();
+ virtual ~DataTreeBuilderBase();
+
+ void setColumns(int key, int value, int type);
+
+
+ protected:
+
+ using DictBuilder = armarx::TreeWidgetBuilder<std::vector<std::string>>;
+ using ListBuilder = armarx::TreeWidgetBuilder<std::vector<size_t>>;
+
+ DictBuilder getDictBuilder() const;
+ ListBuilder getListBuilder() const;
+ std::vector<size_t> getIndex(size_t size) const;
+
+
+ QTreeWidgetItem* makeItem(const std::string& key) const;
+ QTreeWidgetItem* makeItem(size_t key) const;
+
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ public:
+
+ int columnKey = 0;
+ int columnValue = 1;
+ int columnType = 2;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5be64a3f5657893e2c6f36e4695eff4a08c9dc9e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -0,0 +1,180 @@
+#include "TypedDataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ TypedDataTreeBuilder::TypedDataTreeBuilder()
+ {
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childType = type.getMemberType(key);
+ auto childData = data.getElement(key);
+
+ if (childType && childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, type.getAllKeys());
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
+ {
+ auto childData = children.at(key);
+ if (childData)
+ {
+ this->update(item, std::to_string(key), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ ARMARX_CHECK_EQUAL(data.childrenSize(), 2);
+ auto childTypes = type.getAcceptedTypes();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
+ {
+ auto childType = i == 0 ? childTypes.first : childTypes.second;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(data.childrenSize()));
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childTypes = type.getAcceptedTypes();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
+ {
+ auto childType = childTypes.at(i);
+ auto childData = data.getElement(static_cast<unsigned int>(i));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(type.getAcceptedTypes().size()));
+ }
+
+
+ void TypedDataTreeBuilder::update(
+ QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data)
+ {
+ using namespace aron::typenavigator;
+
+ const std::string value = armarx::aron::TypedDataDisplayVisitor::getValue(type, data);
+ const std::string typeName = sanitizeTypeName(type.getName());
+ setRowTexts(item, key, value, typeName);
+
+ item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
+ item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
+
+ if (auto t = dynamic_cast<ObjectNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<DictNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<ListNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<PairNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<TupleNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ }
+
+ template <class DataT, class TypeT>
+ void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
+ {
+ DataT& dataCast = dynamic_cast<DataT&>(data);
+ updateTree(item, type, dataCast);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..57580a1f6d3a84ca639131e0da375a6129650e0a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class TypedDataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ TypedDataTreeBuilder();
+
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data);
+
+ template <class DataT, class TypeT>
+ void _updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data);
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
similarity index 72%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
index 0d1721ef244c8e4a44eba5003a19302c190518d7..8bc5889d29298e41dbb27cfa6114f9841c7d3f98 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
similarity index 94%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
index b751084bf8af57f2d7f6598e2ae2e835c0948b6c..61d76bcfc58a81d138a442d07e1f64bc0eb7b922 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
@@ -8,10 +8,10 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitor :
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
similarity index 90%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
index 39ba3d5e35de1247790dfb7c0e8837096b3b1b77..87cd2ffb9bdee5de102b3b026fb508784f0ff0c0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
TreeDataVisitorBase::TreeDataVisitorBase()
@@ -62,7 +62,7 @@ namespace armarx::armem::gui
return true;
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const
{
if (n.getValue())
{
@@ -74,12 +74,12 @@ namespace armarx::armem::gui
}
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const
{
ss << "'" << n.getValue() << "'";
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const
{
ss << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
similarity index 80%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
index 35dcca57a868e050c561045af0a948276f43af60..c9d4ab0cdaf462761b10e422ef41f415ec54d7a1 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
@@ -9,7 +9,7 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitorBase
@@ -40,26 +40,31 @@ namespace armarx::armem::gui
return true;
}
- template <class Navigator>
- QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName)
+ QStringList makeValueRowStrings(const std::string& key, const std::string& value, const std::string& typeName) const
{
- std::stringstream value;
- streamValueText(n, value);
QStringList cols;
cols.insert(columnKey, QString::fromStdString(key));
- cols.insert(columnValue, QString::fromStdString(value.str()));
+ cols.insert(columnValue, QString::fromStdString(value));
cols.insert(columnType, QString::fromStdString(typeName));
return cols;
}
template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss)
+ QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName) const
+ {
+ std::stringstream value;
+ streamValueText(n, value);
+ return makeValueRowStrings(key, value.str(), typeName);
+ }
+
+ template <class Navigator>
+ void streamValueText(Navigator& n, std::stringstream& ss) const
{
ss << n.getValue();
}
- void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss);
+ void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const;
public:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4ed919f9b46be2c62966fccbd89fbefa0370449
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
@@ -0,0 +1,56 @@
+#include "TreeTypedDataVisitor.h"
+
+#include <iomanip> // std::setprecision
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/armem/core.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const
+ {
+ QTreeWidgetItem* item = makeItem<NDArrayDataNavigator, PoseTypeNavigator>(key, data, type);
+ // ToDo: Do something special.
+ return item;
+ }
+
+ void TreeTypedDataVisitor::streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ armem::Time time = armem::Time::microSeconds(data.getValue());
+ armem::toDateTimeMilliSeconds(time);
+ ss << armem::toDateTimeMilliSeconds(time);
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
similarity index 53%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index b5493584b2c91eeed01fb6f9758d23a956b5225c..fb1c1baa557167432ffaa19f46e71d223a5c080d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -7,10 +7,14 @@
#include <QLabel>
#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
-namespace armarx::armem::gui
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
+
+
+namespace armarx::armem::gui::instance
{
class TreeTypedDataVisitor :
@@ -83,12 +87,20 @@ namespace armarx::armem::gui
{
return this->addValueRow(key, data, type);
}
+ bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
{
return this->addValueRow(key, data, type);
}
+ bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
{
return this->addValueRow(key, data, type);
@@ -101,33 +113,84 @@ namespace armarx::armem::gui
{
return this->addValueRow(key, data, type);
}
-
-
- std::string sanitizeTypeName(const std::string& typeName) const;
+ bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
+ bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
+ bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
protected:
- template <class DataNavigatorT>
- bool addValueRow(const std::string& key, DataNavigatorT& dataNavigator, const TypeNavigator& type)
+ template <class DataNavigatorT, class TypeNavigatorT>
+ bool addValueRow(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type)
{
if (items.size() > 0)
{
- QTreeWidgetItem* item = new QTreeWidgetItem(this->makeValueRowStrings(key, dataNavigator, sanitizeTypeName(type.getName())));
+ QTreeWidgetItem* item = makeItem(key, data, type);
items.top()->addChild(item);
- item->setData(columnKey, Qt::UserRole, serializePath(dataNavigator.getPath()));
+ item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, int(type.getDescriptor()));
+
+ if (false)
+ {
+ QFont font;
+ font.setFamily("Consolas");
+ font.setStyleHint(QFont::Monospace);
+ font.setFixedPitch(true);
+ font.setPointSize(10);
+ item->setFont(columnValue, font);
+ }
}
return true;
}
- public:
+ template <class DataNavigatorT, class TypeNavigatorT>
+ QTreeWidgetItem* makeItem(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type) const
+ {
+ std::stringstream ss;
+ try
+ {
+ this->streamValueText(data, type, ss);
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ss << "x ";
+ TreeDataVisitorBase::streamValueText(data, ss);
+ std::stringstream es;
+ es << e.what();
+ ss << simox::alg::replace_all(es.str(), "\n", " | ");
+ }
+ return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getName())));
+ }
+
+ QTreeWidgetItem* makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const;
+
+
+ template <class DataNavigatorT, class TypeNavigatorT>
+ void streamValueText(DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
+ {
+ // Fallback to type-agnostic (but data-aware).
+ (void) type;
+ TreeDataVisitorBase::streamValueText(data, ss);
+ }
- static QStringList serializePath(const aron::Path& path);
- static aron::Path deserializePath(const QStringList& qpath);
+ using TreeDataVisitorBase::streamValueText;
+ void streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const;
+ std::string coeffSep = " ";
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 20f0a2f1a4f17cac00d91dd5705b6e8c37d95304..a410033a4dcdda6b0e17f0de7ab35e4f9a044f12 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -1,5 +1,7 @@
#include "TreeWidget.h"
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -72,7 +74,7 @@ namespace armarx::armem::gui::memory
});
workingmemoryCoreSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
{
- updateTypedContainerItem(coreSeg, coreSegItem);
+ updateContainerItem(coreSeg, coreSegItem);
updateChildren(coreSeg, coreSegItem);
return true;
});
@@ -84,7 +86,7 @@ namespace armarx::armem::gui::memory
});
workingmemoryProvSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
{
- updateTypedContainerItem(provSeg, provSegItem);
+ updateContainerItem(provSeg, provSegItem);
updateChildren(provSeg, provSegItem);
return true;
});
@@ -261,11 +263,11 @@ namespace armarx::armem::gui::memory
columns.insert(int(Columns::SIZE), "");
columns.insert(int(Columns::TYPE), "");
columns.insert(int(Columns::LEVEL), QString::fromStdString(simox::alg::capitalize_words(memoryItem.getLevelName())));
- columns.insert(int(Columns::ID), QString::fromStdString(memoryItem.id.str()));
+ columns.insert(int(Columns::ID), QString::fromStdString(memoryItem.id().str()));
QTreeWidgetItem* item = new QTreeWidgetItem(columns);
item->setData(int(Columns::LEVEL), Qt::UserRole, QString::fromStdString(memoryItem.getLevelName()));
- item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(memoryItem.id.str()));
+ item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(memoryItem.id().str()));
item->setTextAlignment(int(Columns::SIZE), Qt::AlignRight);
return item;
}
@@ -277,37 +279,26 @@ namespace armarx::armem::gui::memory
template <class... T>
void TreeWidget::updateContainerItem(
- const armem::wm::detail::MemoryContainer<T...>& container, QTreeWidgetItem* item)
+ const base::detail::MemoryContainerBase<T...>& container, QTreeWidgetItem* item)
{
updateItemItem(container, item);
item->setText(int(Columns::SIZE), QString::number(container.size()));
- }
- template <class... T>
- void TreeWidget::updateTypedContainerItem(
- const armem::wm::detail::TypedEntityContainer<T...>& container, QTreeWidgetItem* item)
- {
- updateContainerItem(container, item);
- std::string typeName;
- if (container.aronType)
+ if constexpr(std::is_base_of_v<base::detail::AronTyped, base::detail::MemoryContainerBase<T...>>)
{
- typeName = container.aronType->getName();
-
- std::string del = "::";
- size_t find = typeName.rfind(del);
- if (find != typeName.npos)
+ const base::detail::AronTyped& cast = dynamic_cast<const base::detail::AronTyped&>(container);
+ std::string typeName;
+ if (cast.aronType())
{
- find += del.size(); // include del
- std::stringstream ss;
- ss << typeName.substr(find) << " (" << typeName.substr(0, find) << ")";
- typeName = ss.str();
+ typeName = cast.aronType()->getName();
+ typeName = instance::sanitizeTypeName(typeName);
}
+ else
+ {
+ typeName = "(no Aron type)";
+ }
+ item->setText(int(Columns::TYPE), QString::fromStdString(typeName));
}
- else
- {
- typeName = "(no Aron type)";
- }
- item->setText(int(Columns::TYPE), QString::fromStdString(typeName));
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 1ce80192013f4d045ff84b4f461589bbd4a43297..2bcf0fa1b6135534407044a3ce341b86afeab442 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -71,9 +71,7 @@ namespace armarx::armem::gui::memory
void updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item);
template <class... T>
- void updateContainerItem(const armem::wm::detail::MemoryContainer<T...>& container, QTreeWidgetItem* item);
- template <class... T>
- void updateTypedContainerItem(const armem::wm::detail::TypedEntityContainer<T...>& container, QTreeWidgetItem* item);
+ void updateContainerItem(const armem::base::detail::MemoryContainerBase<T...>& container, QTreeWidgetItem* item);
private:
diff --git a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index 5ecf962dbef91f033435c532149c49285c408bc2..4f259c7af5a50ff198b139084eb604bfd06fead5 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -29,13 +29,28 @@
#include <iostream>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+
+using namespace armarx::armem::gui::instance;
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName)
{
- armarx::armem::gui::TreeTypedDataVisitor v;
-
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronDictType<AronFloatType>"), "Dict<Float>");
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronListType<AronFloatType>"), "List<Float>");
+ using namespace armarx::aron::typenavigator;
+ {
+ DictNavigator dict;
+ dict.setAcceptedType(std::make_shared<FloatNavigator>());
+ BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Float>");
+ }
+ {
+ ListNavigator dict;
+ dict.setAcceptedType(std::make_shared<LongNavigator>());
+ BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "Dict<Long>");
+ }
+ {
+ ObjectNavigator dict;
+ dict.setObjectName("namespace::MyObjectName");
+ BOOST_CHECK_EQUAL(sanitizeTypeName(dict.getName()), "namespace::MyObjectName");
+ }
}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a26840ac78305a9afcf6ab55498d5bdf45d9c017
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -0,0 +1,54 @@
+set(LIB_NAME armem_objects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::Core
+ RobotAPI::libraries::armem
+ HEADERS
+ aron_conversions.h
+ aron_forward_declarations.h
+
+ server/class/FloorVis.h
+ server/class/Segment.h
+
+ server/instance/Segment.h
+ server/instance/SegmentAdapter.h
+ server/instance/Decay.h
+ server/instance/RobotHeadMovement.h
+ server/instance/Visu.h
+ SOURCES
+ aron_conversions.cpp
+
+ server/class/FloorVis.cpp
+ server/class/Segment.cpp
+
+ server/instance/Segment.cpp
+ server/instance/SegmentAdapter.cpp
+ server/instance/Decay.cpp
+ server/instance/RobotHeadMovement.cpp
+ server/instance/Visu.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/ObjectClass.xml
+ aron/ObjectInstance.xml
+)
+
+add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
new file mode 100644
index 0000000000000000000000000000000000000000..650082eb4ef8b924c8368fdd6cdf8da4578e105a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -0,0 +1,58 @@
+<!--
+Core segment type of Object/Class.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectClass">
+
+ <ObjectChild key="id">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+
+ <ObjectChild key="simoxXmlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshWrlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshObjPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="aabb">
+ <simox::arondto::AxisAlignedBoundingBox />
+ </ObjectChild>
+ <ObjectChild key="oobb">
+ <simox::arondto::OrientedBox />
+ </ObjectChild>
+
+ <ObjectChild key="names">
+ <armarx::arondto::ObjectNames />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
new file mode 100644
index 0000000000000000000000000000000000000000..774c6e0d5d32734a16d7fdc918f491d19cfbfa39
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
@@ -0,0 +1,33 @@
+<!--
+Core segment type of Object/Instance.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectInstance">
+
+ <ObjectChild key="pose">
+ <armarx::objpose::arondto::ObjectPose />
+ </ObjectChild>
+
+ <ObjectChild key="classID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+ <ObjectChild key="sourceID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..859f3d601024ba97dd0e59e938afaba2718a95a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -0,0 +1,27 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
+{
+ bo = dto.pose;
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+{
+ dto.pose = bo;
+}
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+{
+ objpose::fromAron(dto.pose, bo);
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+{
+ objpose::toAron(dto.pose, bo);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ea733e99c75895ff7aba463f36a0f53d0c4bd38
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+namespace armarx::armem
+{
+ void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo);
+
+ void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..9416a033d713f5c8dab28506538c1a56534dbb53
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
@@ -0,0 +1,8 @@
+#pragma once
+
+
+namespace armarx::armem::arondto
+{
+ class ObjectClass;
+ class ObjectInstance;
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08927d1e77d1963234f42ea9ab861ec9a33f86c4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
@@ -0,0 +1,77 @@
+#include "FloorVis.h"
+
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ FloorVis::FloorVis()
+ {
+ }
+
+ void FloorVis::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ properties.define(defs, prefix);
+ }
+
+ void FloorVis::setArViz(armarx::viz::Client arviz)
+ {
+ this->arviz = arviz;
+ }
+
+ void FloorVis::updateFloorObject(const wm::CoreSegment& classCoreSegment)
+ {
+ viz::Layer layer = arviz.layer(properties.layerName);
+ if (properties.show)
+ {
+ const wm::Entity* entity = classCoreSegment.findEntity(MemoryID().withEntityName(properties.entityName));
+ if (entity)
+ {
+ ARMARX_INFO << "Drawing floor class '" << properties.entityName << "'.";
+ layer.add(makeFloorObject(*entity));
+ }
+ else
+ {
+ ARMARX_INFO << "Did not find floor class '" << properties.entityName << "'.";
+ }
+ }
+ arviz.commit(layer);
+ }
+
+ armarx::viz::Object FloorVis::makeFloorObject(const wm::Entity& classEntity)
+ {
+ const wm::EntityInstance& instance = classEntity.getLatestSnapshot().getInstance(0);
+ arondto::ObjectClass data;
+ data.fromAron(instance.data());
+ return makeFloorObject(classEntity.name(), data);
+ }
+
+ armarx::viz::Object FloorVis::makeFloorObject(
+ const std::string& name,
+ const arondto::ObjectClass& objectClass)
+ {
+ ARMARX_TRACE;
+ return armarx::viz::Object(name)
+ .file(objectClass.simoxXmlPath.package, objectClass.simoxXmlPath.path)
+ .position(Eigen::Vector3f(0, 0, properties.height));
+ }
+
+
+
+ void FloorVis::Properties::define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(show, prefix + "Show", "Whether to show the floor.");
+ defs->optional(entityName, prefix + "EntityName", "Object class entity of the floor.");
+ defs->optional(layerName, prefix + "LayerName", "Layer to draw the floor on.");
+ defs->optional(height, prefix + "Height",
+ "Height (z) of the floor plane. \n"
+ "Set slightly below 0 to avoid z-fighting when drawing planes on the ground.");
+ }
+
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c3be719fce4148970fa0ef16d8465d2df7a10d3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem_objects/aron_forward_declarations.h>
+
+
+namespace armarx
+{
+ using PropertyDefinitionsPtr = IceUtil::Handle<class PropertyDefinitionContainer>;
+}
+namespace armarx::armem::wm
+{
+ class CoreSegment;
+ class Entity;
+}
+
+namespace armarx::armem::server::obj::clazz
+{
+ class FloorVis
+ {
+ public:
+
+ FloorVis();
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void setArViz(armarx::viz::Client arviz);
+
+ /// Draw a the floor as a simox object.
+ /// @see `makeFloorObject()`
+ void updateFloorObject(const wm::CoreSegment& classCoreSegment);
+
+ armarx::viz::Object makeFloorObject(const wm::Entity& classEntity);
+ armarx::viz::Object makeFloorObject(const std::string& name, const arondto::ObjectClass& objectClass);
+
+
+ public:
+
+ struct Properties
+ {
+ bool show = true;
+
+ std::string entityName = "Environment/floor-20x20";
+ std::string layerName = "Floor";
+ bool height = 1;
+
+ void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ };
+
+ private:
+
+ Properties properties;
+
+ armarx::viz::Client arviz;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..38b143f2baded43c3a4f028174380abaef288dcf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -0,0 +1,306 @@
+#include "Segment.h"
+
+#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_objects/aron_conversions.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <filesystem>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ClassSegment");
+ }
+
+ 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).");
+
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackgage", "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.");
+
+ floorVis.defineProperties(defs, prefix + "Floor.");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ if (p.loadFromObjectsPackage)
+ {
+ loadByObjectFinder(p.objectsPackage);
+ }
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ this->arviz = arviz;
+
+ floorVis.setArViz(arviz);
+ floorVis.updateFloorObject(*coreSegment);
+ }
+
+
+ void Segment::loadByObjectFinder(const std::string& objectsPackage)
+ {
+ loadByObjectFinder(ObjectFinder(objectsPackage));
+ }
+
+ void Segment::loadByObjectFinder(const ObjectFinder& finder)
+ {
+ this->objectFinder = finder;
+ loadByObjectFinder();
+ }
+
+ void Segment::loadByObjectFinder()
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const bool checkPaths = false;
+ std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
+ << objectFinder.getPackageName() << "' ...";
+ Commit commit;
+ for (ObjectInfo& info : infos)
+ {
+ info.setLogError(false);
+
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(info.id().str());
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::ObjectClass objectClass = objectClassFromInfo(info);
+ update.instancesData =
+ {
+ objectClass.toAron()
+ };
+ }
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
+ << objectFinder.getPackageName() << "'.";
+ iceMemory.commit(commit);
+ }
+
+ void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
+ {
+ const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ viz::Layer layerOrigin = arviz.layer("Origin");
+ layerOrigin.add(viz::Pose("Origin"));
+
+ viz::Layer layerObject = arviz.layer("Class Model");
+ viz::Layer layerAABB = arviz.layer("Class AABB");
+ viz::Layer layerOOBB = arviz.layer("Class OOBB");
+
+ if (coreSegment)
+ {
+ 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());
+
+ if (!aron.simoxXmlPath.package.empty())
+ {
+ layerObject.add(viz::Object(entityID.str())
+ .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)
+ .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)
+ .color(simox::Color::lime(255, 64)));
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ }
+
+ 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());
+
+ if (fs::is_regular_file(info.simoxXML().absolutePath))
+ {
+ toAron(data.simoxXmlPath, info.simoxXML());
+ }
+ if (fs::is_regular_file(info.wavefrontObj().absolutePath))
+ {
+ toAron(data.meshObjPath, info.wavefrontObj());
+ }
+ if (fs::is_regular_file(info.file(".wrl").absolutePath))
+ {
+ toAron(data.meshWrlPath, info.file(".wrl"));
+ }
+
+ auto aabb = info.loadAABB();
+ toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
+ auto oobb = info.loadOOBB();
+ toAron(data.oobb, oobb ? oobb.value() : simox::OrientedBoxf());
+
+ if (auto recogNames = info.loadRecognizedNames())
+ {
+ data.names.recognizedNames = recogNames.value();
+ }
+ if (auto spokenNames = info.loadSpokenNames())
+ {
+ data.names.spokenNames = spokenNames.value();
+ }
+
+ return data;
+ }
+
+
+ void Segment::RemoteGui::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ data.setup(segment);
+ visu.setup(segment);
+
+ VBoxLayout layout;
+ layout.addChildren({data.group, visu.group});
+
+ group = {};
+ group.setLabel("Class");
+ 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;
+
+ maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
+ maxHistorySize.setRange(1, 1e6);
+ infiniteHistory.setValue(segment.p.maxHistorySize == -1);
+
+ 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++;
+
+ group = {};
+ group.setLabel("Data");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::Data::update(Segment& segment)
+ {
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.p.maxHistorySize));
+ }
+ }
+ }
+
+
+
+ void Segment::RemoteGui::Visu::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showComboBox = {};
+ showOptionsIndex.clear();
+ for (const auto& [_, prov] : *segment.coreSegment)
+ {
+ for (const auto& [_, entity] : prov)
+ {
+ std::stringstream option;
+ option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
+ showComboBox.addOption(option.str());
+ showOptionsIndex.push_back(entity.id());
+ }
+ }
+ if (showOptionsIndex.empty())
+ {
+ showComboBox.addOption("<none>");
+ }
+ showButton.setLabel("Visualize Object Class");
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(showComboBox, {row, 0}, {1, 2});
+ row++;
+ grid.add(showButton, {row, 0}, {1, 2});
+ row++;
+
+ group = {};
+ group.setLabel("Visualization");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::Visu::update(Segment& segment)
+ {
+ if (showButton.wasClicked())
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..7356e6a1d9e490840f7c19b5e41e27506ddf2ecf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -0,0 +1,107 @@
+#pragma once
+
+#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>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.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/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/server/class/FloorVis.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ Segment(armem::server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void init();
+ void connect(viz::Client arviz);
+
+ void loadByObjectFinder(const std::string& objectsPackage);
+ void loadByObjectFinder(const ObjectFinder& finder);
+ void loadByObjectFinder();
+
+ void visualizeClass(const MemoryID& entityID, bool showAABB = true, bool showOOBB = true);
+
+
+ static arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
+
+
+ private:
+
+ armem::server::MemoryToIceAdapter& iceMemory;
+ armem::wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ ObjectFinder objectFinder;
+
+ viz::Client arviz;
+ FloorVis floorVis;
+
+
+ struct Properties
+ {
+ std::string coreSegmentName = "Class";
+ long maxHistorySize = -1;
+
+ std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
+ bool loadFromObjectsPackage = true;
+ };
+ Properties p;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ struct Data
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Data data;
+
+ struct Visu
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ std::vector<MemoryID> showOptionsIndex;
+ armarx::RemoteGui::Client::ComboBox showComboBox;
+ armarx::RemoteGui::Client::Button showButton;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Visu visu;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
similarity index 93%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index 522a5cc13d83acf95a4efe1c3289c3aec2912487..f07f526d60dc409bc2bbf7e2dc37389237d9ec96 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -24,15 +24,15 @@ namespace armarx::objpose::observer
"Remove objects whose confidence is lower than this value.");
}
- void Decay::updateConfidence(ObjectPose& pose, IceUtil::Time now) const
+ void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
{
float confidence = calculateConfidence(pose.timestamp, now);
pose.confidence = confidence;
}
- void Decay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const
+ void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
- for (ObjectPose& pose : objectPoses)
+ for (objpose::ObjectPose& pose : objectPoses)
{
if (pose.attachment)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
index ec64221f3e202395ff4f0f8c83d3bb737b6571be..a44652d5528bceefed921dec184fe8b87fd19bfe 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -23,8 +23,8 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
- void updateConfidence(ObjectPose& pose, IceUtil::Time now) const;
- void updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
+ void updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const;
+ void updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
private:
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
index a5e78b5942704a7e53a3aae34d10c32b07baaa73..973cf5a551228705b918560ad48648db5795ab1f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -120,6 +120,30 @@ namespace armarx::objpose::observer
}
+ RobotHeadMovement::Discard RobotHeadMovement::getDiscard()
+ {
+ Discard discard;
+ if (checkHeadVelocity)
+ {
+ if (isMoving())
+ {
+ movementStarts(discardIntervalAfterMoveMS);
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << discardUpdatesUntil;
+ discard.all = true;
+ }
+ else if (TimeUtil::GetTime() < discardUpdatesUntil)
+ {
+ discard.all = true;
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << discardUpdatesUntil;
+ }
+ else
+ {
+ discard.updatesUntil = discardUpdatesUntil;
+ }
+ }
+ return discard;
+ }
+
void RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
similarity index 88%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index fdd6078d9fcefeb619339c15f3e93029e90c3049..5a731720c49fdb33749edab4ca4c9792eedc0599 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -12,7 +12,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
namespace armarx
@@ -21,7 +21,7 @@ namespace armarx
using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
class RobotHeadMovement : public armarx::Logging
{
@@ -40,6 +40,14 @@ namespace armarx::objpose::observer
objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
+ struct Discard
+ {
+ std::optional<IceUtil::Time> updatesUntil;
+ bool all = false;
+ };
+ Discard getDiscard();
+
+
public:
bool checkHeadVelocity = true;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa2125dada86d895e7fe063cbdc20501623576b7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -0,0 +1,713 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.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/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <sstream>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("InstanceSegment");
+
+ oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ {
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ return objectInfo->loadOOBB();
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
+ return std::nullopt;
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ });
+
+ classNameToDatasetCache.setFetchFn([this](const std::string & className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
+ }
+
+ 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).");
+ 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.");
+
+ decay.defineProperties(defs, prefix + "decay.");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+
+ Segment::CommitStats Segment::commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<armem::Time> discardUpdatesUntil)
+ {
+ CommitStats stats;
+
+ // Build new poses.
+ objpose::ObjectPoseSeq newObjectPoses;
+ stats.numUpdated = 0;
+ for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
+ {
+ const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ // Check whether we have an old snapshot for this object.
+ std::optional<objpose::ObjectPose> previousPose;
+ const armem::wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+
+ previousPose = objpose::ObjectPose();
+ fromAron(data, *previousPose);
+ }
+
+ bool discard = false;
+ if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
+ {
+ // Dicard updates temporarily (e.g. due to head movement).
+ discard = true;
+ }
+ else if (previousPose)
+ {
+ if (p.discardSnapshotsWhileAttached && previousPose->attachment)
+ {
+ // Discard update due to active attachemnt.
+ discard = true;
+ }
+ else if (timestamp == previousPose->timestamp)
+ {
+ // Discard update as it is not new.
+ discard = true;
+ }
+ }
+
+ if (!discard)
+ {
+ // Update the entity.
+ stats.numUpdated++;
+
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ newPose.fromProvidedPose(provided, robot);
+
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
+ {
+ newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ }
+ }
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
+ }
+ }
+
+ commitObjectPoses(providerName, newObjectPoses);
+
+ return stats;
+ }
+
+ void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ // Update memory.
+ const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
+ if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ Commit commit;
+ for (const objpose::ObjectPose& pose : objectPoses)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+ {
+ update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
+ update.timeArrived = TimeUtil::GetTime();
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
+ {
+ toAron(dto.classID, instance->id());
+ }
+ update.instancesData.push_back(dto.toAron());
+ }
+
+ }
+ iceMemory.commit(commit);
+ }
+
+
+ wm::CoreSegment& Segment::getCoreSegment()
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ const wm::CoreSegment& Segment::getCoreSegment() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
+ {
+ ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+
+ objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
+ const std::string& providerName,
+ IceUtil::Time now)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+ armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
+ if (providerName.empty())
+ {
+ for (auto& [_, prov] : *coreSegment)
+ {
+ if (prov.hasEntity(entityID.entityName))
+ {
+ return &prov.getEntity(entityID);
+ }
+ }
+ return nullptr;
+ }
+ else
+ {
+ entityID.providerSegmentName = providerName;
+ if (coreSegment->hasProviderSegment(providerName))
+ {
+ armem::wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
+ return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+ }
+
+
+ void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ {
+ bool agentSynchronized = false;
+
+ for (ObjectPose& objectPose : objectPoses)
+ {
+ updateObjectPose(objectPose, now, robot, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ for (ObjectPose& pose : objectPoses)
+ {
+ updateObjectPose(pose, now, agent, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ updateAttachement(objectPose, agent, agentSynchronized);
+
+ if (decay.enabled)
+ {
+ decay.updateConfidence(objectPose, now);
+ }
+ }
+
+
+ objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ {
+ ObjectPoseSeq result;
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ return result;
+ }
+
+
+ void Segment::updateAttachement(
+ ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ {
+ if (!objectPose.attachment)
+ {
+ // No attachment, nothing to do.
+ return;
+ }
+ ARMARX_CHECK(objectPose.attachment);
+
+ if (!synchronized) // Synchronize only once.
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ synchronized = true;
+ }
+ objectPose.updateAttached(agent);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return getLatestObjectPoses(*coreSegment);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(coreSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(provSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
+ {
+ ObjectPose result;
+ getLatestObjectPose(entity, result);
+ return result;
+ }
+
+ void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, provSegment] : coreSeg)
+ {
+ getLatestObjectPoses(provSegment, out);
+ }
+ }
+
+ void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, entity] : provSegment)
+ {
+ if (!entity.empty())
+ {
+ ObjectPose& pose = out.emplace_back();
+ getLatestObjectPose(entity, pose);
+ }
+ }
+ }
+
+ void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
+ {
+ for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ fromAron(dto, out);
+ }
+ }
+
+
+ arondto::ObjectInstance Segment::getLatestInstanceData(const armem::wm::Entity& entity)
+ {
+ ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
+ const armem::wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+
+ ARMARX_CHECK_EQUAL(snapshot.size(), 1);
+ const armem::wm::EntityInstance& instance = snapshot.getInstance(0);
+
+ arondto::ObjectInstance data;
+ data.fromAron(instance.data());
+
+ return data;
+ }
+
+ std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ {
+ return oobbCache.get(id);
+ }
+
+ objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ {
+ try
+ {
+ return providers.at(providerName);
+ }
+ catch (const std::out_of_range&)
+ {
+ std::stringstream ss;
+ ss << "No provider with name '" << providerName << "' available.\n";
+ ss << "Available are:\n";
+ for (const auto& [name, _] : providers)
+ {
+ ss << "- '" << name << "'\n";
+ }
+ throw std::out_of_range(ss.str());
+ }
+ }
+
+
+
+ objpose::AttachObjectToRobotNodeOutput
+ Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
+ {
+ const armem::Time now = armem::Time::now();
+
+ objpose::AttachObjectToRobotNodeOutput output;
+ output.success = false; // We are not successful until proven otherwise.
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+
+ if (input.agentName != "" && input.agentName != this->robot->getName())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ << "\n(You can leave the agent name empty if there is only one agent.)\n"
+ << "\nKnown agents: " << std::vector<std::string> {this->robot->getName()};
+ return output;
+ }
+ VirtualRobot::RobotPtr agent = this->robot;
+
+ if (!agent->hasRobotNode(input.frameName))
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
+ << "' of agent '" << agent->getName() << "'.";
+ return output;
+ }
+ std::string frameName = input.frameName;
+
+
+ // Find object pose (provider name can be empty).
+ armem::wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
+ if (!objectEntity || objectEntity->empty())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
+ << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ return output;
+ }
+ arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
+
+ objpose::ObjectAttachmentInfo info;
+ info.agentName = agent->getName();
+ info.frameName = frameName;
+
+ if (input.poseInFrame)
+ {
+ info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
+ }
+ else
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+
+ armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ if (frameName == armarx::GlobalFrame)
+ {
+ info.poseInFrame = framed.toGlobalEigen(this->robot);
+ }
+ else
+ {
+ framed.changeFrame(this->robot, info.frameName);
+ info.poseInFrame = framed.toEigen();
+ }
+ }
+
+ // Store attachment in new entity snapshot.
+ {
+ armem::EntityUpdate update;
+ update.entityID = objectEntity->id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated = data;
+ toAron(updated.pose.attachment, info);
+ updated.pose.attachmentValid = true;
+ update.instancesData = { updated.toAron() };
+ }
+ objectEntity->update(update);
+ }
+
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
+ << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
+ << "Object pose in frame: \n" << info.poseInFrame;
+
+ output.success = true;
+ output.attachment = new objpose::data::ObjectAttachmentInfo();
+ output.attachment->frameName = info.frameName;
+ output.attachment->agentName = info.agentName;
+ output.attachment->poseInFrame = new Pose(info.poseInFrame);
+
+ return output;
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input)
+ {
+ const armem::Time now = armem::Time::now();
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+ std::string providerName = input.providerName;
+
+ std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
+ {
+ // Remove from latest pose (if it was cached).
+ // Find object pose (provider name can be empty).
+ armem::wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+ if (data.pose.attachmentValid)
+ {
+ attachment = data.pose.attachment;
+
+ // Store non-attached pose in new snapshot.
+ storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
+ }
+
+ if (providerName.empty())
+ {
+ providerName = data.pose.providerName;
+ }
+ }
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput output;
+ output.wasAttached = bool(attachment);
+ if (attachment)
+ {
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
+ << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ }
+ else
+ {
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ << "from robot node, but it was not attached.";
+ }
+
+ return output;
+ }
+
+
+ struct DetachVisitor : public armem::wm::Visitor
+ {
+ Segment& owner;
+ armem::Time now;
+ bool commitAttachedPose;
+
+ int numDetached = 0;
+
+ DetachVisitor(Segment& owner, armem::Time now, bool commitAttachedPose) :
+ owner(owner), now(now), commitAttachedPose(commitAttachedPose)
+ {
+ }
+
+ virtual bool visitEnter(armem::wm::Entity& entity) override;
+ };
+
+ bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
+ {
+ const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ numDetached++;
+ // Store non-attached pose in new snapshot.
+ owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
+ }
+
+ return false; // Stop descending.
+ }
+
+
+ objpose::DetachAllObjectsFromRobotNodesOutput Segment::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ const armem::Time now = armem::Time::now();
+
+ DetachVisitor visitor(*this, now, input.commitAttachedPose);
+ visitor.applyTo(*coreSegment);
+
+ objpose::DetachAllObjectsFromRobotNodesOutput output;
+ output.numDetached = visitor.numDetached;
+
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
+
+ return output;
+ }
+
+ void Segment::storeDetachedSnapshot(
+ armem::wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ armem::Time now,
+ bool commitAttachedPose)
+ {
+ armem::EntityUpdate update;
+ update.entityID = entity.id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated;
+ if (commitAttachedPose && data.pose.attachmentValid)
+ {
+ ObjectPose objectPose;
+ fromAron(data, objectPose);
+
+ bool agentSynchronized = false;
+ updateAttachement(objectPose, robot, agentSynchronized);
+
+ objectPose.attachment = std::nullopt;
+ toAron(updated, objectPose);
+ }
+ else
+ {
+ updated = data;
+ updated.pose.attachmentValid = false;
+ toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
+ }
+
+ update.instancesData = { updated.toAron() };
+ }
+ entity.update(update);
+ }
+
+
+ std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ {
+ const ObjectID classID = { objectID.dataset(), objectID.className() };
+ try
+ {
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
+ return std::nullopt;
+ }
+ catch (const armem::error::ArMemError&)
+ {
+ // Some segment or entity did not exist.
+ return std::nullopt;
+ }
+ }
+
+
+
+ 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();
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..54314054a38f5f104552c045fe37eb8b401651ae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -0,0 +1,191 @@
+#pragma once
+
+#include <map>
+#include <string>
+#include <optional>
+
+#include <SimoxUtility/caching/CacheMap.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.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"
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ struct CommitStats
+ {
+ int numUpdated = 0;
+ };
+ using ObjectPose = objpose::ObjectPose;
+ using ObjectPoseSeq = objpose::ObjectPoseSeq;
+
+
+ public:
+
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+
+ CommitStats commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+
+
+ wm::CoreSegment& getCoreSegment();
+ const wm::CoreSegment& getCoreSegment() const;
+
+
+ objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
+ objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+
+ wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
+ std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+
+ objpose::ProviderInfo getProviderInfo(const std::string& providerName);
+
+ objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
+ objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
+ objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
+
+
+ /**
+ * @brief If the object is attached to a robot node, update it according to the current robot state.
+ *
+ * If there is no attachement info in `objectPose` itself, the internal data
+ * structure `attachments` is queried. If an attachment is found there,
+ * it is written into the given `objectPose` (thus, it is "cached" in the
+ * info `objectPose`).
+ *
+ * @param synchronized Indicates whether the agent is already synchronized to the current time.
+ */
+ void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ bool& synchronized) const;
+
+
+ static ObjectPoseSeq getLatestObjectPoses(const wm::CoreSegment& coreSeg);
+ static ObjectPoseSeq getLatestObjectPoses(const wm::ProviderSegment& provSeg);
+ static ObjectPose getLatestObjectPose(const wm::Entity& entity);
+
+ static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
+
+ static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+
+
+ private:
+
+ ObjectPoseSeq getLatestObjectPoses() const;
+
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now);
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+ void updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+
+
+ ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+
+
+ void storeDetachedSnapshot(
+ wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose);
+
+
+ std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
+
+
+ friend struct DetachVisitor;
+
+
+ public:
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ VirtualRobot::RobotPtr robot;
+
+ objpose::ProviderInfoMap providers;
+
+
+ ObjectFinder objectFinder;
+
+ /// Decay model.
+ Decay decay;
+
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+
+ struct Properties
+ {
+ std::string coreSegmentName = "Instance";
+ long maxHistorySize = -1;
+ bool discardSnapshotsWhileAttached = true;
+ };
+ Properties p;
+
+
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ /// Class name -> dataset name.
+ simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+
+
+ 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);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3773c087bf70fa68981be86696b01c6fae7645d6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -0,0 +1,499 @@
+/*
+ * 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::armem_objects::SegmentAdapter
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SegmentAdapter.h"
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
+ segment(iceMemory, memoryMutex),
+ memoryMutex(memoryMutex)
+ {
+ }
+
+ std::string SegmentAdapter::getName() const
+ {
+ return Logging::tag.tagName;
+ }
+
+ void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ calibration.defineProperties(defs, prefix + "calibration.");
+ segment.defineProperties(defs, prefix);
+ robotHead.defineProperties(defs, prefix + "head.");
+ visu.defineProperties(defs, prefix + "visu.");
+ }
+
+ void SegmentAdapter::init()
+ {
+ segment.setTag(getName());
+ segment.decay.setTag(getName());
+ robotHead.setTag(getName());
+ visu.setTag(getName());
+
+ segment.init();
+ }
+
+ void SegmentAdapter::connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ )
+ {
+ this->debugObserver = debugObserver;
+ this->arviz = arviz;
+
+ segment.robot = robot;
+ segment.robotStateComponent = robotStateComponent;
+
+ robotHead.kinematicUnitObserver = kinematicUnitObserver;
+ robotHead.debugObserver = debugObserver;
+ robotHead.fetchDatafields();
+
+ visu.arviz = arviz;
+ if (!visu.updateTask)
+ {
+ visu.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ visu.updateTask->start();
+ }
+ }
+
+ void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+ {
+ updateProviderInfo(providerName, info);
+ }
+
+
+ void SegmentAdapter::reportObjectPoses(
+ const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
+ updateObjectPoses(providerName, providedPoses);
+ }
+
+
+ void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ {
+ if (!info.proxy)
+ {
+ ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+ return;
+ }
+ {
+ std::scoped_lock lock(memoryMutex);
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+ << "Supported objects: \n" << ss.str();
+ segment.providers[providerName] = info;
+ }
+ }
+
+
+ void SegmentAdapter::updateObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+ {
+ TIMING_START(tReportObjectPoses);
+
+ RobotHeadMovement::Discard discard;
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ discard = robotHead.getDiscard();
+ }
+ if (debugObserver)
+ {
+ StringVariantBaseMap map;
+ map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
+ if (discard.all)
+ {
+ map["Proportion Updated Poses"] = new Variant(0.f);
+ }
+ debugObserver->setDebugChannel(getName(), map);
+ }
+
+ if (discard.all)
+ {
+ return;
+ }
+
+ {
+ std::scoped_lock lock(memoryMutex);
+ RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
+
+ if (segment.robot->hasRobotNode(calibration.robotNode))
+ {
+ VirtualRobot::RobotNodePtr robotNode = segment.robot->getRobotNode(calibration.robotNode);
+ float value = robotNode->getJointValue();
+ robotNode->setJointValue(value + calibration.offset);
+ }
+
+ TIMING_START(tCommitObjectPoses);
+ Segment::CommitStats stats =
+ segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+ TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
+ { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
+ });
+ }
+
+ handleProviderUpdate(providerName);
+
+ TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
+ { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+
+
+ void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+ {
+ // Initialized to 0 on first access.
+ if (segment.providers.count(providerName) == 0)
+ {
+ segment.providers[providerName] = objpose::ProviderInfo();
+ }
+ }
+
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
+ {
+ TIMING_START(tGetObjectPoses);
+
+ TIMING_START(tGetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+
+ TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ {
+ TIMING_START(GetObjectPoses);
+
+ TIMING_START(GetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+
+ TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+
+ objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
+ const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
+ {
+ std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
+ std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
+
+ objpose::observer::RequestObjectsOutput output;
+
+ auto updateProxy = [&](const std::string & providerName)
+ {
+ if (proxies.count(providerName) == 0)
+ {
+ if (auto it = segment.providers.find(providerName); it != segment.providers.end())
+ {
+ proxies[providerName] = it->second.proxy;
+ }
+ else
+ {
+ ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
+ proxies[providerName] = nullptr;
+ }
+ }
+ };
+
+ if (input.provider.size() > 0)
+ {
+ providerRequests[input.provider] = input.request;
+ updateProxy(input.provider);
+ }
+ else
+ {
+ std::scoped_lock lock(memoryMutex);
+ for (const auto& objectID : input.request.objectIDs)
+ {
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
+ {
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ {
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
+ {
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
+ }
+ }
+ }
+
+ for (const auto& [providerName, request] : providerRequests)
+ {
+ if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
+ {
+ ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
+ << providerName << "' for " << request.relativeTimeoutMS << " ms.";
+ objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
+
+ int successful = 0;
+ for (const auto& [objectID, result] : providerOutput.results)
+ {
+ objpose::observer::ObjectRequestResult& res = output.results[objectID];
+ res.providerName = providerName;
+ res.result = result;
+ successful += int(result.success);
+ }
+ ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
+ }
+ }
+ return output;
+ }
+
+ objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers;
+ }
+
+ Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return simox::alg::get_keys(segment.providers);
+ }
+
+ objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.getProviderInfo(providerName);
+ }
+
+ bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers.count(providerName) > 0;
+ }
+
+
+ objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
+ const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.attachObjectToRobotNode(input);
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachObjectFromRobotNode(input);
+ }
+
+ objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachAllObjectsFromRobotNodes(input);
+ }
+
+
+ objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ }
+
+ objpose::SignalHeadMovementOutput
+ SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ return robotHead.signalHeadMovement(input);
+ }
+
+
+ void SegmentAdapter::visualizeRun()
+ {
+ CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+ while (visu.updateTask && !visu.updateTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(visuMutex);
+
+ if (visu.enabled)
+ {
+ TIMING_START(Visu);
+
+ objpose::ObjectPoseSeq objectPoses;
+ ObjectFinder objectFinder;
+ visu.minConfidence = -1;
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ objectPoses = segment.getObjectPoses(now);
+ objectFinder = segment.objectFinder;
+ if (segment.decay.enabled)
+ {
+ visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
+ }
+ }
+ const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+ arviz.commit(layers);
+
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+ }
+
+
+
+ void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ this->visu.setup(adapter.visu);
+ this->segment.setup(adapter.segment);
+ this->decay.setup(adapter.segment.decay);
+ this->robotHead.setup(adapter.robotHead);
+
+ layout = VBoxLayout
+ {
+ this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
+ VSpacer()
+ };
+
+ group = {};
+ group.setLabel("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
+ {
+ std::scoped_lock lock(adapter.visuMutex);
+ this->visu.update(adapter.visu);
+ }
+ {
+ std::scoped_lock lock(adapter.memoryMutex);
+ this->segment.update(adapter.segment);
+ this->decay.update(adapter.segment.decay);
+ }
+ {
+ std::scoped_lock lock(adapter.robotHeadMutex);
+ this->robotHead.update(adapter.robotHead);
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1e356e53c77608937b0df952798d0427a3cc86b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -0,0 +1,169 @@
+/*
+ * 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::armem_objects::Adapter
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Decay.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Visu.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ /**
+ * @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
+ */
+ class SegmentAdapter :
+ virtual public armarx::Logging
+ , virtual public armarx::armem::server::ObjectInstanceSegmentInterface
+ {
+ public:
+
+ SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ std::string getName() const;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+ void connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ );
+
+
+ // ObjectPoseTopic interface
+ public:
+ virtual void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
+ virtual void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+
+ // ObjectInstanceSegmentInterface interface
+ public:
+
+ // OBJECT POSES
+
+ virtual objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+ virtual objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+
+ // PROVIDER INFORMATION
+
+ virtual bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
+
+
+ // REQUESTING
+
+ virtual objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+ // ATTACHING
+
+ virtual objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
+
+ virtual objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
+
+ // HEAD MOVEMENT SIGNALS
+
+ virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+
+
+ private:
+
+ void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+ void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+ void handleProviderUpdate(const std::string& providerName);
+
+
+ // Visualization
+
+ void visualizeRun();
+
+
+ private:
+
+ viz::Client arviz;
+ DebugObserverInterfacePrx debugObserver;
+
+ instance::Segment segment;
+ std::mutex& memoryMutex;
+
+ instance::RobotHeadMovement robotHead;
+ std::mutex robotHeadMutex;
+
+ instance::Visu visu;
+ std::mutex visuMutex;
+
+
+ struct Calibration
+ {
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ };
+ Calibration calibration;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::VBoxLayout layout;
+
+ instance::Visu::RemoteGui visu;
+ instance::Segment::RemoteGui segment;
+ instance::Decay::RemoteGui decay;
+ instance::RobotHeadMovement::RemoteGui robotHead;
+
+ void setup(const SegmentAdapter& adapter);
+ void update(SegmentAdapter& adapter);
+ };
+
+ };
+
+}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
similarity index 71%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 467dba24f947b38c02b47044986cf3086775b6a1..5cc251ca984e22a0129411b6b05a59db79d8a543 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -1,11 +1,13 @@
#include "Visu.h"
+#include <SimoxUtility/math/pose.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -30,47 +32,71 @@ namespace armarx::objpose::observer
std::vector<viz::Layer> Visu::visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- layers.push_back(visualizeProvider(name, poses, minConfidence, objectFinder));
+ layers.push_back(visualizeProvider(name, poses, objectFinder));
}
return layers;
}
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const objpose::ObjectPoseSeq& objectPoses,
+ const ObjectFinder& objectFinder) const
+ {
+ std::map<std::string, viz::Layer> layers;
+
+ auto getLayer = [this, &layers](const std::string & providerName) -> viz::Layer &
+ {
+ auto it = layers.find(providerName);
+ if (it == layers.end())
+ {
+ it = layers.emplace(providerName, arviz.layer(providerName)).first;
+ }
+ return it->second;
+ };
+
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
+ }
+
+ return simox::alg::get_values(layers);
+ }
+
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
- float minConfidence,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
- for (const ObjectPose& objectPose : objectPoses)
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
- const bool show = not(objectPose.confidence < minConfidence);
- if (show)
- {
- visualizeObjectPose(layer, objectPose, objectFinder);
- }
+ visualizeObjectPose(layer, objectPose, objectFinder);
}
return layer;
}
void Visu::visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
+ const bool show = not(objectPose.confidence < minConfidence);
+ if (!show)
+ {
+ return;
+ }
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
- Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
{
- viz::Object object = viz::Object(key).pose(pose);
+ viz::Object object(key);
+ object.pose(pose);
if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
{
object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
@@ -92,9 +118,10 @@ namespace armarx::objpose::observer
if (oobbs && objectPose.localOOBB)
{
- const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
- layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
- .color(simox::Color::lime(255, 64)));
+ const simox::OrientedBoxf oobb = inGlobalFrame
+ ? objectPose.oobbGlobal().value()
+ : objectPose.oobbRobot().value();
+ layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
}
if (objectFrames)
{
@@ -130,7 +157,7 @@ namespace armarx::objpose::observer
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});
+ grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
row++;
grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
row++;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
similarity index 78%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 9677f6198adcd9f52918061925876860640ba999..1cd1d41c835e56e820ce3db8b3397b1e117d8202 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -14,7 +14,7 @@ namespace armarx
{
class ObjectFinder;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -28,21 +28,26 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
std::vector<viz::Layer> visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
+ const ObjectFinder& objectFinder
+ ) const;
+
+ /// Visualize the given object poses, with one layer per provider.
+ std::vector<viz::Layer> visualizeCommit(
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
- float minConfidence,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
+
void visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder
) const;
@@ -51,10 +56,11 @@ namespace armarx::objpose::observer
viz::Client arviz;
- bool enabled = false;
+ bool enabled = true;
float frequencyHz = 25;
bool inGlobalFrame = true;
+ float minConfidence = -1;
float alpha = 1.0;
bool alphaByConfidence = false;
bool oobbs = false;
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 7d667ba123002fc209437136933dfed58b1e763f..dda61ac81ac0846f24beee94a23025e41623553c 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -31,6 +31,7 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -148,14 +149,14 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- if (entity.history().empty())
+ if (entity.empty())
{
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.history().size();
+ ARMARX_INFO << "History size: " << entity.size();
- for (const auto &[ss, entitySnapshot] : entity.history())
+ for (const auto &[ss, entitySnapshot] : entity)
{
for (const auto& entityInstance : entitySnapshot.instances())
{
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
index 7e0d690a48da10f3d07290f6bf32281251a877e5..7ecffc237d480fd33c30226ef7f1cf02c09308de 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
@@ -63,7 +63,7 @@ namespace armarx
inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
{
- return aron::datanavigator::NDArrayNavigator::FromVector(laserScan);
+ return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
}
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index 124be827dda9a63c01b5af5da088806dbf99d47b..c45ff0856ef2185644f899a23be41b9943eb9dfa 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -1,3 +1,4 @@
add_subdirectory(core)
add_subdirectory(converter)
add_subdirectory(codegenerationhelper)
+add_subdirectory(common)
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7a4f383eb5118009de6cfb43299490e5fd8b83ab
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -0,0 +1,40 @@
+set(LIB_NAME aroncommon)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ aron
+
+ HEADERS
+ aron_conversions.h
+ aron_conversions/core.h
+ aron_conversions/armarx.h
+ aron_conversions/simox.h
+ aron_conversions/stl.h
+
+ SOURCES
+ aron_conversions/core.cpp
+ aron_conversions/armarx.cpp
+ aron_conversions/simox.cpp
+ aron_conversions/stl.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/AxisAlignedBoundingBox.xml
+ aron/OrientedBox.xml
+ aron/PackagePath.xml
+)
+
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml b/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4001f42e7c5f6530ea24ad563446f5318535ec0f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml
@@ -0,0 +1,22 @@
+<!--
+The ARON DTO of simox::AxisAlignedBoundingBox.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="simox::arondto::AxisAlignedBoundingBox">
+ <ObjectChild key="center">
+ <Position />
+ </ObjectChild>
+ <ObjectChild key="extents">
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c4a4e407fd7ce3aca8cdca5aacc991f05645023f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
@@ -0,0 +1,25 @@
+<!--
+The ARON DTO of simox::OrientedBoxf.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="simox::arondto::OrientedBox">
+ <ObjectChild key='center'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ <ObjectChild key='extents'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml b/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1c2faf0902ac04c0c2b3051f279683e65e37a9bb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml
@@ -0,0 +1,19 @@
+<!--
+The ARON DTO of armarx::PackagePath.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::PackagePath">
+ <ObjectChild key='package'>
+ <String />
+ </ObjectChild>
+ <ObjectChild key='path'>
+ <String />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f96d0285b9fb4dbd96abdb92c12fe9518ee88b6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "aron_conversions/core.h"
+
+#include "aron_conversions/armarx.h"
+#include "aron_conversions/simox.h"
+#include "aron_conversions/stl.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da205a5238351e06ee78224198c66e6d37151df9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -0,0 +1,14 @@
+#include "armarx.h"
+
+
+void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackagePath& bo)
+{
+ bo = { dto.package, dto.path };
+}
+void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackagePath& bo)
+{
+ const armarx::data::PackagePath icedto = bo.serialize();
+ dto.package = icedto.package;
+ dto.path = icedto.path;
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
new file mode 100644
index 0000000000000000000000000000000000000000..241067b7b70ef2f0016c3bfbd33e7a5beff2624b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+
+
+namespace armarx
+{
+ void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
+ void toAron(arondto::PackagePath& dto, const PackagePath& bo);
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..790714d9c04f963f91a7492b396f95b32f684754
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp
@@ -0,0 +1,2 @@
+#include "core.h"
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/core.h b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
new file mode 100644
index 0000000000000000000000000000000000000000..bcc537a288613e28f4441477654a8466bac56d7d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
@@ -0,0 +1,81 @@
+#pragma once
+
+
+
+namespace armarx::aron
+{
+
+ /**
+ * Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
+ * (Business Objects) and back.
+ *
+ * To allow conversion between custom ARON and C++ types, declare two
+ * functions in the namespace of the BO:
+ *
+ * @code
+ * // aron_conversions.h
+ *
+ * namespace bo_namespace
+ * {
+ * void toAron(arondto::MyObject& dto, const MyObject& bo);
+ * void fromAron(const arondto::MyObject& dto, MyObject& bo);
+ * }
+ * @endcode
+ *
+ * Note that the DTO always comes first, and the target object is
+ * non-const.
+ *
+ * In the implementation,
+ *
+ *
+ * @code
+ * // aron_conversions.cpp
+ *
+ * #include "aron_conversions.h"
+ * #include <Path/to/MyValue/aron_conversions.h>
+ *
+ * void bo_namespace::toAron(arondto::MyObject& dto, const MyObject& bo)
+ * {
+ * dto.name = bo.name;
+ * toAron(dto.myValue, bo.myValue);
+ * }
+ *
+ * void bo_namespace::fromAron(const arondto::MyObject& dto, MyObject& bo)
+ * {
+ * bo.name = dto.name;
+ * fromAron(dto.myValue, bo.myValue);
+ * }
+ * @endcode
+ */
+
+ // Same type
+ template <class T>
+ void toAron(T& dto, const T& bo)
+ {
+ dto = bo;
+ }
+ template <class T>
+ void fromAron(const T& dto, T& bo)
+ {
+ bo = dto;
+ }
+
+
+ // Generic return version
+
+ template <class DtoT, class BoT>
+ DtoT toAron(const BoT& bo)
+ {
+ DtoT dto;
+ toAron(dto, bo);
+ return dto;
+ }
+ template <class BoT, class DtoT>
+ BoT fromAron(const DtoT& dto)
+ {
+ BoT bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c87d13cdbd7e30aa5b7984cda69cb80261c378a7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
@@ -0,0 +1,30 @@
+#include "simox.h"
+
+
+
+void simox::fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo)
+{
+ bo.center() = dto.center;
+ bo.extents() = dto.extents;
+}
+
+void simox::toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo)
+{
+ dto.center = bo.center();
+ dto.extents = bo.extents();
+}
+
+
+
+void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+{
+ bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
+}
+
+void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
+{
+ dto.center = bo.center();
+ dto.orientation = bo.rotation();
+ dto.extents = bo.dimensions();
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
new file mode 100644
index 0000000000000000000000000000000000000000..64e847afbb06c5b58d92db3817c7adccf7e3b563
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
+
+
+namespace simox
+{
+ void fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo);
+ void toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo);
+
+ void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..857d161ec035df81f9b7c1aca873c30f5f4a21aa
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp
@@ -0,0 +1 @@
+#include "stl.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
new file mode 100644
index 0000000000000000000000000000000000000000..12c353e15d4e40975f6fae10cd927cd1393284d8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
@@ -0,0 +1,187 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <vector>
+
+#include "core.h"
+
+
+namespace armarx::aron
+{
+
+ // std::unique_ptr
+
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
+ {
+ if (bo)
+ {
+ toAron(dto, *bo);
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
+ {
+ bo = std::make_unique<BoT>();
+ fromAron(dto, *bo);
+ }
+
+
+ // std::optional
+
+ template <class DtoT, class BoT>
+ void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
+ {
+ if (bo.has_value())
+ {
+ dto = DtoT{};
+ toAron(*dto, *bo);
+ }
+ else
+ {
+ dto = std::nullopt;
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
+ {
+ if (dto.has_value())
+ {
+ bo = BoT{};
+ fromAron(*dto, *bo);
+ }
+ else
+ {
+ bo = std::nullopt;
+ }
+ }
+
+ // Flag-controlled optional
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
+ {
+ dtoValid = boValid;
+ if (boValid)
+ {
+ toAron(dto, bo);
+ }
+ else
+ {
+ dto = {};
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
+ {
+ boValid = dtoValid;
+ if (dtoValid)
+ {
+ fromAron(dto, bo);
+ }
+ else
+ {
+ bo = {};
+ }
+ }
+
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
+ {
+ dtoValid = bo.has_value();
+ if (dtoValid)
+ {
+ toAron(dto, *bo);
+ }
+ else
+ {
+ dto = {};
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
+ {
+ if (dtoValid)
+ {
+ bo = BoT{};
+ fromAron(dto, *bo);
+ }
+ else
+ {
+ bo = std::nullopt;
+ }
+ }
+
+
+ // std::vector
+
+ template <class DtoT, class BoT>
+ void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
+ {
+ dtos.clear();
+ dtos.reserve(bos.size());
+ for (const auto& bo : bos)
+ {
+ toAron(dtos.emplace_back(), bo);
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
+ {
+ bos.clear();
+ bos.reserve(dtos.size());
+ for (const auto& dto : dtos)
+ {
+ fromAron(dto, bos.emplace_back());
+ }
+ }
+
+ template <class DtoT, class BoT>
+ std::vector<DtoT> toAron(const std::vector<BoT>& bos)
+ {
+ std::vector<DtoT> dtos;
+ toAron(dtos, bos);
+ return dtos;
+ }
+
+
+ // std::map
+
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
+ const std::map<BoKeyT, BoValueT>& boMap)
+ {
+ dtoMap.clear();
+ for (const auto& [boKey, boValue] : boMap)
+ {
+ DtoKeyT dtoKey;
+ toAron(dtoKey, boKey);
+ auto [it, _] = dtoMap.emplace(std::move(dtoKey), DtoValueT{});
+ toAron(it->second, boValue);
+ }
+ }
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
+ std::map<BoKeyT, BoValueT>& boMap)
+ {
+ boMap.clear();
+ for (const auto& [dtoKey, dtoValue] : dtoMap)
+ {
+ BoKeyT boKey;
+ fromAron(dtoKey, boKey);
+ auto [it, _] = boMap.emplace(boKey, BoValueT{});
+ fromAron(dtoValue, it->second);
+ }
+ }
+
+
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
+ {
+ std::map<DtoKeyT, DtoValueT> dtoMap;
+ toAron(dtoMap, boMap);
+ return dtoMap;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8562347796faaae591d21c755fa2730776fa74e3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt
@@ -0,0 +1,14 @@
+
+add_library(aroncommon_test_files SHARED
+ MyCustomType.h
+ MyCustomType.cpp
+)
+target_link_libraries(aroncommon_test_files PRIVATE aroncommon)
+
+
+# Libs required for the tests
+SET(LIBS
+ aroncommon
+ aroncommon_test_files
+)
+armarx_add_test(aron_common_test aron_common_test.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6431732ee0e295d826a8af01995e20584789c7eb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
@@ -0,0 +1,67 @@
+/*
+ * 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::aron_common::aron_common
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "MyCustomType.h"
+
+#include <ostream>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
+
+template <class CustomTypeT>
+std::ostream& ostreamop(std::ostream& os, const CustomTypeT& rhs)
+{
+ return os << "(name='" << rhs.name << "'"
+ << " index=" << rhs.index
+ << " value=" << rhs.value
+ << ")";
+}
+
+std::ostream& my::operator<<(std::ostream& os, const CustomType& rhs)
+{
+ return ostreamop(os, rhs);
+}
+
+std::ostream& my::arondto::operator<<(std::ostream& os, const CustomType& rhs)
+{
+ return ostreamop(os, rhs);
+}
+
+bool my::operator==(const CustomType& lhs, const arondto::CustomType& rhs)
+{
+ return lhs.name == rhs.name
+ && lhs.index == rhs.index
+ && lhs.value == rhs.value;
+}
+
+
+void my::toAron(arondto::CustomType& dto, const CustomType& bo)
+{
+ dto.name = bo.name;
+ armarx::aron::toAron(dto.index, bo.index);
+ armarx::aron::toAron(dto.value, bo.value);
+}
+void my::fromAron(const arondto::CustomType& dto, CustomType& bo)
+{
+ bo.name = dto.name;
+ armarx::aron::fromAron(dto.index, bo.index);
+ armarx::aron::fromAron(dto.value, bo.value);
+}
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.h b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
new file mode 100644
index 0000000000000000000000000000000000000000..31a4507dbfc02a4100bae5426b4c2b2a9d87c447
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
@@ -0,0 +1,117 @@
+/*
+ * 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::aron_common::aron_common
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+
+namespace my
+{
+ struct CustomType
+ {
+ std::string name;
+ int index = 0;
+ float value = 0.0;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const CustomType& rhs);
+
+
+ namespace arondto
+ {
+ struct CustomType
+ {
+ std::string name;
+ int index;
+ float value;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const arondto::CustomType& rhs);
+ }
+
+ bool operator==(const CustomType& lhs, const arondto::CustomType& rhs);
+ bool operator!=(const CustomType& lhs, const arondto::CustomType& rhs)
+ {
+ return !(lhs == rhs);
+ }
+ bool operator==(const arondto::CustomType& lhs, const CustomType& rhs)
+ {
+ return rhs == lhs;
+ }
+ bool operator!=(const arondto::CustomType& lhs, const CustomType& rhs)
+ {
+ return !(rhs == lhs);
+ }
+
+ void toAron(arondto::CustomType& dto, const CustomType& bo);
+ void fromAron(const arondto::CustomType& dto, CustomType& bo);
+}
+
+
+namespace std
+{
+
+ template <class L1, class L2, class R1, class R2>
+ bool operator==(const std::pair<L1, L2>& lhs,
+ const std::pair<R1, R2>& rhs)
+ {
+ return lhs.first == rhs.first && lhs.second == rhs.second;
+ }
+ template <class L1, class L2, class R1, class R2>
+ bool operator!=(const std::pair<L1, L2>& lhs,
+ const std::pair<R1, R2>& rhs)
+ {
+ return !(lhs == rhs);
+ }
+
+ template <class L1, class L2>
+ std::ostream& operator<<(std::ostream& os, const std::pair<L1, L2>& pair)
+ {
+ return os << "(" << pair.first << " | " << pair.second << ")";
+ }
+
+
+ template <class L, class R>
+ bool operator==(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ {
+ if (lhs.size() != rhs.size())
+ {
+ return false;
+ };
+ for (size_t i = 0; i < lhs.size(); ++i)
+ {
+ if (lhs[i] != rhs[i])
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+ template <class L, class R>
+ bool operator!=(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ {
+ return !(lhs == rhs);
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0503f01327f4f8c6653bbf09d379f79549ccc5fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
@@ -0,0 +1,261 @@
+/*
+ * 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::ArmarXObjects
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron_common
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
+#include <iostream>
+
+#include "MyCustomType.h"
+
+
+BOOST_AUTO_TEST_CASE(test_direct)
+{
+ const my::CustomType bo { "name", -10, 42.42f };
+
+ my::arondto::CustomType dto;
+ toAron(dto, bo);
+ BOOST_CHECK_EQUAL(dto, bo);
+
+ my::CustomType boOut;
+ fromAron(dto, boOut);
+ BOOST_CHECK_EQUAL(boOut, dto);
+}
+
+
+template <class BOs, class DTOs>
+void test_complex(const BOs bos, DTOs& dtos)
+{
+ armarx::aron::toAron(dtos, bos);
+ BOOST_CHECK_EQUAL_COLLECTIONS(dtos.begin(), dtos.end(),
+ bos.begin(), bos.end());
+
+ BOs bosOut;
+ armarx::aron::fromAron(dtos, bosOut);
+ BOOST_CHECK_EQUAL_COLLECTIONS(bosOut.begin(), bosOut.end(),
+ dtos.begin(), dtos.end());
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_vector)
+{
+ const std::vector<my::CustomType> bos
+ {
+ { "name", -10, 42.42f },
+ { "name2", 20, -128.128f },
+ };
+ std::vector<my::arondto::CustomType> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_map)
+{
+ const std::map<std::string, my::CustomType> bos
+ {
+ { "key1", { "name", -10, 42.42f } },
+ { "key2", { "name2", 20, -128.128f } },
+ };
+ std::map<std::string, my::arondto::CustomType> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_vector_of_vector)
+{
+ const std::vector<std::vector<my::CustomType>> bos
+ {
+ { { "name", -10, 42.42f } },
+ { },
+ { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } },
+ };
+ std::vector<std::vector<my::arondto::CustomType>> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_map_of_vector)
+{
+ const std::map<std::string, std::vector<my::CustomType>> bos
+ {
+ { "key1", { { "name", -10, 42.42f } } },
+ { "key2", { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } } },
+ { "key3", { } },
+ };
+ std::map<std::string, std::vector<my::arondto::CustomType>> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_optional)
+{
+ using std::nullopt;
+
+ std::optional<my::CustomType> bo;
+ std::optional<my::arondto::CustomType> dto;
+
+ auto reset = [&](
+ std::optional<my::CustomType> theBo,
+ std::optional<my::arondto::CustomType> theDto)
+ {
+ bo = theBo;
+ dto = theDto;
+ };
+
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(!dto.has_value());
+ }
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(dto.has_value());
+ BOOST_CHECK_EQUAL(dto.value(), bo.value());
+ }
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(!dto.has_value());
+ }
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto.value());
+ }
+
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(dto.has_value());
+ BOOST_CHECK_EQUAL(dto.value(), bo.value());
+ BOOST_CHECK_EQUAL(dto->name, "bo");
+ }
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto.value());
+ BOOST_CHECK_EQUAL(bo->name, "dto");
+ }
+}
+
+
+
+BOOST_AUTO_TEST_CASE(test_optional_value_flagged)
+{
+ using std::nullopt;
+
+ std::optional<my::CustomType> bo;
+ my::arondto::CustomType dto;
+ bool dtoValid;
+
+ auto reset = [&](
+ std::optional<my::CustomType> theBo,
+ std::optional<my::arondto::CustomType> theDto)
+ {
+ bo = theBo;
+ if (theDto)
+ {
+ dto = *theDto;
+ dtoValid = true;
+ }
+ else
+ {
+ dto = {};
+ dtoValid = false;
+ }
+ };
+
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(!dtoValid);
+ }
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(dtoValid);
+ BOOST_CHECK_EQUAL(dto, bo.value());
+ }
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(!dtoValid);
+ }
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto);
+ }
+
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(dtoValid);
+ BOOST_CHECK_EQUAL(dto, bo.value());
+ BOOST_CHECK_EQUAL(dto.name, "bo");
+ }
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto);
+ BOOST_CHECK_EQUAL(bo->name, "dto");
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
index 9eaaa7c6e79174badf7f0e4e035693660f145564..78cce4a1a7d2d9b6d9f5fc9a1f156683c67c0d71 100644
--- a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
@@ -64,6 +64,19 @@ namespace armarx::aron::converter
return v;
}
+
+
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromVector(const std::vector<T>& data)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(data.size()), sizeof(T)});
+ ndArr->setData(sizeof(T) * data.size(), reinterpret_cast <const unsigned char* >(data.data()));
+
+ return ndArr;
+ }
+
};
} // namespace armarx::aron::converter
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
index e266d962d5b1b2d6bc691354f5a80b1b4c38761d..faa6fadc1fbf5f3776ca73ee2363ff980713b41e 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
@@ -53,4 +53,44 @@ namespace armarx::aron::converter
{
return ConvertToMatrix<double, 4, 4>(n);
}
+
+ Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToQuaternion<float>(n);
+ }
+ Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToQuaternion<double>(n);
+ }
+ Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToVector<float, 3>(n);
+ }
+ Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToVector<double, 3>(n);
+ }
+ Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToMatrix<float, 4, 4>(n);
+ }
+ Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToMatrix<double, 4, 4>(n);
+ }
+
+ void AronEigenConverter::checkDimensions(
+ const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ const std::string& method, const std::string& caller)
+ {
+ if (nav.getDimensions() != expected)
+ {
+ std::stringstream ss;
+ ss << "The size of an NDArray does not match.";
+ ss << "\n Expected: \t" << datanavigator::NDArrayNavigator::DimensionsToString(expected);
+ ss << "\n Got: \t" << datanavigator::NDArrayNavigator::DimensionsToString(nav.getDimensions());
+ ss << "\n";
+ throw error::AronException(caller, method, ss.str(), nav.getPath());
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 06a973559c24b42dd306b62f474e12a40f0badfa..787ae30a23f097821989fcd9658a3fa31d6b4761 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -43,62 +43,85 @@ namespace armarx::aron::converter
public:
// TODO: Add inplace methods
- // TODO: Remove Ptrs for references
static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigator&);
static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigator&);
static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigator&);
static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigator&);
static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigator&);
static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigator&);
+
template<typename T>
static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToQuaternion<T>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({1, 4, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToQuaternion", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T>
+ static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion");
+ auto dims = nav.getDimensions();
Eigen::Quaternion<T> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
template<typename T, int Size>
static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToVector<T, Size>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({Size, 1, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToVector", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T, int Size>
+ static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector");
+ auto dims = nav.getDimensions();
Eigen::Matrix<T, Size, 1> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
template<typename T, int Rows, int Cols>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToMatrix<T, Rows, Cols>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({Rows, Cols, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToMatrix", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T, int Rows, int Cols>
+ static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ auto dims = nav.getDimensions();
Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
+ private:
+
+ /**
+ * @throw `error::AronException` If `nav`'s dimensions do not match `expected`.
+ */
+ static void checkDimensions(const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ const std::string& method, const std::string& caller = "AronEigenConverter");
+
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index 27c683a382afc5f378faafa7fd8c93b94a41e31d..c7f32781eb6fbcd4776b44416a894d80162cfc4c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -186,7 +186,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for copy \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + getFullCppTypename() + "& c)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(const " + getFullCppTypename() + "& c)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = c.value;");
m->setBlock(b);
@@ -199,7 +199,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + enumName + " v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(" + enumName + " v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = v;");
m->setBlock(b);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
index 5e447b102d889f2ba91d95e23a60a89028cf1ff4..c6d12cc6b896a53a8b28cbc2e6c134fae58612f0 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
@@ -68,31 +68,10 @@ namespace armarx::aron::datanavigator
/// Return dimensions in a readable string such as "(2, 3, 4)".
static std::string DimensionsToString(const std::vector<int>& dimensions);
- // TODO(fabian.reister): move this to VectorConverter?
- template<typename T>
- static NDArrayNavigatorPtr FromVector(const std::vector<T>& data)
- {
- NDArrayNavigatorPtr ndArr(new NDArrayNavigator);
-
- ndArr->setDimensions({static_cast<int>(data.size()), sizeof(T)});
- ndArr->setData(data);
-
- return ndArr;
- }
-
-
// public member functions
unsigned char* getData() const;
void setData(unsigned int, const unsigned char*);
- // TODO(fabian.reister): move this to VectorConverter?
- template<typename T>
- void setData(const std::vector<T>& data)
- {
- using E = typename decltype(aron->data)::value_type;
- setData(sizeof(T) * data.size(), reinterpret_cast < const E* >(data.data()));
- }
-
std::vector<int> getDimensions() const;
void setDimensions(const std::vector<int>&);
void addDimension(int);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
index 263253c8830ccd065648fd210be9721a1ea28030..55a30cff91afbd74bbf4cbae769df34992843096 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
@@ -87,8 +87,20 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
}
+ else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
- if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenQuaternionTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
@@ -100,7 +112,15 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
- else if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ else if (auto t = dynamic_cast<PoseTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<PositionTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OrientationTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
index 13800fa67e320bfa45d144b004baf3ec584f7225..f577903a4524da4ce220d6d402e9719f1afdac7a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
@@ -29,8 +29,8 @@ namespace armarx::aron::visitor
* To the the location of the passed navigator in the original `AronType`,
* use `navigator.getPath()`.
*
- * @see `aron::Type::AronTypePtr`
- * @see `aron::Typenavigator::Navigator`
+ * @see `aron::Type::AronTypePtr`
+ * @see `aron::Typenavigator::Navigator`
* @see `std::stack` To manage Type with stack semantics.
*/
class TypedDataVisitor
@@ -73,12 +73,18 @@ namespace armarx::aron::visitor
using IntTypeNavigator = typenavigator::IntNavigator;
using LongTypeNavigator = typenavigator::LongNavigator;
using StringTypeNavigator = typenavigator::StringNavigator;
+ using TimeTypeNavigator = typenavigator::TimeNavigator;
+
// Array-valued
using EigenMatrixTypeNavigator = typenavigator::EigenMatrixNavigator;
+ using EigenQuaternionTypeNavigator = typenavigator::EigenQuaternionNavigator;
using IVTCByteImageTypeNavigator = typenavigator::IVTCByteImageNavigator;
using OpenCVMatTypeNavigator = typenavigator::OpenCVMatNavigator;
using PCLPointCloudTypeNavigator = typenavigator::PCLPointCloudNavigator;
+ using PoseTypeNavigator = typenavigator::PoseNavigator;
+ using PositionTypeNavigator = typenavigator::PositionNavigator;
+ using OrientationTypeNavigator = typenavigator::OrientationNavigator;
public:
@@ -166,26 +172,55 @@ namespace armarx::aron::visitor
(void) type, (void) data;
return true;
}
+ virtual bool visit(TimeTypeNavigator& type, LongDataNavigator& data)
+ {
+ return visit(type, IceUtil::Time::microSeconds(data.getValue()));
+ }
+ virtual bool visit(TimeTypeNavigator& type, const IceUtil::Time& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
virtual bool visit(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(EigenQuaternionTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
return true;
}
virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
return true;
}
virtual bool visit(OpenCVMatTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
return true;
}
virtual bool visit(PCLPointCloudTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(PoseTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(PositionTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(OrientationTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
return true;
}
@@ -194,119 +229,130 @@ namespace armarx::aron::visitor
virtual bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data)
+ {
+ (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(TimeTypeNavigator& type, const std::string& key, const IceUtil::Time& data)
+ {
+ (void) key;
+ return visit(type, data);
}
virtual bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
}
virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
}
-
private:
@@ -322,76 +368,3 @@ namespace armarx::aron::visitor
};
}
-
-
-/* Copy-and-paste example below.
- * Remove functions you don't need.
- * Add `const std::string& key, ` to parameter list if you need an items's key
- * in the parent container.
- */
-
-#if 0
-
-struct MyDerivedAronTypeVisitor : public armarx::aron::visitor::AronTypeVisitor
-{
-
- bool visitEnter(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitExit(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitEnter(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
- bool visitExit(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
-
- bool visit(BoolTypeNavigator& b) override
- {
- (void) b;
- return true;
- }
- bool visit(DoubleTypeNavigator& d) override
- {
- (void) d;
- return true;
- }
- bool visit(FloatTypeNavigator& f) override
- {
- (void) f;
- return true;
- }
- bool visit(IntTypeNavigator& i) override
- {
- (void) i;
- return true;
- }
- bool visit(LongTypeNavigator& l) override
- {
- (void) l;
- return true;
- }
- bool visit(NDArrayTypeNavigator& array) override
- {
- (void) array;
- return true;
- }
- bool visit(StringTypeNavigator& string) override
- {
- (void) string;
- return true;
- }
-
-};
-
-#endif
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 5743f098b27b9de73ed56047ee020089a046af22..9babf0b23f1eeae6bc2bc01e9238d93809c6c40c 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -49,7 +49,7 @@ set(LIB_FILES
CartesianVelocityControllerWithRamp.cpp
CartesianNaturalPositionController.cpp
#CartesianNaturalVelocityController.cpp
-
+
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
@@ -102,7 +102,7 @@ set(LIB_HEADERS
CartesianNaturalPositionController.h
#CartesianNaturalVelocityController.h
EigenHelpers.h
-
+
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
@@ -114,3 +114,5 @@ set(LIB_HEADERS
add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_library(RobotAPI::Core ALIAS RobotAPICore)