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/ArMemExample.scx b/scenarios/ArMemExample/ArMemExample.scx
index fc00a01308fd61be772d4b3362bca7656d66f34d..49d2a0a435b2f4fdb13424d5d0f20488922b3b3d 100644
--- a/scenarios/ArMemExample/ArMemExample.scx
+++ b/scenarios/ArMemExample/ArMemExample.scx
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="ArMemExample" creation="2020-11-10.10:38:44" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="ArMemExampleMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ArMemExampleClient" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ArMemMemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ExampleMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ExampleMemoryClient" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemExample/config/ArMemExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
similarity index 77%
rename from scenarios/ArMemExample/config/ArMemExampleMemory.cfg
rename to scenarios/ArMemExample/config/ExampleMemory.cfg
index a7b234054d462683b2dd20aab5b0f08e23a9af69..d9e24e657c2b57a11bac7d34b2df65dff2e9eb8d 100644
--- a/scenarios/ArMemExample/config/ArMemExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# ArMemExampleMemory properties
+# ExampleMemory 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.
@@ -18,155 +18,171 @@
# ArmarX.ApplicationName = ""
-# ArmarX.ArMemExampleMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener: No Description
# Attributes:
-# - Default: false
+# - Default: MemoryUpdates
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# 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
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ArMemExampleMemory.EnableProfiling = false
+# ArmarX.CachePath = mongo/.cache
-# ArmarX.ArMemExampleMemory.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
-# - Default: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ArMemExampleMemory.MinimumLoggingLevel = Undefined
+# ArmarX.Config = ""
-# ArmarX.ArMemExampleMemory.ObjectName: Name of IceGrid well-known object
+# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.ObjectName = ""
+# ArmarX.DataPath = ""
-# ArmarX.ArMemExampleMemory.RemoteGuiName: Name of the remote gui provider
+# 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: RemoteGuiProvider
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.RemoteGuiName = RemoteGuiProvider
+# 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.ArMemExampleMemory.core.AddOnUsage: If enabled, core segments are added when required by a new provider segment.
+# 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.ArMemExampleMemory.core.AddOnUsage = false
+# ArmarX.DisableLogging = false
-# ArmarX.ArMemExampleMemory.core.DefaultSegments: Core segments to add on start up.
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
-# - Default: ExampleModality, ExampleConcept
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.core.DefaultSegments = ExampleModality, ExampleConcept
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
-# ArmarX.ArMemExampleMemory.memory.Name: Name of this memory.
+# ArmarX.ExampleMemory.:
# Attributes:
-# - Default: Example
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.memory.Name = Example
+# ArmarX.ExampleMemory. = ""
-# ArmarX.ArMemExampleMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.ExampleMemory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ArMemExampleMemory.mns.MemoryNameSystemEnabled = true
+# ArmarX.ExampleMemory.EnableProfiling = false
-# ArmarX.ArMemExampleMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.ExampleMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: ArMemMemoryNameSystem
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.mns.MemoryNameSystemName = ArMemMemoryNameSystem
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ExampleMemory.MinimumLoggingLevel = Undefined
-# ArmarX.ArMemExampleMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.ExampleMemory.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: DebugObserver
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleMemory.tpc.pub.DebugObserver = DebugObserver
+# ArmarX.ExampleMemory.ObjectName = ""
-# ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.ExampleMemory.RemoteGuiName: Name of the remote gui provider
# Attributes:
-# - Default: MemoryUpdates
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ExampleMemory.RemoteGuiName = RemoteGuiProvider
-# 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)
+# ArmarX.ExampleMemory.core.AddOnUsage: If enabled, core segments are added when required by a new provider segment.This will usually be off for most memory servers.
# Attributes:
-# - Default: mongo/.cache
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.CachePath = mongo/.cache
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.core.AddOnUsage = false
-# ArmarX.Config: Comma-separated list of configuration files
+# ArmarX.ExampleMemory.core.DefaultSegments: Core segments to add on start up (just as example).
# Attributes:
-# - Default: ""
+# - Default: ExampleModality, ExampleConcept
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Config = ""
+# ArmarX.ExampleMemory.core.DefaultSegments = ExampleModality, ExampleConcept
-# ArmarX.DataPath: Semicolon-separated search list for data files
+# ArmarX.ExampleMemory.memory.Name: Name of this memory (server).
# Attributes:
-# - Default: ""
+# - Default: Example
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DataPath = ""
+# ArmarX.ExampleMemory.memory.Name = Example
-# 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'.
+# ArmarX.ExampleMemory.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: Default value not mapped.
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled = true
-# 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.
+# ArmarX.ExampleMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ./config/dependencies.cfg
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+# ArmarX.ExampleMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.DisableLogging: Turn logging off in whole application
+# ArmarX.ExampleMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: false
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
+# ArmarX.ExampleMemory.tpc.pub.DebugObserver = DebugObserver
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# ArmarX.ExampleMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
# Attributes:
-# - Default: false
+# - Default: MemoryUpdates
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
+# ArmarX.ExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# 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;...
diff --git a/scenarios/ArMemExample/config/ArMemExampleClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
similarity index 80%
rename from scenarios/ArMemExample/config/ArMemExampleClient.cfg
rename to scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index ceaa86ed36d6745a23d3fd00018e667d2cf289f4..10325be750b47ccdfa1827265cce4d881d58d2ed 100644
--- a/scenarios/ArMemExample/config/ArMemExampleClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# ArMemExampleClient properties
+# ExampleMemoryClient 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.
@@ -18,138 +18,146 @@
# ArmarX.ApplicationName = ""
-# ArmarX.ArMemExampleClient.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ArMemExampleClient.tpc.sub.MemoryListener: No Description
# Attributes:
-# - Default: false
+# - Default: MemoryUpdates
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# 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
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ArMemExampleClient.EnableProfiling = false
+# ArmarX.CachePath = mongo/.cache
-# ArmarX.ArMemExampleClient.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
-# - Default: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ArMemExampleClient.MinimumLoggingLevel = Undefined
+# ArmarX.Config = ""
-# ArmarX.ArMemExampleClient.ObjectName: Name of IceGrid well-known object
+# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleClient.ObjectName = ""
+# ArmarX.DataPath = ""
-# ArmarX.ArMemExampleClient.RemoteGuiName: Name of the remote gui provider
+# 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: RemoteGuiProvider
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleClient.RemoteGuiName = RemoteGuiProvider
+# ArmarX.DefaultPackages = Default value not mapped.
-# ArmarX.ArMemExampleClient.mem.MemoryName: Name of the memory to use.
+# 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: Example
+# - Default: ./config/dependencies.cfg
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleClient.mem.MemoryName = Example
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-# ArmarX.ArMemExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ArMemExampleClient.mns.MemoryNameSystemEnabled = true
+# ArmarX.DisableLogging = false
-# ArmarX.ArMemExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
-# - Default: ArMemMemoryNameSystem
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleClient.mns.MemoryNameSystemName = ArMemMemoryNameSystem
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
-# ArmarX.ArMemExampleClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.ExampleMemoryClient.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: DebugObserver
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArMemExampleClient.tpc.pub.DebugObserver = DebugObserver
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemoryClient.EnableProfiling = false
-# ArmarX.ArMemExampleClient.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# ArmarX.ExampleMemoryClient.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: MemoryUpdates
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ExampleMemoryClient.MinimumLoggingLevel = Undefined
-# 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)
+# ArmarX.ExampleMemoryClient.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: mongo/.cache
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.CachePath = mongo/.cache
+# ArmarX.ExampleMemoryClient.ObjectName = ""
-# ArmarX.Config: Comma-separated list of configuration files
+# ArmarX.ExampleMemoryClient.RemoteGuiName: Name of the remote gui provider
# Attributes:
-# - Default: ""
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Config = ""
+# ArmarX.ExampleMemoryClient.RemoteGuiName = RemoteGuiProvider
-# ArmarX.DataPath: Semicolon-separated search list for data files
+# ArmarX.ExampleMemoryClient.mem.MemoryName: Name of the memory to use.
# Attributes:
-# - Default: ""
+# - Default: Example
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DataPath = ""
+# ArmarX.ExampleMemoryClient.mem.MemoryName = Example
-# 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'.
+# ArmarX.ExampleMemoryClient.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: Default value not mapped.
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemEnabled = true
-# 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.
+# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ./config/dependencies.cfg
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.DisableLogging: Turn logging off in whole application
+# ArmarX.ExampleMemoryClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: false
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
+# ArmarX.ExampleMemoryClient.tpc.pub.DebugObserver = DebugObserver
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# ArmarX.ExampleMemoryClient.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
# Attributes:
-# - Default: false
+# - Default: MemoryUpdates
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
+# ArmarX.ExampleMemoryClient.tpc.sub.MemoryListener = MemoryUpdates
# 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;...
diff --git a/scenarios/ArMemExample/config/ArMemMemoryNameSystem.cfg b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
similarity index 90%
rename from scenarios/ArMemExample/config/ArMemMemoryNameSystem.cfg
rename to scenarios/ArMemExample/config/MemoryNameSystem.cfg
index 194953115f319abc741ff5298186673ee668820e..7dd22218243ca4f9e67e843da8b42916f3b8568a 100644
--- a/scenarios/ArMemExample/config/ArMemMemoryNameSystem.cfg
+++ b/scenarios/ArMemExample/config/MemoryNameSystem.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# ArMemMemoryNameSystem properties
+# 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.
@@ -18,40 +18,6 @@
# ArmarX.ApplicationName = ""
-# ArmarX.ArMemMemoryNameSystem.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.ArMemMemoryNameSystem.EnableProfiling = false
-
-
-# ArmarX.ArMemMemoryNameSystem.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.ArMemMemoryNameSystem.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.ArMemMemoryNameSystem.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArMemMemoryNameSystem.ObjectName = ""
-
-
-# ArmarX.ArMemMemoryNameSystem.RemoteGuiName: Name of the remote gui provider
-# Attributes:
-# - Default: RemoteGuiProvider
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArMemMemoryNameSystem.RemoteGuiName = RemoteGuiProvider
-
-
# 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
@@ -126,6 +92,40 @@
# 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
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/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
deleted file mode 100644
index d173c24a7a110c93d94a95d3df6b7c1d1a6d5e78..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
+++ /dev/null
@@ -1,37 +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::ObjectPoseProviderExample
- * @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 ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
- armarx::ObjectPoseProviderExample instance;
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index b80992aeaf1f03968a7b229d600def905796d1f7..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/RobotSensorMemory)
-add_subdirectory(server/SkillsMemory)
+add_subdirectory(server)
# memory server addons
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index a8e816bf283467a0a0efbadf9fff40d555ec153b..4eda2528386b1edd333bab29f06480255434b69d 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1 +1 @@
-add_subdirectory(ExampleClient)
+add_subdirectory(ExampleMemoryClient)
diff --git a/source/RobotAPI/components/armem/client/ExampleClient/test/CMakeLists.txt b/source/RobotAPI/components/armem/client/ExampleClient/test/CMakeLists.txt
deleted file mode 100644
index 775fb5e2e01b19ab9996a6be1b7575fb53caade6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/client/ExampleClient/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ArMemExampleClient)
-
-armarx_add_test(ArMemExampleClientTest ArMemExampleClientTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/armem/client/ExampleClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
similarity index 84%
rename from source/RobotAPI/components/armem/client/ExampleClient/CMakeLists.txt
rename to source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
index a6c8b9c637db73dc5cae91c430346542b834dec4..d699638cd0eb88c194fd66a9382968012ce40660 100644
--- a/source/RobotAPI/components/armem/client/ExampleClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
@@ -1,4 +1,4 @@
-armarx_component_set_name("ExampleClient")
+armarx_component_set_name("ExampleMemoryClient")
find_package(IVT QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
@@ -11,11 +11,11 @@ set(COMPONENT_LIBS
)
set(SOURCES
- ExampleClient.cpp
+ ExampleMemoryClient.cpp
)
set(HEADERS
- ExampleClient.h
+ ExampleMemoryClient.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
similarity index 81%
rename from source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.cpp
rename to source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index b69d0aa5380712e216996be6f69b0a8b0c28b39d..5ed005e7a82c5c5eb24b056d0f5d71e10ebcf380 100644
--- a/source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -13,14 +13,14 @@
* 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::ExampleClient
+ * @package RobotAPI::ArmarXObjects::ExampleMemoryClient
* @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 "ExampleClient.h"
+#include "ExampleMemoryClient.h"
#include <SimoxUtility/color/cmaps.h>
@@ -30,21 +30,21 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
namespace armarx
{
- ExampleClientPropertyDefinitions::ExampleClientPropertyDefinitions(std::string prefix) :
+ ExampleMemoryClientPropertyDefinitions::ExampleMemoryClientPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
}
- armarx::PropertyDefinitionsPtr ExampleClient::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr ExampleMemoryClient::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ExampleClientPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ExampleMemoryClientPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
@@ -53,18 +53,18 @@ namespace armarx
return defs;
}
- std::string ExampleClient::getDefaultName() const
+ std::string ExampleMemoryClient::getDefaultName() const
{
- return "ExampleClient";
+ return "ExampleMemoryClient";
}
- void ExampleClient::onInitComponent()
+ void ExampleMemoryClient::onInitComponent()
{
}
- void ExampleClient::onConnectComponent()
+ void ExampleMemoryClient::onConnectComponent()
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
@@ -90,26 +90,26 @@ namespace armarx
ARMARX_INFO << "Entity " << entityID << " was updated by " << snapshotIDs.size() << " snapshots.";
});
// Using a member function:
- memoryReader.subscribe(entityID, this, &ExampleClient::example_entityUpdated);
+ memoryReader.subscribe(entityID, this, &ExampleMemoryClient::example_entityUpdated);
- task = new RunningTask<ExampleClient>(this, &ExampleClient::run);
+ task = new RunningTask<ExampleMemoryClient>(this, &ExampleMemoryClient::run);
task->start();
}
- void ExampleClient::onDisconnectComponent()
+ void ExampleMemoryClient::onDisconnectComponent()
{
task->stop();
}
- void ExampleClient::onExitComponent()
+ void ExampleMemoryClient::onExitComponent()
{
}
- void ExampleClient::run()
+ void ExampleMemoryClient::run()
{
ARMARX_IMPORTANT << "Running example.";
@@ -142,7 +142,7 @@ namespace armarx
}
- armem::MemoryID ExampleClient::addProviderSegment()
+ armem::MemoryID ExampleMemoryClient::addProviderSegment()
{
armem::data::AddSegmentInput input;
input.coreSegmentName = "ExampleModality";
@@ -155,7 +155,7 @@ namespace armarx
return armem::MemoryID(result.segmentID);
}
- armem::MemoryID ExampleClient::commitSingleSnapshot(const armem::MemoryID& entityID)
+ armem::MemoryID ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
// Prepare the update with some empty instances.
armem::EntityUpdate update;
@@ -178,7 +178,7 @@ namespace armarx
return updateResult.snapshotID;
}
- void ExampleClient::commitMultipleSnapshots(const armem::MemoryID& entityID, int num)
+ void ExampleMemoryClient::commitMultipleSnapshots(const armem::MemoryID& entityID, int num)
{
// Commit a number of updates with different timestamps and number of instances.
armem::Commit commit;
@@ -202,7 +202,7 @@ namespace armarx
}
- void ExampleClient::queryLatestSnapshot(const armem::MemoryID& entityID)
+ void ExampleMemoryClient::queryLatestSnapshot(const armem::MemoryID& entityID)
{
ARMARX_IMPORTANT
<< "Querying latest snapshot: "
@@ -221,19 +221,19 @@ namespace armarx
if (qResult.success)
{
- armem::Memory& memory = qResult.memory;
+ armem::wm::Memory& memory = qResult.memory;
- ARMARX_CHECK_EQUAL(memory.coreSegments.size(), 1);
- armem::CoreSegment& coreSeg = memory.coreSegments.begin()->second;
+ ARMARX_CHECK_EQUAL(memory.coreSegments().size(), 1);
+ armem::wm::CoreSegment& coreSeg = memory.coreSegments().begin()->second;
- ARMARX_CHECK_EQUAL(coreSeg.providerSegments.size(), 1);
- armem::ProviderSegment& provSeg = coreSeg.providerSegments.begin()->second;
+ ARMARX_CHECK_EQUAL(coreSeg.providerSegments().size(), 1);
+ armem::wm::ProviderSegment& provSeg = coreSeg.providerSegments().begin()->second;
- ARMARX_CHECK_EQUAL(provSeg.entities.size(), 1);
- armem::Entity& entity = provSeg.entities.begin()->second;
+ ARMARX_CHECK_EQUAL(provSeg.entities().size(), 1);
+ armem::wm::Entity& entity = provSeg.entities().begin()->second;
- ARMARX_CHECK_GREATER_EQUAL(entity.history.size(), 1);
- armem::EntitySnapshot& snapshot = entity.history.begin()->second;
+ ARMARX_CHECK_GREATER_EQUAL(entity.history().size(), 1);
+ armem::wm::EntitySnapshot& snapshot = entity.history().begin()->second;
ARMARX_INFO << "Result: "
<< "\n- memory: \t" << memory.name()
@@ -245,7 +245,7 @@ namespace armarx
;
ARMARX_IMPORTANT << "Getting entity via ID";
- const armem::Entity& e = memory.getEntity(entityID);
+ const armem::wm::Entity& e = memory.getEntity(entityID);
ARMARX_INFO << "Result: "
<< "\n- entity: \t" << e.name()
<< "\n- snapshot: \t" << e.getLatestSnapshot().time()
@@ -262,10 +262,10 @@ namespace armarx
}
}
- void ExampleClient::queryExactSnapshot(const armem::MemoryID& snapshotID)
+ void ExampleMemoryClient::queryExactSnapshot(const armem::MemoryID& snapshotID)
{
ARMARX_IMPORTANT
- << "Querying exact smapshot: "
+ << "Querying exact snapshot: "
<< "\n- snapshotID: \t'" << snapshotID << "'"
;
@@ -282,8 +282,8 @@ namespace armarx
if (qResult.success)
{
- armem::Memory memory = std::move(qResult.memory);
- armem::EntitySnapshot& entitySnapshot = memory.getEntity(snapshotID).getLatestSnapshot();
+ armem::wm::Memory memory = std::move(qResult.memory);
+ const armem::wm::EntitySnapshot& entitySnapshot = memory.getEntity(snapshotID).getLatestSnapshot();
ARMARX_INFO << "Result snapshot: "
<< "\n- time: \t" << entitySnapshot.time()
@@ -297,7 +297,7 @@ namespace armarx
}
- void ExampleClient::commitExampleData()
+ void ExampleMemoryClient::commitExampleData()
{
ARMARX_IMPORTANT << "Adding segment " << "ExampleData" << "/" << getName();
@@ -376,7 +376,7 @@ namespace armarx
}
}
- void ExampleClient::queryExampleData()
+ void ExampleMemoryClient::queryExampleData()
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -400,7 +400,7 @@ namespace armarx
- void ExampleClient::example_entityUpdated(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void ExampleMemoryClient::example_entityUpdated(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
{
std::stringstream ss;
ss << "example_entity got updated: " << subscriptionID << "\n";
@@ -414,7 +414,7 @@ namespace armarx
}
- void ExampleClient::createRemoteGuiTab()
+ void ExampleMemoryClient::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -427,7 +427,7 @@ namespace armarx
RemoteGui_createTab(getName(), root, &tab);
}
- void ExampleClient::RemoteGui_update()
+ void ExampleMemoryClient::RemoteGui_update()
{
if (tab.rebuild.exchange(false))
{
diff --git a/source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
similarity index 91%
rename from source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.h
rename to source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 3ee6d82e44f6efa8feb62b504d3d966ac23387fb..e2e975c4a5450fa614cf0020b1e21a09fabbdb4f 100644
--- a/source/RobotAPI/components/armem/client/ExampleClient/ExampleClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -34,7 +34,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
namespace armarx
@@ -43,11 +43,11 @@ namespace armarx
* @class ExampleClientPropertyDefinitions
* @brief Property definitions of `ExampleClient`.
*/
- class ExampleClientPropertyDefinitions :
+ class ExampleMemoryClientPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- ExampleClientPropertyDefinitions(std::string prefix);
+ ExampleMemoryClientPropertyDefinitions(std::string prefix);
};
@@ -62,7 +62,7 @@ namespace armarx
*
* Detailed description of class ExampleClient.
*/
- class ExampleClient :
+ class ExampleMemoryClient :
virtual public armarx::Component,
virtual public armarx::armem::client::ComponentPluginUser,
virtual public LightweightRemoteGuiComponentPluginUser
@@ -114,7 +114,7 @@ namespace armarx
private:
- armarx::RunningTask<ExampleClient>::pointer_type task;
+ armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
@@ -124,7 +124,7 @@ namespace armarx
{
std::atomic_bool rebuild = false;
- std::optional<armem::Memory> queryResult;
+ std::optional<armem::wm::Memory> queryResult;
RemoteGui::Client::GroupBox queryResultGroup;
};
RemoteGuiTab tab;
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 19b547583a3678d9b244e8fa4b13b48730229033..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()
{
- memory.name() = p.memoryName;
+ workingMemory.name() = p.memoryName;
// Usually, the memory server will specify a number of core segments with a specific aron type.
- memory.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();
- memory.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(memoryMutex);
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(memory);
+ 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 6cc16927355f90c56f99ea9552bb356581a492bc..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)
{
@@ -51,9 +43,9 @@ BOOST_AUTO_TEST_CASE(test_ExampleData_type)
BOOST_CHECK_EQUAL(type->childrenSize(), 15);
- armem::Memory memory;
- armem::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
- armem::ProviderSegment& prov = core.addProviderSegment("Provider");
+ armem::wm::Memory memory;
+ armem::wm::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
+ armem::wm::ProviderSegment& prov = core.addProviderSegment("Provider");
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 7cc9356831b2da36ee8facfb55c777722fa47350..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()
{
- memory.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/armem/client/ExampleClient/test/ArMemExampleClientTest.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
similarity index 71%
rename from source/RobotAPI/components/armem/client/ExampleClient/test/ArMemExampleClientTest.cpp
rename to source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
index 67269fecc99271fcc1f88ffb7eed89e07ae2b7a8..40329162efb831da65f1dcd1671dc458e5d6d1b0 100644
--- a/source/RobotAPI/components/armem/client/ExampleClient/test/ArMemExampleClientTest.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::ArMemExampleClient
+ * @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::ArMemExampleClient
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectMemory
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../ArMemExampleClient.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::ArMemExampleClient instance;
+using namespace armarx;
- BOOST_CHECK_EQUAL(true, true);
+
+BOOST_AUTO_TEST_CASE(test_ObjectMemory)
+{
}
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
index fd1987ee72282d4fdf1c99e6229c9b1fdf1b7637..a719b42bf112d6d7a58f06066dee3b96efabae83 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
@@ -71,7 +71,7 @@ namespace armarx
robotStateComponentMemoryBatchSize = std::max((unsigned int) 1, robotStateComponentMemoryBatchSize);
robotStateComponentPollFrequency = std::clamp(robotStateComponentPollFrequency, 1, ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY);
- memory.name() = memoryName;
+ workingMemory.name() = workingMemoryName;
}
@@ -114,7 +114,7 @@ namespace armarx
void RobotSensorMemory::setupRobotUnitSegment()
{
ARMARX_INFO << "Adding core segment " << robotUnitCoreSegmentName;
- memory.addCoreSegments({robotUnitCoreSegmentName});
+ workingMemory.addCoreSegments({robotUnitCoreSegmentName});
ARMARX_INFO << "Adding provider segment " << robotUnitCoreSegmentName << "/" << robotUnitProviderSegmentName;
armem::data::AddSegmentInput input;
@@ -136,7 +136,7 @@ namespace armarx
ARMARX_ERROR << "Could not add segment " << robotUnitCoreSegmentName << "/" << robotUnitProviderSegmentName << ". The error message is: " << result.errorMessage;
}
- robotUnitProviderID.memoryName = memoryName;
+ robotUnitProviderID.memoryName = workingMemoryName;
robotUnitProviderID.coreSegmentName = robotUnitCoreSegmentName;
robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
}
@@ -379,7 +379,7 @@ namespace armarx
void RobotSensorMemory::setupRobotStateComponentSegment()
{
ARMARX_INFO << "Adding core segment " << robotStateComponentCoreSegmentName;
- memory.addCoreSegments({robotStateComponentCoreSegmentName});
+ workingMemory.addCoreSegments({robotStateComponentCoreSegmentName});
ARMARX_INFO << "Adding provider segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName;
armem::data::AddSegmentInput input;
@@ -395,7 +395,7 @@ namespace armarx
ARMARX_ERROR << "Could not add segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName << ". The error message is: " << result.errorMessage;
}
- robotStateComponentProviderID.memoryName = memoryName;
+ robotStateComponentProviderID.memoryName = workingMemoryName;
robotStateComponentProviderID.coreSegmentName = robotStateComponentCoreSegmentName;
robotStateComponentProviderID.providerSegmentName = robotStateComponentProviderSegmentName;
}
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h
index 6b6ea32c9ce4b8faa790f2c5158213e7c516b52c..b16ee1296319b386ee77760a2c81df53019e37c4 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.h
@@ -104,7 +104,7 @@ namespace armarx
void stopRobotStateComponentPoll();
private:
- std::string memoryName = "RobotSensorMemory";
+ std::string workingMemoryName = "RobotStateMemory";
bool addCoreSegmentOnUsage = false;
mutable std::recursive_mutex startStopMutex;
@@ -114,6 +114,7 @@ namespace armarx
std::string robotUnitProviderSegmentName = "RobotUnit"; // get robot name?
armem::data::MemoryID robotUnitProviderID;
+ // TODO: Remove the whole connection to the RSC since this memory will replace it?
std::string robotStateComponentCoreSegmentName = "Localization";
std::string robotStateComponentProviderSegmentName = "RobotStateComponent"; // get robot name?
armem::data::MemoryID robotStateComponentProviderID;
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 14dd8887985350929b4a729a4387a5ba448c68f9..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()
{
- memory.name() = memoryName;
+ workingMemory.name() = memoryName;
}
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
index a972b97b164a83ef25695aaf618e790f250ac09a..819331b5f27f58c036bb867e8a17dda8a0c8b977 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
@@ -15,19 +15,43 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
- <layout class="QHBoxLayout" name="updateWidgetLayout">
+ <layout class="QHBoxLayout" name="topRowLayout">
<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="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>
+ <item>
+ <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 efe81d7170e97515f8bf39e692a16bcd68f1a6fe..83d1d8743a3b6ed0ebfeca5c704c96334b22108a 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
@@ -41,7 +41,9 @@ namespace armarx
widget.setupUi(getWidget());
viewer = std::make_unique<MemoryViewer>(
+
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 d8f47110e34bf45513b29455969abe4e1d84ba33..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
@@ -115,12 +115,16 @@ set(SLICE_FILES
armem/client/MemoryListenerInterface.ice
armem/server.ice
- armem/server/LongTermMemoryInterface.ice
+ armem/server/StoringMemoryInterface.ice
+ armem/server/LoadingMemoryInterface.ice
armem/server/MemoryInterface.ice
armem/server/MemoryPredictorInterface.ice
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/io.ice b/source/RobotAPI/interface/armem/io.ice
index cccea43c0b749441c30bc1834f3c86ce690b7195..f721edfad97923d8615d956bf1d93788ae0e16c3 100644
--- a/source/RobotAPI/interface/armem/io.ice
+++ b/source/RobotAPI/interface/armem/io.ice
@@ -2,6 +2,7 @@
#include <ArmarXCore/interface/core/PackagePath.ice>
#include <RobotAPI/interface/armem/memory.ice>
+#include <RobotAPI/interface/armem/query.ice>
module armarx
{
@@ -9,31 +10,11 @@ module armarx
{
module data
{
- struct LoadInput
- {
- armarx::data::PackagePath path;
- armem::data::MemoryID entityID;
- };
-
- struct LoadResult
- {
- bool success = false;
- long timeStartedMicroSeconds;
- long timeFinishedMicroSeconds;
-
- long numLoadedCoreSegments;
- long numLoadedProviderSegments;
- long numLoadedEntities;
- long numLoadedEntitySnapshots;
-
- string errorMessage;
- };
-
struct StoreInput
{
- armarx::data::PackagePath path;
- armem::data::MemoryID entityID;
+ armem::query::data::Input query;
};
+ sequence<StoreInput> StoreInputSeq;
struct StoreResult
{
@@ -41,11 +22,6 @@ module armarx
long timeStartedMicroSeconds;
long timeFinishedMicroSeconds;
- long numStoredCoreSegments;
- long numStoredLoadedProviderSegments;
- long numStoredLoadedEntities;
- long numStoredLoadedEntitySnapshots;
-
string errorMessage;
};
};
diff --git a/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..bb1e7f221856b4505184fdeb380683c6f8cb917b
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/LoadingMemoryInterface.ice
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/io.ice>
+
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+ interface LoadingMemoryInterface
+ {
+ armem::query::data::Result load(armem::query::data::Input query);
+ };
+ };
+ };
+};
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index 93213eed1d85269c83cfcd2547908e2be61f2d2a..a4c8ba2f7602be3eccf333584cffe7a773486f03 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -1,9 +1,11 @@
#pragma once
+#include <RobotAPI/interface/armem/server/LoadingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/StoringMemoryInterface.ice>
+
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
-#include <RobotAPI/interface/armem/server/LongTermMemoryInterface.ice>
module armarx
{
@@ -11,7 +13,15 @@ module armarx
{
module server
{
- interface MemoryInterface extends ReadingMemoryInterface, WritingMemoryInterface
+ interface LongTermMemoryInterface extends LoadingMemoryInterface, StoringMemoryInterface
+ {
+ };
+
+ interface WorkingMemoryInterface extends ReadingMemoryInterface, WritingMemoryInterface
+ {
+ };
+
+ interface MemoryInterface extends WorkingMemoryInterface, LongTermMemoryInterface
{
};
};
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/armem/server/LongTermMemoryInterface.ice b/source/RobotAPI/interface/armem/server/StoringMemoryInterface.ice
similarity index 70%
rename from source/RobotAPI/interface/armem/server/LongTermMemoryInterface.ice
rename to source/RobotAPI/interface/armem/server/StoringMemoryInterface.ice
index ba64d626c1c156c3820a508625d1c3a3b36ec847..4e369bf259f24eccb36bcd1a2a58373da0fec442 100644
--- a/source/RobotAPI/interface/armem/server/LongTermMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/StoringMemoryInterface.ice
@@ -9,9 +9,8 @@ module armarx
{
module server
{
- interface LongTermMemoryInterface
+ interface StoringMemoryInterface
{
- data::LoadResult load(data::LoadInput input);
data::StoreResult store(data::StoreInput input);
};
};
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index 50e821e1b4d794906f353d30623f904d21266098..d01b0d60aa39d65b39d14fbc1abb8100a9ec0014 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -51,7 +51,7 @@ module armarx
class AronList extends AronType { AronType acceptedType; };
class AronTuple extends AronType { AronTypeList elementTypes; };
class AronPair extends AronType { AronType acceptedType1; AronType acceptedType2; };
- class AronObject extends AronType { AronObject parent; string objectName; AronTypeDict elementTypes;};
+ class AronObject extends AronType { AronObject parent; string objectName; AronTypeDict elementTypes; };
class AronDict extends AronType { AronType acceptedType; };
// Complex Types (serialize to ndarray)
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 84bb291a72f5d9b86ed932b1e1422b5a89252ceb..be30284443c02aa0c4a90225855df5b2d753f6aa 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -3,15 +3,12 @@ set(LIB_NAME armem)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-
set(LIBS
ArmarXCoreInterfaces ArmarXCore
RemoteGui
aron
-)
-
-set(ARON_FILES
- core/aron/MemoryID.xml
+ ${LIBMONGOCXX_LIBRARIES}
+ ${LIBBSONCXX_LIBRARIES}
)
set(LIB_FILES
@@ -20,28 +17,49 @@ set(LIB_FILES
core/SuccessHeader.cpp
core/Time.cpp
core/ice_conversions.cpp
-
- core/CoreSegment.cpp
- core/Entity.cpp
- core/EntityInstance.cpp
- core/EntitySnapshot.cpp
- core/ice_conversions.cpp
- core/Memory.cpp
- core/ProviderSegment.cpp
-
- core/detail/MemoryItem.cpp
- core/detail/MaxHistorySize.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/AronTyped.cpp
+
+ core/base/CoreSegmentBase.cpp
+ core/base/EntityBase.cpp
+ core/base/EntityInstanceBase.cpp
+ core/base/EntitySnapshotBase.cpp
+ core/base/MemoryBase.cpp
+ core/base/ProviderSegmentBase.cpp
+
+ core/workingmemory/ice_conversions.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
+ core/longtermmemory/Entity.cpp
+ core/longtermmemory/EntityInstance.cpp
+ core/longtermmemory/EntitySnapshot.cpp
+ core/longtermmemory/Memory.cpp
+ core/longtermmemory/ProviderSegment.cpp
+
+ core/diskmemory/TypeIO.cpp
+ core/diskmemory/CoreSegment.cpp
+ core/diskmemory/Entity.cpp
+ core/diskmemory/EntityInstance.cpp
+ core/diskmemory/EntitySnapshot.cpp
+ core/diskmemory/Memory.cpp
+ core/diskmemory/ProviderSegment.cpp
core/error/ArMemError.cpp
- core/io/DiskReaderWriter.cpp
- core/io/FileSystemLookupMemory.cpp
- core/io/MemoryFileSystemStorage.cpp
- core/io/DiskWriter/DiskWriter.cpp
- core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.cpp
- core/io/DiskReader/DiskReader.cpp
- core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.cpp
-
client/ComponentPlugin.cpp
client/Reader.cpp
client/ReaderComponentPlugin.cpp
@@ -57,11 +75,23 @@ set(LIB_FILES
server/MemoryRemoteGui.cpp
server/RemoteGuiAronDataVisitor.cpp
- server/query_proc/BaseQueryProcessor.cpp
- server/query_proc/EntityQueryProcessor.cpp
- server/query_proc/ProviderSegmentQueryProcessor.cpp
- server/query_proc/CoreSegmentQueryProcessor.cpp
- server/query_proc/MemoryQueryProcessor.cpp
+ server/query_proc/base/BaseQueryProcessorBase.cpp
+ server/query_proc/base/EntityQueryProcessorBase.cpp
+ server/query_proc/base/ProviderSegmentQueryProcessorBase.cpp
+ server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
+ server/query_proc/base/MemoryQueryProcessorBase.cpp
+
+ server/query_proc/workingmemory/BaseQueryProcessor.cpp
+ server/query_proc/workingmemory/EntityQueryProcessor.cpp
+ server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
+ server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
+ server/query_proc/workingmemory/MemoryQueryProcessor.cpp
+
+ server/query_proc/longtermmemory/BaseQueryProcessor.cpp
+ server/query_proc/longtermmemory/EntityQueryProcessor.cpp
+ server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
+ server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
+ server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
mns/MemoryNameSystem.cpp
mns/ClientPlugin.cpp
@@ -77,33 +107,54 @@ 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
- core/CoreSegment.h
- core/Entity.h
- core/EntityInstance.h
- core/EntitySnapshot.h
+ core/base/detail/MemoryItem.h
+ core/base/detail/MaxHistorySize.h
+ core/base/detail/MemoryContainerBase.h
+ core/base/detail/EntityContainerBase.h
+ core/base/detail/AronTyped.h
+
+ core/base/CoreSegmentBase.h
+ core/base/EntityBase.h
+ core/base/EntityInstanceBase.h
+ core/base/EntitySnapshotBase.h
+ core/base/MemoryBase.h
+ core/base/ProviderSegmentBase.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
+ core/longtermmemory/Entity.h
+ core/longtermmemory/EntityInstance.h
+ core/longtermmemory/EntitySnapshot.h
+ core/longtermmemory/Memory.h
+ core/longtermmemory/ProviderSegment.h
+ core/longtermmemory/mongodb/MongoDBConnectionManager.h
+
+ core/diskmemory/TypeIO.h
+ core/diskmemory/CoreSegment.h
+ core/diskmemory/Entity.h
+ core/diskmemory/EntityInstance.h
+ core/diskmemory/EntitySnapshot.h
+ core/diskmemory/Memory.h
+ core/diskmemory/ProviderSegment.h
+
+ core/ice_conversions_templates.h
core/ice_conversions.h
- core/Memory.h
- core/ProviderSegment.h
-
- core/detail/EntityContainer.h
- core/detail/MaxHistorySize.h
- core/detail/MemoryContainer.h
- core/detail/MemoryItem.h
- core/detail/TypedEntityContainer.h
-
- core/io/FileSystemLookupMemory.h
- core/io/MemoryFileSystemStorage.h
- core/io/DiskReaderWriter.h
- core/io/DiskWriter/DiskWriter.h
- core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.h
- core/io/DiskReader/DiskReader.h
- core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
client.h
client/ComponentPlugin.h
@@ -126,11 +177,23 @@ set(LIB_HEADERS
server/RemoteGuiAronDataVisitor.h
server/query_proc.h
- server/query_proc/BaseQueryProcessor.h
- server/query_proc/EntityQueryProcessor.h
- server/query_proc/ProviderSegmentQueryProcessor.h
- server/query_proc/CoreSegmentQueryProcessor.h
- server/query_proc/MemoryQueryProcessor.h
+ server/query_proc/base/BaseQueryProcessorBase.h
+ server/query_proc/base/EntityQueryProcessorBase.h
+ server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+ server/query_proc/base/CoreSegmentQueryProcessorBase.h
+ server/query_proc/base/MemoryQueryProcessorBase.h
+
+ server/query_proc/workingmemory/BaseQueryProcessor.h
+ server/query_proc/workingmemory/EntityQueryProcessor.h
+ server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
+ server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
+ server/query_proc/workingmemory/MemoryQueryProcessor.h
+
+ server/query_proc/longtermmemory/BaseQueryProcessor.h
+ server/query_proc/longtermmemory/EntityQueryProcessor.h
+ server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
+ server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
+ server/query_proc/longtermmemory/MemoryQueryProcessor.h
mns.h
mns/MemoryNameSystem.h
@@ -148,12 +211,12 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${LIB_NAME}
ARON_FILES
- ${ARON_FILES}
- #ENABLE_DEBUG_INFO
+ aron/MemoryID.xml
)
add_library(RobotAPI::libraries::armem ALIAS "${LIB_NAME}")
+
# add unit tests
add_subdirectory(test)
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/client/Query.cpp b/source/RobotAPI/libraries/armem/client/Query.cpp
index 1c649e3405316704c4df8f652ee37ec893a77bca..1777839f24e6f2522e7f0efb9670f79ff463231b 100644
--- a/source/RobotAPI/libraries/armem/client/Query.cpp
+++ b/source/RobotAPI/libraries/armem/client/Query.cpp
@@ -1,7 +1,6 @@
#include "Query.h"
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
namespace armarx::armem::client
{
@@ -52,13 +51,13 @@ namespace armarx::armem
void client::toIce(armem::query::data::Result& ice, const QueryResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
- toIce(ice.memory, result.memory);
+ armarx::armem::toIce(ice.memory, result.memory);
}
void client::fromIce(const armem::query::data::Result& ice, QueryResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
- fromIce(ice.memory, result.memory);
+ armarx::armem::fromIce(ice.memory, result.memory);
}
}
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index 06379aff71b04c8e5bacfce866a53a494d8ac1b5..b22c880013ad594acad439f3941eaff324763287 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -3,10 +3,10 @@
// RobotAPI
#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/SuccessHeader.h>
#include <RobotAPI/libraries/armem/core/DataMode.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
namespace armarx::armem::client
@@ -31,7 +31,7 @@ namespace armarx::armem::client
*/
struct QueryResult : public detail::SuccessHeader
{
- Memory memory;
+ wm::Memory memory;
static QueryResult fromIce(const armem::query::data::Result& ice);
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 8c336da2fcee343364e479734203a63cfef55625..d9cf1e4f39d48bbc6f4aeb1dd39bc448c3cd6cbf 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -9,7 +9,7 @@
namespace armarx::armem::client
{
- Reader::Reader(server::ReadingMemoryInterfacePrx memory) : memory(memory)
+ Reader::Reader(server::ReadingMemoryInterfacePrx memory) : memoryPrx(memory)
{
}
@@ -33,7 +33,7 @@ namespace armarx::armem::client
armem::query::data::Result result;
try
{
- result = memory->query(input);
+ result = memoryPrx->query(input);
}
catch (const Ice::ConnectionRefusedException& e)
{
@@ -63,7 +63,7 @@ namespace armarx::armem::client
return this->query(input);
}
- QueryResult Reader::getAll(DataMode dataMode)
+ QueryResult Reader::getAll(DataMode dataMode) const
{
using namespace client::query_fns;
@@ -73,7 +73,7 @@ namespace armarx::armem::client
return this->query(qb.buildQueryInput());
}
- QueryResult Reader::getLatestSnapshots(DataMode dataMode)
+ QueryResult Reader::getLatestSnapshots(DataMode dataMode) const
{
using namespace client::query_fns;
@@ -84,7 +84,7 @@ namespace armarx::armem::client
}
void
- Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs)
+ Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
{
for (const auto& [subscription, callback] : callbacks)
{
@@ -105,6 +105,21 @@ namespace armarx::armem::client
}
}
+ data::StoreResult
+ Reader::readAndStore(const data::StoreInput& input) const
+ {
+ server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
+ if (storingMemoryPrx)
+ {
+ return storingMemoryPrx->store(input);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ return {};
+ }
+ }
+
void
Reader::subscribe(const MemoryID& id, callback callback)
@@ -116,7 +131,7 @@ namespace armarx::armem::client
void
Reader::setReadingMemory(server::ReadingMemoryInterfacePrx memory)
{
- this->memory = memory;
+ this->memoryPrx = memory;
}
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 26237f31b832057908823152057bc5d1f360df97..cf0adbb45b087faf16188cb89a45bd22839dda97 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -8,9 +8,11 @@
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
#include "Query.h"
@@ -38,7 +40,6 @@ namespace armarx::armem::client
void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
-
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
@@ -46,9 +47,11 @@ namespace armarx::armem::client
QueryResult query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode = DataMode::WithData) const;
- QueryResult getAll(DataMode dataMode = DataMode::WithData);
- QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData);
+ QueryResult getAll(DataMode dataMode = DataMode::WithData) const;
+ QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
+ //data::StoreResult readAndStore(data::StoreInputSeq& input);
+ data::StoreResult readAndStore(const data::StoreInput& input) const;
void subscribe(const MemoryID& subscriptionID, callback callback);
/**
@@ -67,17 +70,17 @@ namespace armarx::armem::client
subscribe(subscriptionID, cb);
}
/// Function handling updates from the MemoryListener ice topic.
- void updated(const std::vector<MemoryID>& updatedIDs);
+ void updated(const std::vector<MemoryID>& updatedIDs) const;
inline operator bool() const
{
- return bool(memory);
+ return bool(memoryPrx);
}
public:
- server::ReadingMemoryInterfacePrx memory;
+ server::ReadingMemoryInterfacePrx memoryPrx;
private:
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index f4c0754a10c129062f482d581d3d81f5bc0977ce..9c6e8851e4fe3792ad728e90eaa9319a88568e0c 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -2,7 +2,7 @@
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index 13d4e14c87054ce1010c81957563965d70c2b3e6..f2bae712818b566ef51444aafb84af6031d338d0 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,6 +1,6 @@
#include "selectors.h"
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
namespace dq = ::armarx::armem::query::data;
diff --git a/source/RobotAPI/libraries/armem/core.h b/source/RobotAPI/libraries/armem/core.h
index 8e0a1ea2280a128fac3339fbdfdc1da39c8cfdbb..c8ddee31ce054decbc8fdc33edf7959502601da7 100644
--- a/source/RobotAPI/libraries/armem/core.h
+++ b/source/RobotAPI/libraries/armem/core.h
@@ -1,18 +1,18 @@
#pragma once
#include "core/error.h"
-#include "core/ice_conversions.h"
#include "core/Commit.h"
#include "core/DataMode.h"
#include "core/MemoryID.h"
#include "core/Time.h"
-#include "core/Memory.h"
-#include "core/CoreSegment.h"
-#include "core/ProviderSegment.h"
-#include "core/Entity.h"
-#include "core/EntitySnapshot.h"
-#include "core/EntityInstance.h"
+#include "core/workingmemory/Memory.h"
+#include "core/workingmemory/CoreSegment.h"
+#include "core/workingmemory/ProviderSegment.h"
+#include "core/workingmemory/Entity.h"
+#include "core/workingmemory/EntitySnapshot.h"
+#include "core/workingmemory/EntityInstance.h"
+#include "core/workingmemory/ice_conversions.h"
namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 6a25f7a7c0e362dbf25ba9ee4a895724bea50a05..ff3215ab7ea6d01de892eec9f99ec911e6e667b8 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -10,7 +10,6 @@
namespace armarx::armem
{
-
/**
* @brief An update of an entity for a specific point in time.
*/
diff --git a/source/RobotAPI/libraries/armem/core/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/CoreSegment.cpp
deleted file mode 100644
index 61098ee9a268d6bd06748c02878cc70e013e72af..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/CoreSegment.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-#include "CoreSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem
-{
-
- CoreSegment::CoreSegment()
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- CoreSegment(name, MemoryID(), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- CoreSegment(parentID.getMemoryID().withCoreSegmentName(name), aronType)
- {
- }
-
- CoreSegment::CoreSegment(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- Base(id, aronType)
- {
- }
-
- CoreSegment::CoreSegment(const CoreSegment& other) : Base(other)
- {
- *this = other;
- }
-
- CoreSegment& CoreSegment::operator=(const CoreSegment& other)
- {
- other._copySelf(*this);
- other._copyElements(*this, true);
- return *this;
- }
-
- bool CoreSegment::equalsDeep(const CoreSegment& other) const
- {
- if (size() != other.size())
- {
- return false;
- }
- for (const auto& [key, provider] : providerSegments)
- {
- if (not other.hasProviderSegment(key))
- {
- return false;
- }
- if (not provider.equalsDeep(other.getProviderSegment(key)))
- {
- return false;
- }
- }
- return true;
- }
-
- CoreSegment::~CoreSegment()
- {
- }
-
- const std::string& CoreSegment::name() const
- {
- return id().coreSegmentName;
- }
-
- void CoreSegment::_copyElements(CoreSegment& other, bool withData) const
- {
- other.providerSegments.clear();
- for (const auto& [name, providerSegment] : this->providerSegments)
- {
- other.addProviderSegment(providerSegment.copy(withData));
- }
- }
-
-
- bool CoreSegment::hasProviderSegment(const std::string& name) const
- {
- return providerSegments.count(name) > 0;
- }
-
- ProviderSegment& CoreSegment::getProviderSegment(const std::string& name)
- {
- return const_cast<ProviderSegment&>(const_cast<const CoreSegment*>(this)->getProviderSegment(name));
- }
-
- const ProviderSegment& CoreSegment::getProviderSegment(const std::string& name) const
- {
- auto it = this->providerSegments.find(name);
- if (it != providerSegments.end())
- {
- return it->second;
- }
- else
- {
- throw armem::error::MissingEntry("provider segment", name, getLevelName(), this->name());
- }
- }
-
- const Entity& CoreSegment::getEntity(const MemoryID& id) const
- {
- _checkContainerName(id.coreSegmentName, this->name());
- return getProviderSegment(id.providerSegmentName).getEntity(id);
- }
-
-
- ProviderSegment& CoreSegment::addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType)
- {
- aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType();
- return addProviderSegment(ProviderSegment(name, type));
- }
-
- ProviderSegment& CoreSegment::addProviderSegment(const ProviderSegment& providerSegment)
- {
- return addProviderSegment(ProviderSegment(providerSegment));
- }
-
- ProviderSegment& CoreSegment::addProviderSegment(ProviderSegment&& providerSegment)
- {
- if (hasProviderSegment(providerSegment.name()))
- {
- throw armem::error::ContainerEntryAlreadyExists(
- providerSegment.getLevelName(), providerSegment.name(), getLevelName(), this->name());
- }
-
- auto it = providerSegments.emplace(providerSegment.name(), std::move(providerSegment)).first;
- it->second.id().setCoreSegmentID(id());
- it->second.setMaxHistorySize(maxHistorySize);
- return it->second;
- }
-
- void CoreSegment::setMaxHistorySize(long maxSize)
- {
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, seg] : providerSegments)
- {
- seg.setMaxHistorySize(maxSize);
- }
- }
-
- std::string CoreSegment::getLevelName() const
- {
- return "core segment";
- }
-
- CoreSegment::ContainerT& CoreSegment::_underlyingContainer()
- {
- return providerSegments;
- }
-
-
- MemoryID CoreSegment::update(const EntityUpdate& update)
- {
- _checkContainerName(update.entityID.coreSegmentName, this->name());
-
- auto it = this->providerSegments.find(update.entityID.providerSegmentName);
- if (it != providerSegments.end())
- {
- return it->second.update(update);
- }
- else
- {
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName,
- getLevelName(), name());
- }
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/CoreSegment.h b/source/RobotAPI/libraries/armem/core/CoreSegment.h
deleted file mode 100644
index f492c0f1a735bd8c7343bfb503bc66f3add11908..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/CoreSegment.h
+++ /dev/null
@@ -1,93 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-
-#include "ProviderSegment.h"
-#include "detail/TypedEntityContainer.h"
-#include "detail/MaxHistorySize.h"
-
-
-namespace armarx::armem
-{
-
- /**
- * @brief Data of a core segment containing multiple provider segments.
- */
- class CoreSegment :
- public detail::TypedEntityContainer<ProviderSegment, CoreSegment>
- , public detail::MaxHistorySize
- {
- using Base = detail::TypedEntityContainer<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);
-
- ~CoreSegment() override;
-
-
- using Base::name;
- const std::string& name() const override;
-
-
- bool hasProviderSegment(const std::string& name) const;
- ProviderSegment& getProviderSegment(const std::string& name);
- const ProviderSegment& getProviderSegment(const std::string& name) const;
-
-
- using Base::getEntity;
- const Entity& getEntity(const MemoryID& id) const override;
-
-
- using Base::update;
- virtual MemoryID update(const EntityUpdate& update) override;
-
- /**
- * @brief Add an empty provider segment with the given name and optional provider segment type.
- * @param name The segment name.
- * @param providerSegmentType The provider type. If nullptr, the core segment type is used.
- * @return The added provider segment.
- */
- ProviderSegment& addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType = nullptr);
- /// Copy and insert a provider segment.
- ProviderSegment& addProviderSegment(const ProviderSegment& providerSegment);
- /// Move and insert a provider segment.
- ProviderSegment& addProviderSegment(ProviderSegment&& providerSegment);
-
-
- /**
- * @brief Sets the maximum history size of entities in this segment.
- * This affects all current entities as well as new ones.
- * @see Entity::setMaxHistorySize()
- */
- void setMaxHistorySize(long maxSize) override;
-
-
- bool equalsDeep(const CoreSegment& other) const;
-
-
- // MemoryItem interface
- public:
- std::string getLevelName() const override;
-
- // MemoryContainer interface
- protected:
- ContainerT& _underlyingContainer() override;
- void _copyElements(CoreSegment& other, bool withData) const override;
-
-
- public:
-
- /// The provider segments.
- std::map<std::string, ProviderSegment> providerSegments;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/Entity.cpp b/source/RobotAPI/libraries/armem/core/Entity.cpp
deleted file mode 100644
index 876f5a19b63f6667c6e591644eef0ed2c5b71e09..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/Entity.cpp
+++ /dev/null
@@ -1,215 +0,0 @@
-#include "Entity.h"
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem
-{
-
- Entity::Entity()
- {
- }
-
- Entity::Entity(const std::string& name, const MemoryID& parentID) :
- Entity(parentID.withEntityName(name))
- {
- }
-
- Entity::Entity(const MemoryID& id) : Base(id)
- {
- }
-
- Entity::Entity(const Entity& other) : Base(other)
- {
- *this = other;
- }
-
- Entity& Entity::operator=(const Entity& other)
- {
- other._copySelf(*this);
- other._copyElements(*this, true);
- return *this;
- }
-
- bool Entity::equalsDeep(const Entity& other) const
- {
- if (size() != other.size())
- {
- return false;
- }
- for (const auto& [key, snapshot] : history)
- {
- if (not other.hasSnapshot(key))
- {
- return false;
- }
- if (not snapshot.equalsDeep(other.getSnapshot(key)))
- {
- return false;
- }
- }
- return true;
- }
-
-
- void Entity::_copySelf(Entity& other) const
- {
- Base::_copySelf(other);
- other.maxHistorySize = this->maxHistorySize;
- }
-
- void Entity::_copyElements(Entity& other, bool withData) const
- {
- other.history.clear();
- for (const auto& [time, snapshot] : this->history)
- {
- other.addSnapshot(snapshot.copy(withData));
- }
- }
-
-
- bool Entity::hasSnapshot(Time time) const
- {
- return history.count(time) > 0;
- }
-
- bool Entity::hasSnapshotData(Time time) const
- {
- return history.count(time) > 0;
- }
-
- EntitySnapshot& Entity::getSnapshot(Time time)
- {
- return const_cast<EntitySnapshot&>(const_cast<const Entity*>(this)->getSnapshot(time));
- }
-
- const EntitySnapshot& Entity::getSnapshot(Time time) const
- {
- auto it = history.find(time);
- if (it != history.end())
- {
- return it->second;
- }
- else
- {
- throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time),
- getLevelName(), this->id().str());
- }
- }
-
- EntitySnapshot& Entity::getSnapshot(const MemoryID& id)
- {
- return const_cast<EntitySnapshot&>(const_cast<const Entity*>(this)->getSnapshot(id));
- }
-
- const EntitySnapshot& Entity::getSnapshot(const MemoryID& id) const
- {
- checkEntityName(id.entityName);
- return getSnapshot(id.timestamp);
- }
-
- EntitySnapshot& Entity::getLatestSnapshot()
- {
- return const_cast<EntitySnapshot&>(const_cast<const Entity*>(this)->getLatestSnapshot());
- }
-
- const EntitySnapshot& Entity::getLatestSnapshot() const
- {
- return getLatestItem().second;
- }
-
- Time Entity::getLatestTimestamp() const
- {
- return getLatestItem().first;
- }
-
- std::vector<Time> Entity::getTimestamps() const
- {
- return simox::alg::get_keys(history);
- }
-
-
- EntitySnapshot& Entity::addSnapshot(const EntitySnapshot& snapshot)
- {
- return addSnapshot(EntitySnapshot(snapshot));
- }
-
-
- EntitySnapshot& Entity::addSnapshot(EntitySnapshot&& snapshot)
- {
- auto it = history.emplace(snapshot.time(), std::move(snapshot)).first;
- it->second.id().setEntityID(id());
- return it->second;
- }
-
- EntitySnapshot& Entity::addSnapshot(const Time& timestamp)
- {
- return addSnapshot(EntitySnapshot(timestamp));
- }
-
- MemoryID Entity::update(const EntityUpdate& update)
- {
- checkEntityName(update.entityID.entityName);
- id() = update.entityID;
-
- EntitySnapshot* snapshot;
-
- auto it = history.find(update.timeCreated);
- if (it == history.end())
- {
- // Insert into history.
- snapshot = &addSnapshot(update.timeCreated);
- truncateHistoryToSize();
- }
- else
- {
- snapshot = &it->second;
- }
- // Update entry.
- snapshot->update(update);
-
- return snapshot->id();
- }
-
-
- void Entity::setMaxHistorySize(long maxSize)
- {
- this->maxHistorySize = maxSize;
- truncateHistoryToSize();
- }
-
-
- void Entity::truncateHistoryToSize()
- {
- if (maxHistorySize >= 0)
- {
- while (history.size() > size_t(maxHistorySize))
- {
- history.erase(history.begin());
- }
- ARMARX_CHECK_LESS_EQUAL(history.size(), maxHistorySize);
- }
- }
-
- void Entity::checkEntityName(const std::string& name) const
- {
- if (name != this->name())
- {
- throw armem::error::ContainerNameMismatch(name, getLevelName(), this->name());
- }
- }
-
- const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
- {
- if (history.empty())
- {
- throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
- }
- return *history.rbegin();
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/Entity.h b/source/RobotAPI/libraries/armem/core/Entity.h
deleted file mode 100644
index b81ffe76199f7a978d822a4d91cc421b48537682..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/Entity.h
+++ /dev/null
@@ -1,192 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-
-#include "../core/Time.h"
-#include "../core/MemoryID.h"
-
-#include "EntitySnapshot.h"
-#include "detail/MemoryContainer.h"
-
-
-namespace armarx::armem
-{
-
- /**
- * @brief An entity over a period of time.
- *
- * An entity should be a physical thing or abstract concept existing
- * (and potentially evolving) over some time.
- *
- * Examples are:
- * - objects (the green box)
- * - agents (robot, human)
- * - locations (frige, sink)
- * - grasp affordances (general, or for a specific object)
- * - images
- * - point clouds
- * - other sensory values
- *
- * At each point in time (`EntitySnapshot`), the entity can have a
- * (potentially variable) number of instances (`EntityInstance`),
- * each containing a single `AronData` object of a specific `AronType`.
- */
- class Entity :
- public detail::MemoryContainer<std::map<Time, EntitySnapshot>, Entity>
- {
- using Base = detail::MemoryContainer<std::map<Time, 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);
-
-
- inline const std::string& name() const
- {
- return id().entityName;
- }
- inline std::string& name()
- {
- return id().entityName;
- }
-
-
- /**
- * @brief Indicates whether a history entry for the given time exists.
- */
- bool hasSnapshot(Time time) const;
- /**
- * @brief Indicates whether a history entry for the given time exists
- * and its data is not null.
- */
- bool hasSnapshotData(Time time) const;
-
-
- /**
- * @brief Get the latest timestamp.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- */
- Time getLatestTimestamp() const;
- /**
- * @brief Get all timestamps in the history.
- */
- std::vector<Time> getTimestamps() const;
-
-
- /**
- * @brief Get a snapshot.
- * @param time The time.
- * @return The snapshot, if it exists.
- *
- * @throws `armem::error::MissingEntry` If there is no such entry.
- * @throws `armem::error::MissingData` If the entry has no data.
- */
- EntitySnapshot& getSnapshot(Time time);
- const EntitySnapshot& getSnapshot(Time time) const;
-
- EntitySnapshot& getSnapshot(const MemoryID& id);
- const EntitySnapshot& getSnapshot(const MemoryID& id) const;
-
- /**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingData` If the latest snapshot has no data.
- */
- EntitySnapshot& getLatestSnapshot();
- const EntitySnapshot& getLatestSnapshot() const;
-
-
- /**
- * @brief Add the given update to this entity's history.
- * @param update The update.
- * @return The snapshot ID of the update.
- */
- MemoryID update(const EntityUpdate& update);
-
- /**
- * @brief Add a single snapshot with data.
- * @param snapshot The snapshot.
- * @return The stored snapshot.
- */
- EntitySnapshot& addSnapshot(const EntitySnapshot& snapshot);
- EntitySnapshot& addSnapshot(EntitySnapshot&& snapshot);
- EntitySnapshot& addSnapshot(const Time& timestamp);
-
-
- /**
- * @brief Sets the maximum history size.
- *
- * The current history is truncated if necessary.
- */
- void setMaxHistorySize(long maxSize);
-
-
- bool equalsDeep(const Entity& other) const;
-
-
- protected:
-
- void _copySelf(Entity& other) const override;
- void _copyElements(Entity& other, bool withData) const override;
-
-
-
- private:
-
- /// If maximum size is set, ensure `history`'s is not higher.
- void truncateHistoryToSize();
- void checkEntityName(const std::string& name) const;
- ContainerT::iterator _addSnapshot();
-
- /**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- */
- const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const;
-
-
- public:
-
- std::map<Time, EntitySnapshot> history;
-
- /**
- * @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
-
- // MemoryItem interface
- public:
- std::string getKeyString() const override
- {
- return id().entityName;
- }
- std::string getLevelName() const override
- {
- return "entity";
- }
-
- // MemoryContainer interface
- protected:
- ContainerT& _underlyingContainer() override
- {
- return history;
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/EntitySnapshot.cpp
deleted file mode 100644
index 7a53400f9c75849c805e443e61afafc83f43e7c0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/EntitySnapshot.cpp
+++ /dev/null
@@ -1,165 +0,0 @@
-#include "EntitySnapshot.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem
-{
-
-
- EntitySnapshot::EntitySnapshot()
- {
- }
-
- EntitySnapshot::EntitySnapshot(Time time, const MemoryID& parentID) :
- EntitySnapshot(parentID.withTimestamp(time))
- {
- }
-
- EntitySnapshot::EntitySnapshot(const MemoryID& id) : Base(id)
- {
- }
-
- EntitySnapshot::EntitySnapshot(const EntitySnapshot& other) : Base(other)
- {
- *this = other;
- }
-
- EntitySnapshot& EntitySnapshot::operator=(const EntitySnapshot& other)
- {
- other._copySelf(*this);
- other._copyElements(*this, true);
- return *this;
- }
-
- bool EntitySnapshot::equalsDeep(const EntitySnapshot& other) const
- {
- if (size() != other.size())
- {
- return false;
- }
- int i = 0;
- for (const auto& instance : instances)
- {
- if (not instance.equalsDeep(other.getInstance(i)))
- {
- return false;
- }
- i++;
- }
- return true;
- }
-
-
- void EntitySnapshot::_copyElements(EntitySnapshot& other, bool withData) const
- {
- other.instances.clear();
- other.instances.reserve(this->instances.size());
- for (const auto& data : this->instances)
- {
- other.instances.emplace_back(data.copy(withData));
- }
- }
-
-
- void EntitySnapshot::update(const EntityUpdate& update,
- std::optional<MemoryID> parentID)
- {
- if (parentID)
- {
- id() = *parentID;
- }
- time() = update.timeCreated;
-
- instances.clear();
- for (int i = 0; i < int(update.instancesData.size()); ++i)
- {
- EntityInstance& data = instances.emplace_back(i, id());
- data.update(update, i);
- }
- }
-
- bool EntitySnapshot::hasInstance(int index) const
- {
- size_t si = size_t(index);
- return index >= 0 && si < instances.size();
- }
-
-
- EntityInstance& EntitySnapshot::getInstance(int index)
- {
- return const_cast<EntityInstance&>(const_cast<const EntitySnapshot*>(this)->getInstance(index));
- }
-
- const EntityInstance& EntitySnapshot::getInstance(int index) const
- {
- if (hasInstance(index))
- {
- size_t si = size_t(index);
- return instances[si];
- }
- else
- {
- throw armem::error::MissingEntry(EntityInstance().getLevelName(), std::to_string(index),
- getLevelName(), toDateTimeMilliSeconds(time()));
- }
- }
-
-
- EntityInstance& EntitySnapshot::getInstance(const MemoryID& id)
- {
- return const_cast<EntityInstance&>(const_cast<const EntitySnapshot*>(this)->getInstance(id));
- }
-
- const EntityInstance& EntitySnapshot::getInstance(const MemoryID& id) const
- {
- if (!id.hasInstanceIndex())
- {
- throw armem::error::InvalidMemoryID(id, "ID has no instance index.");
- }
- return getInstance(id.instanceIndex);
- }
-
- EntityInstance& EntitySnapshot::addInstance(const EntityInstance& instance)
- {
- return addInstance(EntityInstance(instance));
- }
-
-
- EntityInstance& EntitySnapshot::addInstance(EntityInstance&& instance)
- {
- if (instance.index() > 0 && (size_t) instance.index() < instances.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() > instances.size())
- {
- throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index is too big.");
- }
-
- int new_index = instances.size();
- auto& it = instances.emplace_back(std::move(instance));
- it.index() = new_index;
- return it;
- }
-
- std::string EntitySnapshot::getKeyString() const
- {
- return toDateTimeMilliSeconds(this->time());
- }
-
- std::string EntitySnapshot::getLevelName() const
- {
- return "entity snapshot";
- }
-
- EntitySnapshot::ContainerT& EntitySnapshot::_underlyingContainer()
- {
- return instances;
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/EntitySnapshot.h
deleted file mode 100644
index da1301ccfaf08701358c7c912dfd617ee7e7a67b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/EntitySnapshot.h
+++ /dev/null
@@ -1,103 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <vector>
-
-#include "../core/MemoryID.h"
-#include "../core/Time.h"
-
-#include "EntityInstance.h"
-#include "detail/MemoryItem.h"
-#include "detail/MemoryContainer.h"
-
-
-namespace armarx::armem
-{
-
- /**
- * @brief Data of an entity at one point in time.
- */
- class EntitySnapshot :
- public detail::MemoryContainer<std::vector<EntityInstance>, EntitySnapshot>
- {
- using Base = detail::MemoryContainer<std::vector<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);
- /// Copy the instances from `other` to this.
- EntitySnapshot& operator=(const EntitySnapshot& other);
-
-
- inline Time& time()
- {
- return id().timestamp;
- }
- inline const Time& time() const
- {
- return id().timestamp;
- }
-
-
- void update(const EntityUpdate& update,
- std::optional<MemoryID> parentID = std::nullopt);
-
-
- bool hasInstance(int index) const;
-
- /**
- * @brief Get the given instance.
- * @param index The instance's index.
- * @return The instance.
- * @throw `armem::error::MissingEntry` If the given index is invalid.
- */
- EntityInstance& getInstance(int index);
- const EntityInstance& getInstance(int index) const;
- /**
- * @brief Get the given instance.
- * @param index The instance's index.
- * @return The instance.
- * @throw `armem::error::MissingEntry` If the given index is invalid.
- * @throw `armem::error::InvalidMemoryID` If memory ID does not have an instance index.
- */
- EntityInstance& getInstance(const MemoryID& id);
- const EntityInstance& getInstance(const MemoryID& id) const;
-
-
- /**
- * @brief Add a single instance with data.
- * @param instance The instance.
- * @return The stored instance.
- * @throw `armem::error::InvalidArgument` If the given index is invalid. Must be equal to instances.size() or -1 (meaning push_back)
- */
- EntityInstance& addInstance(const EntityInstance& instance);
- EntityInstance& addInstance(EntityInstance&& instance);
-
-
- bool equalsDeep(const EntitySnapshot& other) const;
-
-
- // MemoryItem interface
- public:
- std::string getKeyString() const override;
- std::string getLevelName() const override;
-
-
- // MemoryContainer interface
- protected:
- ContainerT& _underlyingContainer() override;
- void _copyElements(EntitySnapshot& other, bool withData) const override;
-
-
- public:
-
- std::vector<EntityInstance> instances;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/Memory.cpp b/source/RobotAPI/libraries/armem/core/Memory.cpp
deleted file mode 100644
index 3d218f0b098670cbc6e704e9e500ffecd752c2ac..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/Memory.cpp
+++ /dev/null
@@ -1,173 +0,0 @@
-#include "Memory.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem
-{
-
- Memory::Memory()
- {
- }
-
- Memory::Memory(const std::string& name) : Memory(MemoryID().withMemoryName(name))
- {
- }
-
- Memory::Memory(const MemoryID& id) : EntityContainer(id)
- {
- }
-
- Memory::Memory(const Memory& other) : EntityContainer(other)
- {
- *this = other;
- }
-
- Memory& Memory::operator=(const Memory& other)
- {
- other._copySelf(*this);
- other._copyElements(*this, true);
- return *this;
- }
-
- Memory::~Memory()
- {
- }
-
-
- const std::string& Memory::name() const
- {
- return id().memoryName;
- }
-
- void Memory::_copyElements(Memory& other, bool withData) const
- {
- other.coreSegments.clear();
- for (const auto& [name, coreSegment] : this->coreSegments)
- {
- other.coreSegments.emplace(name, coreSegment.copy(withData));
- }
- }
-
-
- bool Memory::hasCoreSegment(const std::string& name) const
- {
- return coreSegments.count(name) > 0;
- }
-
- CoreSegment& Memory::getCoreSegment(const std::string& name)
- {
- return const_cast<CoreSegment&>(const_cast<const Memory*>(this)->getCoreSegment(name));
- }
- const CoreSegment& Memory::getCoreSegment(const std::string& name) const
- {
- auto it = this->coreSegments.find(name);
- if (it != coreSegments.end())
- {
- return it->second;
- }
- else
- {
- throw armem::error::MissingEntry(CoreSegment().getLevelName(), name, getLevelName(), this->name());
- }
- }
-
-
- const Entity& Memory::getEntity(const MemoryID& id) const
- {
- _checkContainerName(id.memoryName, this->name());
- return getCoreSegment(id.coreSegmentName).getEntity(id);
- }
-
-
- CoreSegment& Memory::addCoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType)
- {
- return addCoreSegment(CoreSegment(name, coreSegmentType));
- }
-
- CoreSegment& Memory::addCoreSegment(const CoreSegment& coreSegment)
- {
- return addCoreSegment(CoreSegment(coreSegment));
- }
-
- CoreSegment& Memory::addCoreSegment(CoreSegment&& coreSegment)
- {
- if (coreSegments.count(coreSegment.name()) > 0)
- {
- throw armem::error::ContainerEntryAlreadyExists(coreSegment.getLevelName(), coreSegment.name(),
- this->getLevelName(), this->name());
- }
- auto it = coreSegments.emplace(coreSegment.name(), std::move(coreSegment)).first;
- it->second.id().setMemoryID(id());
- return it->second;
- }
-
- std::vector<CoreSegment*> Memory::addCoreSegments(const std::vector<std::string>& names)
- {
- std::vector<CoreSegment*> segments;
- for (const auto& name : names)
- {
- try
- {
- segments.push_back(&addCoreSegment(name));
- }
- catch (const armem::error::ContainerEntryAlreadyExists& e)
- {
- ARMARX_INFO << e.what() << "\nIgnoring multiple addition.";
- }
- }
- return segments;
- }
-
-
- MemoryID Memory::update(const EntityUpdate& update)
- {
- _checkContainerName(update.entityID.memoryName, this->name());
-
- auto it = this->coreSegments.find(update.entityID.coreSegmentName);
- if (it != coreSegments.end())
- {
- return it->second.update(update);
- }
- else
- {
- throw armem::error::MissingEntry(CoreSegment().getLevelName(), update.entityID.coreSegmentName,
- getLevelName(), this->name());
- }
- }
-
- std::string Memory::getLevelName() const
- {
- return "memory";
- }
-
- Memory::ContainerT& Memory::_underlyingContainer()
- {
- return coreSegments;
- }
-
-
- bool Memory::equalsDeep(const Memory& other) const
- {
- if (size() != other.size())
- {
- return false;
- }
- for (const auto& [key, core] : coreSegments)
- {
- if (not other.hasCoreSegment(key))
- {
- return false;
- }
- if (not core.equalsDeep(other.getCoreSegment(key)))
- {
- return false;
- }
- }
- return true;
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/Memory.h b/source/RobotAPI/libraries/armem/core/Memory.h
deleted file mode 100644
index 2059d0c0dcdc13e683e20fd9c4646bb499b6d1ce..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/Memory.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-
-#include "CoreSegment.h"
-#include "detail/EntityContainer.h"
-
-namespace armarx::armem
-{
-
- /**
- * @brief Data of a memory consisting of multiple core segments.
- */
- class Memory : public detail::EntityContainer<CoreSegment, Memory>
- {
- using Base = detail::EntityContainer<CoreSegment, Memory>;
-
- public:
-
- Memory();
- Memory(const std::string& name);
- Memory(const MemoryID& id);
-
- Memory(const Memory& other);
- Memory& operator=(const Memory& other);
-
- ~Memory() override;
-
-
- using Base::name;
- const std::string& name() const override;
-
-
- bool hasCoreSegment(const std::string& name) const;
-
- CoreSegment& getCoreSegment(const std::string& name);
- const CoreSegment& getCoreSegment(const std::string& name) const;
-
-
- using Base::getEntity;
- const Entity& getEntity(const MemoryID& id) const override;
-
- /**
- * @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.
- */
- CoreSegment& addCoreSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr);
- /// Copy and insert a core segment.
- CoreSegment& addCoreSegment(const CoreSegment& coreSegment);
- /// Move and insert a core segment.
- CoreSegment& addCoreSegment(CoreSegment&& coreSegment);
-
- /**
- * @brief Add multiple core segments.
- * @param The core segment names.
- * @return The core segments. The contained pointers are never null.
- */
- std::vector<CoreSegment*> addCoreSegments(const std::vector<std::string>& names);
-
- virtual MemoryID update(const EntityUpdate& update) override;
- using Base::update;
-
-
- bool equalsDeep(const Memory& other) const;
-
-
- // MemoryItem interface
- public:
- std::string getLevelName() const override;
-
- // MemoryContainer interface
- protected:
- ContainerT& _underlyingContainer() override;
- void _copyElements(Memory& other, bool withData) const override;
-
-
- public:
- std::map<std::string, CoreSegment> coreSegments;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp
deleted file mode 100644
index 879932739ff86a8a9b091d561053f0f76ca08be0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-#include "ProviderSegment.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "error.h"
-
-
-namespace armarx::armem
-{
-
- ProviderSegment::ProviderSegment()
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType) :
- ProviderSegment(name, MemoryID(), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType) :
- ProviderSegment(parentID.withProviderSegmentName(name), aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType) :
- Base(id, aronType)
- {
- }
-
- ProviderSegment::ProviderSegment(const ProviderSegment& other) : Base(other)
- {
- *this = other;
- }
-
- ProviderSegment& ProviderSegment::operator=(const ProviderSegment& other)
- {
- other._copySelf(*this);
- other._copyElements(*this, true);
- return *this;
- }
-
- bool ProviderSegment::equalsDeep(const ProviderSegment& other) const
- {
- if (size() != other.size())
- {
- return false;
- }
- for (const auto& [key, value] : entities)
- {
- if (not other.hasEntity(key))
- {
- return false;
- }
-
- if (not value.equalsDeep(other.getEntity(key)))
- {
- return false;
- }
- }
- return true;
- }
-
- ProviderSegment::~ProviderSegment()
- {
- }
-
- const std::string& ProviderSegment::name() const
- {
- return id().providerSegmentName;
- }
-
-
- void ProviderSegment::_copySelf(ProviderSegment& other) const
- {
- Base::_copySelf(other);
- other.maxHistorySize = this->maxHistorySize;
- }
-
- void ProviderSegment::_copyElements(ProviderSegment& other, bool withData) const
- {
- other.entities.clear();
- for (const auto& [name, entity] : this->entities)
- {
- other.addEntity(entity.copy(withData));
- }
- }
-
-
- bool ProviderSegment::hasEntity(const std::string& name) const
- {
- return entities.count(name) > 0;
- }
-
- Entity& ProviderSegment::getEntity(const std::string& name)
- {
- return const_cast<Entity&>(const_cast<const ProviderSegment*>(this)->getEntity(name));
- }
-
- const Entity& ProviderSegment::getEntity(const std::string& name) const
- {
- auto it = this->entities.find(name);
- if (it != entities.end())
- {
- return it->second;
- }
- else
- {
- throw error::MissingEntry("entity", name, getLevelName(), this->name());
- }
- }
-
- const Entity& ProviderSegment::getEntity(const MemoryID& id) const
- {
- _checkContainerName(id.providerSegmentName, this->name());
- return getEntity(id.entityName);
- }
-
- MemoryID ProviderSegment::update(const EntityUpdate& update)
- {
- _checkContainerName(update.entityID.providerSegmentName, this->name());
-
- Entity* entity;
- auto it = this->entities.find(update.entityID.providerSegmentName);
- if (it == entities.end())
- {
- // Add entity entry.
- entity = &addEntity(update.entityID.entityName);
- entity->setMaxHistorySize(maxHistorySize);
- }
- else
- {
- entity = &it->second;
- }
- // Update entity.
- return entity->update(update);
- }
-
- Entity& ProviderSegment::addEntity(const std::string& name)
- {
- return addEntity(Entity(name));
- }
-
- Entity& ProviderSegment::addEntity(const Entity& entity)
- {
- return addEntity(Entity(entity));
- }
-
- Entity& ProviderSegment::addEntity(Entity&& entity)
- {
- auto it = entities.emplace(entity.name(), std::move(entity)).first;
- it->second.id().setProviderSegmentID(id());
- return it->second;
- }
-
- void ProviderSegment::setMaxHistorySize(long maxSize)
- {
- MaxHistorySize::setMaxHistorySize(maxSize);
- for (auto& [name, entity] : entities)
- {
- entity.setMaxHistorySize(maxSize);
- }
- }
-
- std::string ProviderSegment::getLevelName() const
- {
- return "provider segment";
- }
-
- ProviderSegment::ContainerT& ProviderSegment::_underlyingContainer()
- {
- return entities;
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/ProviderSegment.h
deleted file mode 100644
index cbb2080de0af157cb45f9d938edcde62315f7483..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/ProviderSegment.h
+++ /dev/null
@@ -1,97 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-
-#include "Entity.h"
-#include "detail/TypedEntityContainer.h"
-#include "detail/MaxHistorySize.h"
-
-
-namespace armarx::armem
-{
-
- /**
- * @brief Data of a provider segment containing multiple entities.
- */
- class ProviderSegment :
- public detail::TypedEntityContainer<Entity, ProviderSegment>
- , public detail::MaxHistorySize
-
- {
- using Base = detail::TypedEntityContainer<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);
-
- ~ProviderSegment() override;
-
-
- using Base::name;
- const std::string& name() const override;
-
-
- bool hasEntity(const std::string& name) const;
-
- using Base::getEntity;
- const Entity& getEntity(const MemoryID& id) const override;
-
- Entity& getEntity(const std::string& name);
- const Entity& getEntity(const std::string& name) const;
-
-
-
- using Base::update;
- /**
- * @brief Updates an entity's history.
- *
- * Missing entity entries are added before updating.
- */
- virtual MemoryID update(const EntityUpdate& update) override;
-
- /// Add an empty entity with the given name.
- Entity& addEntity(const std::string& name);
- /// Copy and insert an entity.
- Entity& addEntity(const Entity& entity);
- /// Move and insert an entity.
- Entity& addEntity(Entity&& entity);
-
-
- /**
- * @brief Sets the maximum history size of entities in this segment.
- * This affects all current entities as well as new ones.
- * @see Entity::setMaxHistorySize()
- */
- void setMaxHistorySize(long maxSize) override;
-
-
- bool equalsDeep(const ProviderSegment& other) const;
-
-
- // MemoryItem interface
- public:
- std::string getLevelName() const override;
-
- // MemoryContainer interface
- protected:
- ContainerT& _underlyingContainer() override;
- void _copySelf(ProviderSegment& other) const override;
- void _copyElements(ProviderSegment& other, bool withData) const override;
-
-
- public:
-
- /// The entities.
- std::map<std::string, Entity> entities;
-
- };
-
-}
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.cpp b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2958547f4f410b7027de8d263a77d90ba4621bf1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.cpp
@@ -0,0 +1 @@
+#include "CoreSegmentBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ea66c74b5216c9e4a92c4e719e01193e39b5ae1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -0,0 +1,242 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include "ProviderSegmentBase.h"
+#include "detail/AronTyped.h"
+#include "detail/EntityContainerBase.h"
+#include "detail/MaxHistorySize.h"
+
+
+namespace armarx::armem::base
+{
+
+ /**
+ * @brief Data of a core segment containing multiple provider segments.
+ */
+ template <class _ProviderSegmentT, class _Derived>
+ class CoreSegmentBase :
+ public detail::EntityContainerBase<_ProviderSegmentT, typename _ProviderSegmentT::EntityT, _Derived>,
+ public detail::MaxHistorySize,
+ public detail::AronTyped
+ {
+ 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;
+
+
+ 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)
+ {
+ }
+
+ 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 this->id().coreSegmentName;
+ }
+ inline std::string& name()
+ {
+ return const_cast<std::string&>(const_cast<const CoreSegmentBase*>(this)->name());
+ }
+
+
+ inline const std::map<std::string, ProviderSegmentT>& providerSegments() const
+ {
+ return this->_container;
+ }
+ inline std::map<std::string, ProviderSegmentT>& providerSegments()
+ {
+ return const_cast<std::map<std::string, ProviderSegmentT>&>(const_cast<const CoreSegmentBase*>(this)->providerSegments());
+ }
+
+
+ bool hasProviderSegment(const std::string& name) const
+ {
+ return this->_container.count(name) > 0;
+ }
+
+ ProviderSegmentT& getProviderSegment(const std::string& name)
+ {
+ return const_cast<ProviderSegmentT&>(const_cast<const CoreSegmentBase*>(this)->getProviderSegment(name));
+ }
+
+ const ProviderSegmentT& getProviderSegment(const std::string& name) const
+ {
+ auto it = this->_container.find(name);
+ if (it != this->_container.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ throw armem::error::MissingEntry("provider segment", name, getLevelName(), this->getKeyString());
+ }
+ }
+
+ using Base::getEntity;
+ const EntityT& getEntity(const MemoryID& id) const override
+ {
+ 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
+ {
+ this->_checkContainerName(update.entityID.coreSegmentName, this->name());
+
+ auto it = this->_container.find(update.entityID.providerSegmentName);
+ if (it != this->_container.end())
+ {
+ return it->second.update(update);
+ }
+ else
+ {
+ throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ }
+ }
+
+ /**
+ * @brief Add an empty provider segment with the given name and optional provider segment type.
+ * @param name The segment name.
+ * @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)
+ {
+ aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType();
+ return addProviderSegment(ProviderSegmentT(name, type));
+ }
+
+ /// Copy and insert a provider segment.
+ ProviderSegmentT& addProviderSegment(const ProviderSegmentT& providerSegment)
+ {
+ return addProviderSegment(ProviderSegment(providerSegment));
+ }
+
+ /// Move and insert a provider segment.
+ ProviderSegmentT& addProviderSegment(ProviderSegmentT&& providerSegment)
+ {
+ if (hasProviderSegment(providerSegment.name()))
+ {
+ throw armem::error::ContainerEntryAlreadyExists(
+ providerSegment.getLevelName(), providerSegment.name(), getLevelName(), this->getKeyString());
+ }
+
+ auto it = this->_container.emplace(providerSegment.name(), std::move(providerSegment)).first;
+ it->second.id().setCoreSegmentID(this->id());
+ it->second.setMaxHistorySize(_maxHistorySize);
+ return it->second;
+ }
+
+
+ /**
+ * @brief Sets the maximum history size of entities in this segment.
+ * This affects all current entities as well as new ones.
+ * @see Entity::setMaxHistorySize()
+ */
+ void setMaxHistorySize(long maxSize) override
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ for (auto& [name, seg] : this->_container)
+ {
+ seg.setMaxHistorySize(maxSize);
+ }
+ }
+
+ virtual bool equalsDeep(const CoreSegmentBase& other) const
+ {
+ //std::cout << "CoreSegment::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ for (const auto& [key, provider] : this->_container)
+ {
+ if (not other.hasProviderSegment(key))
+ {
+ return false;
+ }
+ if (not provider.equalsDeep(other.getProviderSegment(key)))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ std::string getLevelName() const override
+ {
+ return "core segment";
+ }
+
+ std::string getKeyString() const override
+ {
+ return this->name();
+ }
+
+
+ protected:
+
+ virtual void _copySelf(DerivedT& other) const override
+ {
+ Base::_copySelf(other);
+ other.aronType() = _aronType;
+ }
+ virtual void _copySelfEmpty(DerivedT& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.aronType() = _aronType;
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d2c67b63d566f7170bd9b31cfaf18785b85f87b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
@@ -0,0 +1 @@
+#include "EntityBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..39dcc12c92902bad4c7a2e0c9cb7385de8e0a40c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -0,0 +1,311 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#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"
+
+
+namespace armarx::armem::base
+{
+
+ /**
+ * @brief An entity over a period of time.
+ *
+ * An entity should be a physical thing or abstract concept existing
+ * (and potentially evolving) over some time.
+ *
+ * Examples are:
+ * - objects (the green box)
+ * - agents (robot, human)
+ * - locations (frige, sink)
+ * - grasp affordances (general, or for a specific object)
+ * - images
+ * - point clouds
+ * - other sensory values
+ *
+ * At each point in time (`EntitySnapshot`), the entity can have a
+ * (potentially variable) number of instances (`EntityInstance`),
+ * each containing a single `AronData` object of a specific `AronType`.
+ */
+ template <class _EntitySnapshotT, class _Derived>
+ class EntityBase :
+ 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;
+
+
+ public:
+
+ EntityBase()
+ {
+ }
+ EntityBase(const std::string& name, const MemoryID& parentID = {}) :
+ EntityBase(parentID.withEntityName(name))
+ {
+ }
+ 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;
+
+
+ virtual bool equalsDeep(const EntityBase& other) const
+ {
+ //std::cout << "Entity::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ for (const auto& [key, snapshot] : this->_container)
+ {
+ if (not other.hasSnapshot(key))
+ {
+ return false;
+ }
+ if (not snapshot.equalsDeep(other.getSnapshot(key)))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+
+ inline const std::string& name() const
+ {
+ return this->id().entityName;
+ }
+ inline std::string& name()
+ {
+ return const_cast<std::string&>(const_cast<const EntityBase*>(this)->name());
+ }
+
+
+ inline const std::map<Time, EntitySnapshotT>& history() const
+ {
+ return this->_container;
+ }
+ inline std::map<Time, EntitySnapshotT>& history()
+ {
+ return const_cast<std::map<Time, EntitySnapshotT>&>(const_cast<const EntityBase*>(this)->history());
+ }
+
+
+ /**
+ * @brief Indicates whether a history entry for the given time exists.
+ */
+ bool hasSnapshot(Time time) const
+ {
+ return this->_container.count(time) > 0;
+ }
+
+ /**
+ * @brief Get the latest timestamp.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ */
+ Time getLatestTimestamp() const
+ {
+ return getLatestItem().first;
+ }
+
+ /**
+ * @brief Get all timestamps in the history.
+ */
+ std::vector<Time> getTimestamps() const
+ {
+ return simox::alg::get_keys(this->_container);
+ }
+
+
+ /**
+ * @brief Get a snapshot.
+ * @param time The time.
+ * @return The snapshot, if it exists.
+ *
+ * @throws `armem::error::MissingEntry` If there is no such entry.
+ * @throws `armem::error::MissingData` If the entry has no data.
+ */
+ EntitySnapshotT& getSnapshot(Time time)
+ {
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
+ }
+
+ const EntitySnapshotT& getSnapshot(Time time) const
+ {
+ 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());
+ }
+ }
+
+ EntitySnapshotT& getSnapshot(const MemoryID& id)
+ {
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(id));
+ }
+
+ const EntitySnapshotT& getSnapshot(const MemoryID& id) const
+ {
+ checkEntityName(id.entityName);
+ return getSnapshot(id.timestamp);
+ }
+
+ /**
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingData` If the latest snapshot has no data.
+ */
+ EntitySnapshotT& getLatestSnapshot()
+ {
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
+ }
+
+ const EntitySnapshotT& getLatestSnapshot() const
+ {
+ return getLatestItem().second;
+ }
+
+
+ /**
+ * @brief Add the given update to this entity's history.
+ * @param update The update.
+ * @return The snapshot ID of the update.
+ */
+ virtual MemoryID update(const EntityUpdate& update)
+ {
+ checkEntityName(update.entityID.entityName);
+ this->id() = update.entityID;
+
+ EntitySnapshotT* snapshot;
+
+ auto it = this->_container.find(update.timeCreated);
+ if (it == this->_container.end())
+ {
+ // Insert into history.
+ snapshot = &addSnapshot(update.timeCreated);
+ truncateHistoryToSize();
+ }
+ else
+ {
+ snapshot = &it->second;
+ }
+ // Update entry.
+ snapshot->update(update);
+
+ return snapshot->id();
+ }
+
+ /**
+ * @brief Add a single snapshot with data.
+ * @param snapshot The snapshot.
+ * @return The stored snapshot.
+ */
+ EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
+ {
+ return addSnapshot(EntitySnapshotT(snapshot));
+ }
+
+ EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
+ {
+ 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)
+ {
+ return addSnapshot(EntitySnapshotT(timestamp));
+ }
+
+
+ /**
+ * @brief Sets the maximum history size.
+ *
+ * The current history is truncated if necessary.
+ */
+ void setMaxHistorySize(long maxSize) override
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ truncateHistoryToSize();
+ }
+
+ std::string getKeyString() const override
+ {
+ return this->id().entityName;
+ }
+ std::string getLevelName() const override
+ {
+ return "entity";
+ }
+
+
+ private:
+
+ /// If maximum size is set, ensure `history`'s is not higher.
+ void truncateHistoryToSize()
+ {
+ if (_maxHistorySize >= 0)
+ {
+ while (this->_container.size() > size_t(_maxHistorySize))
+ {
+ this->_container.erase(this->_container.begin());
+ }
+ ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
+ }
+ }
+
+ void checkEntityName(const std::string& name) const
+ {
+ if (name != this->name())
+ {
+ throw armem::error::ContainerNameMismatch(name, getLevelName(), getKeyString());
+ }
+ }
+
+ /**
+ * @brief Return the snapshot with the most recent timestamp.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ */
+ const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const
+ {
+ if (this->_container.empty())
+ {
+ throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
+ }
+ return *this->_container.rbegin();
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6cfc7103728d5a81fecefe4ade6c96efef2814e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
@@ -0,0 +1 @@
+#include "EntityInstanceBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..aab70193435c7eec54b9e2127e7c57528ebba842
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -0,0 +1,87 @@
+#pragma once
+
+#include "../../core/Time.h"
+
+#include "../Commit.h"
+#include "detail/MemoryItem.h"
+
+
+namespace armarx::armem::base
+{
+ /**
+ * @brief Data of a single entity instance.
+ */
+ template <class _DerivedT>
+ class EntityInstanceBase :
+ public detail::MemoryItem
+ {
+ using Base = detail::MemoryItem;
+
+ public:
+
+ using DerivedT = _DerivedT;
+
+
+ public:
+
+ EntityInstanceBase()
+ {
+ }
+ EntityInstanceBase(int index, const MemoryID& parentID = {}) :
+ EntityInstanceBase(parentID.withInstanceIndex(index))
+ {
+ }
+ EntityInstanceBase(const MemoryID& id) :
+ Base(id)
+ {
+ }
+
+
+ inline int& index()
+ {
+ return id().instanceIndex;
+ }
+ inline int index() const
+ {
+ return id().instanceIndex;
+ }
+
+ /**
+ * @brief Fill `*this` with the update's values.
+ * @param update The update.
+ * @param index The instances index.
+ */
+ virtual void update(const EntityUpdate& update, int index) = 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
+ {
+ return "entity instance";
+ }
+
+ std::string getKeyString() const override
+ {
+ return std::to_string(index());
+ }
+
+
+ protected:
+
+ virtual void _copySelf(DerivedT& other) const
+ {
+ Base::_copySelf(other);
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f7899f50d954f16b2d9c0a9569519529895eb20
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
@@ -0,0 +1 @@
+#include "EntitySnapshotBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..defdf1c049fa5274b0d112f42d78037730d9eff5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -0,0 +1,202 @@
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include "../MemoryID.h"
+#include "../Time.h"
+
+#include "EntityInstanceBase.h"
+#include "detail/MemoryContainerBase.h"
+
+
+namespace armarx::armem::base
+{
+ /**
+ * @brief Data of an entity at one point in time.
+ */
+ template <class _EntityInstanceT, class _Derived>
+ class EntitySnapshotBase :
+ 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;
+
+
+ public:
+
+ EntitySnapshotBase()
+ {
+ }
+ EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
+ EntitySnapshotBase(parentID.withTimestamp(time))
+ {
+ }
+ EntitySnapshotBase(const MemoryID& id) :
+ Base(id)
+ {
+ }
+
+ 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 (this->size() != other.size())
+ {
+ return false;
+ }
+ int i = 0;
+ for (const auto& instance : this->_container)
+ {
+ if (not instance.equalsDeep(other.getInstance(i)))
+ {
+ return false;
+ }
+ i++;
+ }
+ return true;
+ }
+
+ inline Time& time()
+ {
+ return this->id().timestamp;
+ }
+
+ inline const Time& time() const
+ {
+ return this->id().timestamp;
+ }
+
+
+ inline const std::vector<EntityInstanceT>& instances() const
+ {
+ return this->_container;
+ }
+ inline std::vector<EntityInstanceT>& instances()
+ {
+ return const_cast<std::vector<EntityInstanceT>&>(const_cast<const EntitySnapshotBase*>(this)->instances());
+ }
+
+
+ void update(const EntityUpdate& update, std::optional<MemoryID> parentID = std::nullopt)
+ {
+ if (parentID)
+ {
+ this->id() = *parentID;
+ }
+ time() = update.timeCreated;
+
+ this->_container.clear();
+ for (int i = 0; i < int(update.instancesData.size()); ++i)
+ {
+ EntityInstanceT& data = this->_container.emplace_back(i, this->id());
+ data.update(update, i);
+ }
+ }
+
+
+ bool hasInstance(int index) const
+ {
+ size_t si = size_t(index);
+ return index >= 0 && si < this->_container.size();
+ }
+
+ /**
+ * @brief Get the given instance.
+ * @param index The instance's index.
+ * @return The instance.
+ * @throw `armem::error::MissingEntry` If the given index is invalid.
+ */
+ EntityInstanceT& getInstance(int index)
+ {
+ return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
+ }
+
+ const EntityInstanceT& getInstance(int index) const
+ {
+ if (hasInstance(index))
+ {
+ return this->_container[static_cast<size_t>(index)];
+ }
+ else
+ {
+ throw armem::error::MissingEntry(EntityInstanceT().getLevelName(), std::to_string(index), getLevelName(), toDateTimeMilliSeconds(time()));
+ }
+ }
+
+ /**
+ * @brief Get the given instance.
+ * @param index The instance's index.
+ * @return The instance.
+ * @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)
+ {
+ return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(id));
+ }
+
+ const EntityInstanceT& getInstance(const MemoryID& id) const
+ {
+ if (!id.hasInstanceIndex())
+ {
+ throw armem::error::InvalidMemoryID(id, "ID has no instance index.");
+ }
+ return getInstance(id.instanceIndex);
+ }
+
+
+ /**
+ * @brief Add a single instance with data.
+ * @param instance The instance.
+ * @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)
+ {
+ return addInstance(EntityInstanceT(instance));
+ }
+
+ EntityInstanceT& addInstance(EntityInstanceT&& instance)
+ {
+ 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 && 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 = this->_container.size();
+ auto& it = this->_container.emplace_back(std::move(instance));
+ it.index() = new_index;
+ return it;
+ }
+
+ std::string getKeyString() const override
+ {
+ return toDateTimeMilliSeconds(this->time());
+ }
+
+ std::string getLevelName() const override
+ {
+ return "entity snapshot";
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.cpp b/source/RobotAPI/libraries/armem/core/base/MemoryBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42ab1cf226c35715139a1e2fbcd5a9c69430e20a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.cpp
@@ -0,0 +1 @@
+#include "MemoryBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..53b398a284bc8ebb1151d5266f81ec7a43010cea
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -0,0 +1,222 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "CoreSegmentBase.h"
+#include "detail/EntityContainerBase.h"
+
+
+namespace armarx::armem::base
+{
+
+ /**
+ * @brief Data of a memory consisting of multiple core segments.
+ */
+ template <class _CoreSegmentT, class _Derived>
+ class MemoryBase :
+ 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;
+
+
+ public:
+
+ MemoryBase()
+ {
+ }
+ 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;
+
+
+ inline const std::string& name() const
+ {
+ return this->id().memoryName;
+ }
+ inline std::string& name()
+ {
+ return const_cast<std::string&>(const_cast<const MemoryBase*>(this)->name());
+ }
+
+
+ inline const std::map<std::string, CoreSegmentT>& coreSegments() const
+ {
+ return this->_container;
+ }
+ inline std::map<std::string, CoreSegmentT>& coreSegments()
+ {
+ return const_cast<std::map<std::string, CoreSegmentT>&>(const_cast<const MemoryBase*>(this)->coreSegments());
+ }
+
+
+ bool hasCoreSegment(const std::string& name) const
+ {
+ return this->_container.count(name) > 0;
+ }
+
+ CoreSegmentT& getCoreSegment(const std::string& name)
+ {
+ return const_cast<CoreSegmentT&>(const_cast<const MemoryBase*>(this)->getCoreSegment(name));
+ }
+
+ const CoreSegmentT& getCoreSegment(const std::string& name) const
+ {
+ 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());
+ }
+ }
+
+ using Base::getEntity;
+ const EntityT& getEntity(const MemoryID& id) const override
+ {
+ 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)
+ {
+ return addCoreSegment(CoreSegmentT(name, coreSegmentType));
+ }
+ /// Copy and insert a core segment.
+ CoreSegmentT& addCoreSegment(const CoreSegmentT& coreSegment)
+ {
+ return addCoreSegment(CoreSegmentT(coreSegment));
+ }
+ /// Move and insert a core segment.
+ CoreSegmentT& addCoreSegment(CoreSegmentT&& coreSegment)
+ {
+ if (this->_container.count(coreSegment.name()) > 0)
+ {
+ throw armem::error::ContainerEntryAlreadyExists(coreSegment.getLevelName(), coreSegment.name(),
+ this->getLevelName(), this->name());
+ }
+ auto it = this->_container.emplace(coreSegment.name(), std::move(coreSegment)).first;
+ it->second.id().setMemoryID(this->id());
+ return it->second;
+ }
+
+ /**
+ * @brief Add multiple core segments.
+ * @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*> segments;
+ for (const auto& name : names)
+ {
+ try
+ {
+ segments.push_back(&addCoreSegment(name));
+ }
+ catch (const armem::error::ContainerEntryAlreadyExists& e)
+ {
+ ARMARX_INFO << e.what() << "\nIgnoring multiple addition.";
+ }
+ }
+ return segments;
+ }
+
+ virtual MemoryID update(const EntityUpdate& update) override
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+
+ 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());
+ }
+ }
+
+ virtual bool equalsDeep(const MemoryBase& other) const
+ {
+ //std::cout << "Memory::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ for (const auto& [key, core] : this->_container)
+ {
+ if (not other.hasCoreSegment(key))
+ {
+ return false;
+ }
+ if (not core.equalsDeep(other.getCoreSegment(key)))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ std::string getLevelName() const override
+ {
+ return "memory";
+ }
+
+ std::string getKeyString() const override
+ {
+ return this->name();
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.cpp b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92538a7b9c82e653c9138deb4b1ca766eb3b218f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.cpp
@@ -0,0 +1 @@
+#include "ProviderSegmentBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..503650498d9afb5ddcdbbf22ecb84baaae3201f8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -0,0 +1,233 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include "EntityBase.h"
+#include "detail/AronTyped.h"
+#include "detail/EntityContainerBase.h"
+#include "detail/MaxHistorySize.h"
+
+
+namespace armarx::armem::base
+{
+
+ /**
+ * @brief Data of a provider segment containing multiple entities.
+ */
+ template <class _EntityT, class _Derived>
+ class ProviderSegmentBase :
+ public detail::EntityContainerBase<_EntityT, _EntityT, _Derived>,
+ public detail::MaxHistorySize,
+ public detail::AronTyped
+ {
+ 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;
+
+
+ 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)
+ {
+ }
+ 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;
+
+
+ inline const std::string& name() const
+ {
+ return this->id().providerSegmentName;
+ }
+ inline std::string& name()
+ {
+ return const_cast<std::string&>(const_cast<const ProviderSegmentBase*>(this)->name());
+ }
+
+
+ inline const std::map<std::string, EntityT>& entities() const
+ {
+ return this->_container;
+ }
+ inline std::map<std::string, EntityT>& entities()
+ {
+ return const_cast<std::map<std::string, EntityT>&>(const_cast<const ProviderSegmentBase*>(this)->entities());
+ }
+
+
+ bool hasEntity(const std::string& name) const
+ {
+ return this->_container.count(name) > 0;
+ }
+
+ using Base::getEntity;
+ const EntityT& getEntity(const MemoryID& id) const override
+ {
+ this->_checkContainerName(id.providerSegmentName, this->getKeyString());
+ return getEntity(id.entityName);
+ }
+
+ EntityT& getEntity(const std::string& name)
+ {
+ return const_cast<EntityT&>(const_cast<const ProviderSegmentBase*>(this)->getEntity(name));
+ }
+
+ const EntityT& getEntity(const std::string& name) const
+ {
+ auto it = this->_container.find(name);
+ if (it != this->_container.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ throw error::MissingEntry("entity", name, getLevelName(), this->name());
+ }
+ }
+
+ 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.
+ *
+ * Missing entity entries are added before updating.
+ */
+ virtual MemoryID update(const EntityUpdate& update) override
+ {
+ this->_checkContainerName(update.entityID.providerSegmentName, this->name());
+
+ 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);
+ }
+ else
+ {
+ entity = &it->second;
+ }
+ // Update entity.
+ return entity->update(update);
+ }
+
+ /// Add an empty entity with the given name.
+ EntityT& addEntity(const std::string& name)
+ {
+ return addEntity(EntityT(name));
+ }
+ /// Copy and insert an entity.
+ EntityT& addEntity(const EntityT& entity)
+ {
+ return addEntity(EntityT(entity));
+ }
+ /// Move and insert an entity.
+ EntityT& addEntity(EntityT&& entity)
+ {
+ auto it = this->_container.emplace(entity.name(), std::move(entity)).first;
+ it->second.id().setProviderSegmentID(this->id());
+ return it->second;
+ }
+
+
+ /**
+ * @brief Sets the maximum history size of container in this segment.
+ * This affects all current container as well as new ones.
+ * @see Entity::setMaxHistorySize()
+ */
+ void setMaxHistorySize(long maxSize) override
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ for (auto& [name, entity] : this->_container)
+ {
+ entity.setMaxHistorySize(maxSize);
+ }
+ }
+
+
+ virtual bool equalsDeep(const ProviderSegmentBase& other) const
+ {
+ //std::cout << "ProviderSegment::equalsDeep" << std::endl;
+ if (this->size() != other.size())
+ {
+ return false;
+ }
+ for (const auto& [key, value] : this->_container)
+ {
+ if (not other.hasEntity(key))
+ {
+ return false;
+ }
+
+ if (not value.equalsDeep(other.getEntity(key)))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ std::string getLevelName() const override
+ {
+ return "provider segment";
+ }
+
+ std::string getKeyString() const override
+ {
+ return this->name();
+ }
+
+
+ protected:
+
+ virtual void _copySelf(DerivedT& other) const override
+ {
+ Base::_copySelf(other);
+ other.aronType() = _aronType;
+ }
+ virtual void _copySelfEmpty(DerivedT& other) const override
+ {
+ Base::_copySelfEmpty(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.cpp b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..05a23bc12fea34e175218498beca73e01b6f1b13
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.cpp
@@ -0,0 +1 @@
+#include "EntityContainerBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..09733b648d096c7e33179c2903f3dbe241dca8d2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -0,0 +1,130 @@
+#pragma once
+
+#include "../../Commit.h"
+#include "../../error/ArMemError.h"
+
+#include "MemoryContainerBase.h"
+
+#include "../EntityBase.h"
+#include "../EntitySnapshotBase.h"
+
+
+namespace armarx::armem::base::detail
+{
+
+ /**
+ * @brief A container of entities at some point in the hierarchy.
+ *
+ * Can be updated by multiple entity updates.
+ */
+ template <class _ValueT, class _EntityT, class _Derived>
+ class EntityContainerBase :
+ public MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>
+ {
+ using Base = MemoryContainerBase<std::map<std::string, _ValueT>, _Derived>;
+
+ public:
+
+ using DerivedT = _Derived;
+ using ValueT = _ValueT;
+
+ using EntityT = _EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+ using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+
+
+ public:
+
+ using Base::MemoryContainerBase;
+ using Base::operator=;
+
+
+ /**
+ * @brief Store all updates in `commit`.
+ * @param commit The commit.
+ * @return The resulting memory IDs.
+ */
+ std::vector<MemoryID> update(const Commit& commit)
+ {
+ std::vector<MemoryID> ids;
+ for (const auto& update : commit.updates)
+ {
+ ids.push_back(this->update(update));
+ }
+ return ids;
+ }
+ /**
+ * @brief Store the given update.
+ * @param update The update.
+ * @return The resulting entity snapshot's ID.
+ */
+ virtual MemoryID update(const EntityUpdate& update) = 0;
+
+
+ /**
+ * @brief Retrieve an entity.
+ * @param id The entity ID.
+ * @return The entity.
+ * @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
+ */
+ virtual EntityT& getEntity(const MemoryID& id)
+ {
+ return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
+ }
+ 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.
+ *
+ * Uses `getEntity()` to retrieve the respective entity.
+ *
+ * @param id The snapshot ID.
+ * @return The entity snapshot.
+ * @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
+ */
+ virtual EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
+ {
+ return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
+ }
+
+ virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
+ {
+ const EntityT& entity = getEntity(id);
+
+ if (id.hasTimestamp())
+ {
+ return entity.getSnapshot(id);
+ }
+ else
+ {
+ return entity.getLatestSnapshot();
+ }
+ }
+
+ virtual EntityInstanceT& getEntityInstance(const MemoryID& id)
+ {
+ return getEntitySnapshot(id).getInstance(id);
+ }
+
+ virtual const EntityInstanceT& getEntityInstance(const MemoryID& id) const
+ {
+ return getEntitySnapshot(id).getInstance(id);
+ }
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
similarity index 66%
rename from source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp
rename to source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
index cdd34e3be26351bab89ad6b712026769057f8b0e..2c3b0554c65523826db0a7709599ed8e5140e0a1 100644
--- a/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.cpp
@@ -1,7 +1,7 @@
#include "MaxHistorySize.h"
-namespace armarx::armem::detail
+namespace armarx::armem::base::detail
{
MaxHistorySize::~MaxHistorySize()
{
@@ -9,11 +9,11 @@ namespace armarx::armem::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/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
similarity index 77%
rename from source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.h
rename to source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
index d96e732964103c93987708e2b1d5dbaf9377640a..69539df5c4063b8d27cd751b79ee82eb90ff5b7e 100644
--- a/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
@@ -2,7 +2,7 @@
-namespace armarx::armem::detail
+namespace armarx::armem::base::detail
{
class MaxHistorySize
{
@@ -23,9 +23,12 @@ namespace armarx::armem::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.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a3706b50bfc3e0f0e96b1d5182a0c7fb9f3a3be
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.cpp
@@ -0,0 +1 @@
+#include "MemoryContainerBase.h"
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2155c70dbf645ebe0e2783fd3189bf9b58800af
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -0,0 +1,135 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
+
+#include "MemoryItem.h"
+
+
+namespace armarx::armem::base::detail
+{
+
+ /**
+ * @class Provides default implmentations of `MemoryContainer`, as well as
+ * iterators (which requires a template).
+ */
+ template <class _ContainerT, class _DerivedT>
+ class MemoryContainerBase :
+ public MemoryItem
+ {
+ using Base = MemoryItem;
+
+ public:
+
+ using DerivedT = _DerivedT;
+ using ContainerT = _ContainerT;
+
+
+ public:
+
+
+ MemoryContainerBase()
+ {}
+ MemoryContainerBase(const MemoryID& id) :
+ MemoryItem(id)
+ {
+ }
+
+ 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();
+ }
+ virtual std::size_t size() const
+ {
+ return _container.size();
+ }
+ virtual void clear()
+ {
+ return _container.clear();
+ }
+
+ typename ContainerT::const_iterator begin() const
+ {
+ return _container.begin();
+ }
+ typename ContainerT::iterator begin()
+ {
+ return _container.begin();
+ }
+ typename ContainerT::const_iterator end() const
+ {
+ return _container.end();
+ }
+ typename ContainerT::iterator end()
+ {
+ return _container.end();
+ }
+
+ const ContainerT& container() const
+ {
+ return _container;
+ }
+ ContainerT& container()
+ {
+ return _container;
+ }
+
+
+ // Copying
+
+ virtual DerivedT copy() const
+ {
+ DerivedT d;
+ this->_copySelf(d);
+ return d;
+ }
+
+ /// Make a copy not containing any elements.
+ virtual DerivedT copyEmpty() const
+ {
+ DerivedT d;
+ this->_copySelfEmpty(d);
+ return d;
+ }
+
+
+ protected:
+
+ /**
+ * @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 (!((emptyOk && gottenName.empty()) || gottenName == actualName))
+ {
+ throw armem::error::ContainerNameMismatch(gottenName, this->getLevelName(), actualName);
+ }
+ }
+
+ virtual void _copySelf(DerivedT& other) const
+ {
+ Base::_copySelf(other);
+ other.container() = _container;
+ }
+
+ virtual void _copySelfEmpty(DerivedT& other) const
+ {
+ Base::_copySelf(other);
+ }
+
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..a6a9f73e17443091503c09472bc3cee7259644ac
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
@@ -0,0 +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/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
similarity index 60%
rename from source/RobotAPI/libraries/armem/core/detail/MemoryItem.h
rename to source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
index 534b631274eae77b597387fb8b8b3ea26dafe515..45226e6327520b7bbddc198364c16c2b8229352c 100644
--- a/source/RobotAPI/libraries/armem/core/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
@@ -2,10 +2,10 @@
#include <string>
-#include "../../core/MemoryID.h"
+#include "../../MemoryID.h"
-namespace armarx::armem::detail
+namespace armarx::armem::base::detail
{
/**
@@ -18,9 +18,17 @@ namespace armarx::armem::detail
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& operator=(const MemoryItem& other);
+
+
inline MemoryID& id()
{
return _id;
@@ -42,7 +50,12 @@ namespace armarx::armem::detail
protected:
- /// The memory ID.
+ /// Copy `*this` to `other`.
+ virtual void _copySelf(MemoryItem& other) const;
+
+
+ protected:
+
MemoryID _id;
};
diff --git a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.cpp b/source/RobotAPI/libraries/armem/core/detail/EntityContainer.cpp
deleted file mode 100644
index 43ade411e4058b73fdfde97cd9ce7a28d3f09763..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "EntityContainer.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-
-namespace armarx::armem::detail
-{
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h b/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h
deleted file mode 100644
index 873a15e6ef8379defd24696b216be7e2ceba7190..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h
+++ /dev/null
@@ -1,137 +0,0 @@
-#pragma once
-
-#include "../Commit.h"
-#include "../Entity.h"
-#include "../EntitySnapshot.h"
-#include "../error/ArMemError.h"
-
-#include "MemoryContainer.h"
-
-
-namespace armarx::armem::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 : public MemoryContainer<std::map<std::string, ValueT>, Derived>
- {
- using Base = MemoryContainer<std::map<std::string, ValueT>, Derived>;
-
- public:
-
- EntityContainer()
- {}
- EntityContainer(const MemoryID& id) :
- MemoryContainer<std::map<std::string, ValueT>, Derived> (id)
- {}
-
- virtual ~EntityContainer() override
- {}
-
-
- virtual const std::string& name() const = 0;
- virtual std::string& name()
- {
- return const_cast<std::string&>(const_cast<const EntityContainer*>(this)->name());
- }
-
-
- /**
- * @brief Store all updates in `commit`.
- * @param commit The commit.
- * @return The resulting memory IDs.
- */
- std::vector<MemoryID> update(const Commit& commit)
- {
- std::vector<MemoryID> ids;
- for (const auto& update : commit.updates)
- {
- ids.push_back(this->update(update));
- }
- return ids;
- }
- /**
- * @brief Store the given update.
- * @param update The update.
- * @return The resulting entity snapshot's ID.
- */
- virtual MemoryID update(const EntityUpdate& update) = 0;
-
-
- /**
- * @brief Retrieve an entity.
- * @param id The entity ID.
- * @return The entity.
- * @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
- */
- virtual Entity& getEntity(const MemoryID& id)
- {
- return const_cast<Entity&>(const_cast<const EntityContainer*>(this)->getEntity(id));
- }
- virtual const Entity& getEntity(const MemoryID& id) const = 0;
-
- /**
- * @brief Retrieve an entity snapshot.
- *
- * Uses `getEntity()` to retrieve the respective entity.
- *
- * @param id The snapshot ID.
- * @return The entity snapshot.
- * @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
- */
- virtual EntitySnapshot& getEntitySnapshot(const MemoryID& id)
- {
- return const_cast<EntitySnapshot&>(const_cast<const EntityContainer*>(this)->getEntitySnapshot(id));
- }
- virtual const EntitySnapshot& getEntitySnapshot(const MemoryID& id) const
- {
- const Entity& entity = getEntity(id);
-
- if (id.hasTimestamp())
- {
- return entity.getSnapshot(id);
- }
- else
- {
- return entity.getLatestSnapshot();
- }
- }
-
-
- virtual EntityInstance& getEntityInstance(const MemoryID& id)
- {
- return getEntitySnapshot(id).getInstance(id);
- }
- virtual const EntityInstance& getEntityInstance(const MemoryID& id) const
- {
- return getEntitySnapshot(id).getInstance(id);
- }
-
-
- // MemoryLevel interface
- public:
- /// Return `name`.
- virtual std::string getKeyString() const override
- {
- return name();
- }
-
-
- protected:
-
- /// @throw `armem::error::ContainerNameMismatch` Of `expectedName != actualName`.
- void _checkContainerName(const std::string& expectedName, const std::string& actualName) const
- {
- if (expectedName != actualName)
- {
- throw armem::error::ContainerNameMismatch(expectedName, this->getLevelName(), actualName);
- }
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.cpp b/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.cpp
deleted file mode 100644
index 9fd73048f1a7eef7c03e2500443b0353ffb20177..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.cpp
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "MemoryContainer.h"
-
-
-namespace armarx::armem::detail
-{
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.h b/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.h
deleted file mode 100644
index 6b64db1dd03b00c856d62a6bc0d12139010c0ecf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/MemoryContainer.h
+++ /dev/null
@@ -1,114 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../../error.h"
-
-#include "MemoryItem.h"
-
-
-namespace armarx::armem::detail
-{
-
- /**
- * @class Provides default implmentations of `MemoryContainer`, as well as
- * iterators (which requires a template).
- */
- template <class _ContainerT, class _Derived>
- class MemoryContainer : public MemoryItem
- {
- public:
- using ContainerT = _ContainerT;
- using Derived = _Derived;
-
- public:
-
- MemoryContainer()
- {}
- MemoryContainer(const MemoryID& id) : MemoryItem(id)
- {}
-
- virtual ~MemoryContainer()
- {}
-
-
- // Container methods
-
- virtual bool empty() const
- {
- return _underlyingContainer().empty();
- }
- virtual std::size_t size() const
- {
- return _underlyingContainer().size();
- }
- virtual void clear()
- {
- return _underlyingContainer().clear();
- }
-
- typename ContainerT::const_iterator begin() const
- {
- return _underlyingContainer().begin();
- }
- typename ContainerT::iterator begin()
- {
- return _underlyingContainer().begin();
- }
- typename ContainerT::const_iterator end() const
- {
- return _underlyingContainer().end();
- }
- typename ContainerT::iterator end()
- {
- return _underlyingContainer().end();
- }
-
-
- // Copying
-
- virtual Derived copy(bool withData = true) const
- {
- auto cast = dynamic_cast<const Derived*>(this);
- ARMARX_CHECK_NOT_NULL(cast);
-
- Derived other;
- this->_copySelf(other);
- this->_copyElements(other, withData);
- return other;
- }
-
- /// Make a copy not containing any elements.
- virtual Derived copyEmpty() const
- {
- auto cast = dynamic_cast<const Derived*>(this);
- ARMARX_CHECK_NOT_NULL(cast);
-
- Derived other;
- this->_copySelf(other);
- return other;
- }
-
-
- protected:
-
-
- virtual ContainerT& _underlyingContainer() = 0;
- virtual const ContainerT& _underlyingContainer() const
- {
- return const_cast<const ContainerT&>(const_cast<MemoryContainer*>(this)->_underlyingContainer());
- }
-
- virtual void _copySelf(Derived& other) const
- {
- other.id() = this->id();
- (void) other;
- }
- virtual void _copyElements(Derived& other, bool withData) const
- {
- (void) other, (void) withData;
- }
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/core/detail/MemoryItem.cpp
deleted file mode 100644
index 32640023ba26f2c3bdaba4f4f49a4925068a7db1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/MemoryItem.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "MemoryItem.h"
-
-
-namespace armarx::armem::detail
-{
-
- MemoryItem::MemoryItem()
- {
- }
-
- MemoryItem::MemoryItem(const MemoryID& id) : _id(id)
- {
- }
-
- MemoryItem::~MemoryItem()
- {
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/detail/TypedEntityContainer.h b/source/RobotAPI/libraries/armem/core/detail/TypedEntityContainer.h
deleted file mode 100644
index f932b72297b89eb9248bd05751ef9a3b0085324f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/detail/TypedEntityContainer.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#pragma once
-
-#include "EntityContainer.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-namespace armarx::armem::detail
-{
-
- /**
- * @brief An entity container with a specific (Aron) type.
- */
- template <class _ValueT, class Derived>
- class TypedEntityContainer : public EntityContainer<_ValueT, Derived>
- {
- using ValueT = _ValueT;
- // using Base = EntityContainer<ValueT, Derived>;
-
- public:
-
- TypedEntityContainer(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) : _aronType(aronType)
- {}
- TypedEntityContainer(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
- EntityContainer<ValueT, Derived>(id), _aronType(aronType)
- {}
-
- virtual ~TypedEntityContainer() override
- {}
-
-
- bool hasAronType() const
- {
- return _aronType != nullptr;
- }
- aron::typenavigator::ObjectNavigatorPtr& aronType()
- {
- return _aronType;
- }
- aron::typenavigator::ObjectNavigatorPtr aronType() const
- {
- return _aronType;
- }
-
-
-
- // MemoryContainer interface
- protected:
- virtual void _copySelf(Derived& other) const override
- {
- EntityContainer<ValueT, Derived>::_copySelf(other);
- other._aronType = this->_aronType;
- }
-
-
- private:
-
- /// 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
new file mode 100644
index 0000000000000000000000000000000000000000..6a26be9c7915a54b081e5d03b4f67cb6ae9eeaee
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -0,0 +1,96 @@
+#include "CoreSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "TypeIO.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ std::filesystem::path CoreSegment::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ std::filesystem::path CoreSegment::_fullPath(const std::filesystem::path& path) const
+ {
+ return path / id().memoryName / id().coreSegmentName;
+ }
+
+ wm::CoreSegment CoreSegment::convert() const
+ {
+ wm::CoreSegment m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addProviderSegment(s.convert(_aronType));
+ }
+ return m;
+ }
+
+ void CoreSegment::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
+ {
+ if (!p_ptr)
+ {
+ ARMARX_WARNING << "The entered is NULL.";
+ }
+ std::filesystem::path p = _fullPath(*p_ptr);
+ if (std::filesystem::is_regular_file(p))
+ {
+ ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
+ }
+
+ _container.clear();
+ path = p_ptr;
+
+ if (!std::filesystem::exists(p))
+ {
+ ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
+ }
+ else
+ {
+ 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().withProviderSegmentName(k)));
+ wms.first->second.reload(p_ptr);
+ }
+
+ if (d.is_regular_file())
+ {
+ if (auto type = TypeIO::readAronType(d.path()))
+ {
+ _aronType = type;
+ }
+ }
+ }
+ }
+ }
+
+ void CoreSegment::append(const wm::CoreSegment& m)
+ {
+ std::filesystem::create_directories(_fullPath());
+ TypeIO::writeAronType(_aronType, _fullPath());
+
+ for (const auto& [k, s] : m)
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..76bb297e76d268ce8517449686ffae1cdb3ed2cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/CoreSegmentBase.h"
+#include "../workingmemory/CoreSegment.h"
+
+#include "ProviderSegment.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ /**
+ * @brief Data of a core segment containing multiple provider segments.
+ */
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::CoreSegment convert() const;
+
+ // Filesystem connection
+ void reload(const std::shared_ptr<std::filesystem::path>&);
+ void append(const wm::CoreSegment&);
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a616914ede5c279d11daea7afcfe81a681b0c460
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.cpp
@@ -0,0 +1,81 @@
+#include "Entity.h"
+
+namespace armarx::armem::d_ltm
+{
+ std::filesystem::path Entity::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ std::filesystem::path Entity::_fullPath(const std::filesystem::path& path) const
+ {
+ 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)
+ {
+ m.addSnapshot(s.convert(expectedStructure));
+ }
+ return m;
+ }
+
+ void Entity::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
+ {
+ if (!p_ptr)
+ {
+ ARMARX_WARNING << "The entered is NULL.";
+ }
+ std::filesystem::path p = _fullPath(*p_ptr);
+ if (std::filesystem::is_regular_file(p))
+ {
+ ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
+ }
+
+ _container.clear();
+ path = p_ptr;
+
+ if (!std::filesystem::exists(p))
+ {
+ ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
+ }
+ else
+ {
+ for (const auto& d : std::filesystem::directory_iterator(p))
+ {
+ if (d.is_directory())
+ {
+ 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)));
+ wms.first->second.reload(p_ptr);
+ }
+ }
+ }
+ }
+
+ void Entity::append(const wm::Entity& m)
+ {
+ std::filesystem::create_directories(_fullPath());
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..35022275570c93378cbe0c767aad1776da594838
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Entity.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/EntityBase.h"
+#include "../workingmemory/Entity.h"
+
+#include "EntitySnapshot.h"
+
+
+
+namespace armarx::armem::d_ltm
+{
+ /**
+ * @brief An entity over a period of time.
+ *
+ * An entity should be a physical thing or abstract concept existing
+ * (and potentially evolving) over some time.
+ *
+ * Examples are:
+ * - objects (the green box)
+ * - agents (robot, human)
+ * - locations (frige, sink)
+ * - grasp affordances (general, or for a specific object)
+ * - images
+ * - point clouds
+ * - other sensory values
+ *
+ * At each point in time (`EntitySnapshot`), the entity can have a
+ * (potentially variable) number of instances (`EntityInstance`),
+ * each containing a single `AronData` object of a specific `AronType`.
+ */
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>
+ {
+ using Base = base::EntityBase<EntitySnapshot, Entity>;
+
+ public:
+
+ using Base::EntityBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::Entity convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
+
+ // Filesystem connection
+ void reload(const std::shared_ptr<std::filesystem::path>&);
+ void append(const wm::Entity&);
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49bfe419d8a8cf1052fad7e47a0ecba9e355c529
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.cpp
@@ -0,0 +1,185 @@
+#include "EntityInstance.h"
+
+#include <iostream>
+#include <fstream>
+
+#include "../../core/error.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+
+namespace armarx::armem::d_ltm
+{
+
+ EntityInstance::EntityInstance(const EntityInstance& other) :
+ Base(other),
+ path(other.path)
+ {
+ }
+
+ bool EntityInstance::equalsDeep(const EntityInstance& other) const
+ {
+ return id() == other.id();
+ }
+
+
+
+ void EntityInstance::update(const EntityUpdate& update, int index)
+ {
+ ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
+
+ this->index() = index;
+ }
+
+ EntityInstance EntityInstance::copy() const
+ {
+ EntityInstance d;
+ this->_copySelf(d);
+ return d;
+ }
+
+ void EntityInstance::_copySelf(EntityInstance& other) const
+ {
+ EntityInstanceBase<EntityInstance>::_copySelf(other);
+ }
+
+ std::filesystem::path EntityInstance::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ 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);
+ }
+
+ wm::EntityInstance EntityInstance::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
+ {
+ std::filesystem::path p = _fullPath();
+ std::filesystem::path d = p / (std::string(DATA_FILENAME) + ".json");
+
+ if (std::filesystem::is_regular_file(d))
+ {
+ std::ifstream ifs(d);
+ std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+ aron::dataIO::reader::NlohmannJSONReader dataReader(file_content);
+ aron::dataIO::writer::NavigatorWriter navWriter;
+
+ aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
+
+ aron::datanavigator::DictNavigatorPtr aron = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
+ return unwrapData(aron);
+ }
+ else
+ {
+ throw error::ArMemError("An diskMemory EntityInstance is not leading to a regular file.");
+ }
+ }
+
+ void EntityInstance::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
+ {
+ if (!p_ptr)
+ {
+ ARMARX_WARNING << "The entered is NULL.";
+ }
+ std::filesystem::path p = _fullPath(*p_ptr);
+ if (!std::filesystem::is_directory(p))
+ {
+ ARMARX_ERROR << "The entered path is not leading to a file! This is an error since if the folder for an EntityInstance exists there must be a data file in it (containing at least the metadata).";
+ }
+ else
+ {
+ path = p_ptr;
+ }
+ }
+
+ void EntityInstance::setTo(const wm::EntityInstance& m)
+ {
+ std::filesystem::path p = _fullPath();
+ std::filesystem::create_directories(p);
+
+ std::filesystem::path d = p / (std::string(DATA_FILENAME) + ".json");
+
+ if (std::filesystem::is_regular_file(d))
+ {
+ std::filesystem::remove(d);
+ }
+
+ std::ofstream ofs;
+ ofs.open(d);
+
+ aron::datanavigator::DictNavigatorPtr aron = wrapData(m);
+ aron::dataIO::writer::NlohmannJSONWriter dataWriter;
+ aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
+ std::string new_file_full_content = dataWriter.getResult().dump(2);
+
+ ofs << new_file_full_content;
+ ofs.close();
+ }
+
+
+ wm::EntityInstance EntityInstance::unwrapData(const aron::datanavigator::DictNavigatorPtr& dataWrapped) const
+ {
+ wm::EntityInstance e(id());
+ wm::EntityInstanceMetadata& metadata = e.metadata();
+
+ if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
+ {
+ aron::datanavigator::DictNavigatorPtr data = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
+ e.setData(data);
+ }
+
+ auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
+ metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
+
+ auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
+ metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
+
+ auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
+ metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
+
+ auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
+ metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
+
+ return e;
+ }
+
+ aron::datanavigator::DictNavigatorPtr EntityInstance::wrapData(const wm::EntityInstance& e) const
+ {
+ auto dataWrapped = std::make_shared<aron::datanavigator::DictNavigator>();
+ if (e.data())
+ {
+ dataWrapped->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
+ }
+
+ auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeWrapped->setValue(Time::now().toMicroSeconds());
+ dataWrapped->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
+
+ const wm::EntityInstanceMetadata& metadata = e.metadata();
+ auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
+ dataWrapped->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
+
+ auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeSent->setValue(metadata.timeSent.toMicroSeconds());
+ dataWrapped->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
+
+ auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
+ timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
+ dataWrapped->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
+
+ auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..9ee48c55b33dca02c88f57de1ac30426f8d5ae37
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntityInstance.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/EntityInstanceBase.h"
+#include "../workingmemory/EntityInstance.h"
+
+
+namespace armarx::armem::d_ltm
+{
+ /**
+ * @brief Data of a single entity instance.
+ */
+ class EntityInstance :
+ public base::EntityInstanceBase<EntityInstance>
+ {
+ using Base = base::EntityInstanceBase<EntityInstance>;
+
+ public:
+
+ 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.
+ * @param update The update.
+ * @param index The instances index.
+ */
+ virtual void update(const EntityUpdate& update, int index) override;
+
+ virtual bool equalsDeep(const EntityInstance& other) const override;
+
+ virtual EntityInstance copy() const override;
+
+ // Conversion
+ wm::EntityInstance convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
+
+ // Filesystem connection
+ void reload(const std::shared_ptr<std::filesystem::path>&);
+ void setTo(const wm::EntityInstance&);
+
+ protected:
+ virtual void _copySelf(EntityInstance& other) const override;
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ wm::EntityInstance unwrapData(const aron::datanavigator::DictNavigatorPtr&) const;
+ aron::datanavigator::DictNavigatorPtr wrapData(const wm::EntityInstance&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+
+ private:
+ static const constexpr char* DATA_FILENAME = "data";
+ static constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
+ static constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
+ static constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
+ static constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
+ static constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
+ static constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..199e4f2c9b9eed9d9b099e773d96ba0a03fd7d16
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.cpp
@@ -0,0 +1,87 @@
+#include "EntitySnapshot.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ std::filesystem::path EntitySnapshot::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ 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());
+ }
+
+ wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
+ {
+ wm::EntitySnapshot m;
+ for (const auto& s : _container)
+ {
+ m.addInstance(s.convert(expectedStructure));
+ }
+ return m;
+ }
+
+ void EntitySnapshot::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
+ {
+ if (!p_ptr)
+ {
+ ARMARX_WARNING << "The entered is NULL.";
+ }
+ std::filesystem::path p = _fullPath(*p_ptr);
+ if (!std::filesystem::is_directory(p))
+ {
+ ARMARX_ERROR << "The entered path is not leading to a directory! Every EntitySnapshot must at least contain one EntityInstance.";
+ }
+ else
+ {
+ _container.clear();
+ path = p_ptr;
+
+ // todo
+ for (int i = 0; i < 1000; ++i)
+ {
+ std::filesystem::path d = p / std::to_string(i);
+ if (std::filesystem::is_directory(d))
+ {
+ auto wms = _container.emplace_back(id().withInstanceIndex(i));
+ wms.reload(p_ptr);
+ }
+ else
+ {
+ break;
+ }
+ }
+ }
+ }
+
+ void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
+ {
+ std::filesystem::create_directories(_fullPath());
+
+ // We remove the contente here and reset it with new values
+ _container.clear();
+
+ 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++));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..f73702857fc6700e5cfae5f6d08b369240d08f79
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/EntitySnapshot.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/EntitySnapshotBase.h"
+#include "../workingmemory/EntitySnapshot.h"
+
+#include "EntityInstance.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ /**
+ * @brief Data of an entity at one point in time.
+ */
+ class EntitySnapshot :
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
+ {
+ using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
+
+ public:
+
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
+
+
+ // Conversion
+ void reload(const std::shared_ptr<std::filesystem::path>&);
+ wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
+
+ // MongoDB connection
+ void setTo(const wm::EntitySnapshot&);
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..874d189d6a22a79c9e015d800c292f9af075cfea
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
@@ -0,0 +1,84 @@
+#include "Memory.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::d_ltm
+{
+ std::filesystem::path Memory::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ std::filesystem::path Memory::_fullPath(const std::filesystem::path& path) const
+ {
+ return path / id().memoryName;
+ }
+
+ wm::Memory Memory::convert() const
+ {
+ wm::Memory m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addCoreSegment(s.convert());
+ }
+ return m;
+ }
+
+ void Memory::reload(const std::filesystem::path& p)
+ {
+ if (std::filesystem::is_regular_file(p))
+ {
+ ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
+ }
+
+ _container.clear();
+ path = std::make_shared<std::filesystem::path>(p.parent_path());
+
+ if (!std::filesystem::exists(p))
+ {
+ ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
+ }
+ else
+ {
+ 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)));
+ wms.first->second.reload(path);
+ }
+ }
+ }
+ }
+
+ void Memory::append(const wm::Memory& m)
+ {
+ std::filesystem::create_directories(_fullPath());
+ for (const auto& [k, s] : m.container())
+ {
+ 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().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
new file mode 100644
index 0000000000000000000000000000000000000000..7f7872f7a638cc31122f09b00a95755ede4d9606
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/MemoryBase.h"
+#include "../workingmemory/Memory.h"
+
+#include "CoreSegment.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ /**
+ * @brief Data of a memory consisting of multiple core segments.
+ */
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ {
+ using Base = base::MemoryBase<CoreSegment, Memory>;
+
+ public:
+
+ using Base::MemoryBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::Memory convert() const;
+
+ // Filesystem connection
+ void reload(const std::filesystem::path&);
+ void append(const wm::Memory&);
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d5ed3d4a588827137b33e69ecaec3be1a4c31fc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
@@ -0,0 +1,102 @@
+#include "ProviderSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "TypeIO.h"
+
+
+namespace armarx::armem::d_ltm
+{
+ std::filesystem::path ProviderSegment::_fullPath() const
+ {
+ if (path)
+ {
+ return _fullPath(*path);
+ }
+ return std::filesystem::path();
+ }
+
+ std::filesystem::path ProviderSegment::_fullPath(const std::filesystem::path& path) const
+ {
+ 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)
+ {
+ if (hasAronType())
+ {
+ m.addEntity(s.convert(_aronType));
+ }
+ else
+ {
+ m.addEntity(s.convert(expectedStructure));
+ }
+ }
+ return m;
+ }
+
+ void ProviderSegment::reload(const std::shared_ptr<std::filesystem::path>& p_ptr)
+ {
+ if (!p_ptr)
+ {
+ ARMARX_WARNING << "The entered is NULL.";
+ }
+ std::filesystem::path p = _fullPath(*p_ptr);
+ if (std::filesystem::is_regular_file(p))
+ {
+ ARMARX_ERROR << "The entered path is leading to a file! Abort due to error.";
+ }
+
+ _container.clear();
+ path = p_ptr;
+
+ if (!std::filesystem::exists(p))
+ {
+ ARMARX_INFO << "The entered path does not exist. Assuming an empty container.";
+ }
+ else
+ {
+ 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().withEntityName(k)));
+ wms.first->second.reload(p_ptr);
+ }
+
+ if (d.is_regular_file())
+ {
+ if (auto type = TypeIO::readAronType(d.path()))
+ {
+ _aronType = type;
+ }
+ }
+ }
+ }
+ }
+
+ void ProviderSegment::append(const wm::ProviderSegment& m)
+ {
+ std::filesystem::create_directories(_fullPath());
+ TypeIO::writeAronType(_aronType, _fullPath());
+
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..8ad96ea0630af0d5aba8b7270fe229c235c2a010
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../base/ProviderSegmentBase.h"
+#include "../workingmemory/ProviderSegment.h"
+
+#include "Entity.h"
+
+
+namespace armarx::armem::d_ltm
+{
+
+ /**
+ * @brief Data of a provider segment containing multiple entities.
+ */
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
+ {
+ using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
+
+ public:
+
+ using Base::ProviderSegmentBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::ProviderSegment convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const;
+
+ // Filesystem connection
+ void reload(const std::shared_ptr<std::filesystem::path>&);
+ void append(const wm::ProviderSegment&);
+
+ private:
+ std::filesystem::path _fullPath() const;
+ std::filesystem::path _fullPath(const std::filesystem::path&) const;
+
+ public:
+ std::shared_ptr<std::filesystem::path> path;
+ };
+
+}
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/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 684028ae6c96d8e905de06d1bf957669d6db3d47..d79acf1da2b86f94414205131173797e0354bc24 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -145,138 +145,13 @@ namespace armarx
}
- void armem::detail::toIceItem(data::detail::MemoryItem& ice, const MemoryItem& item)
+ void armem::detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
{
toIce(ice.id, item.id());
}
- void armem::detail::fromIceItem(const data::detail::MemoryItem& ice, MemoryItem& item)
+ void armem::detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
{
fromIce(ice.id, item.id());
}
-
-
- void armem::toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata)
- {
- ice.confidence = metadata.confidence;
- toIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
- toIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
- toIce(ice.timeSentMicroSeconds, metadata.timeSent);
- }
- void armem::fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata)
- {
- metadata.confidence = ice.confidence;
- fromIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
- fromIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
- fromIce(ice.timeSentMicroSeconds, metadata.timeSent);
- }
-
- void armem::toIce(data::EntityInstance& ice, const EntityInstance& data)
- {
- detail::toIceItem(ice, data);
-
- if (data.data())
- {
- ice.data = data.data()->toAronDictPtr();
- }
- toIce(ice.metadata, data.metadata());
- }
- void armem::fromIce(const data::EntityInstance& ice, EntityInstance& data)
- {
- detail::fromIceItem(ice, data);
-
- if (ice.data)
- {
- data.setData(aron::datanavigator::DictNavigator::FromAronDictPtr(ice.data));
- }
- fromIce(ice.metadata, data.metadata());
- }
-
-
- void armem::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
- {
- detail::toIceItem(ice, snapshot);
-
- toIce(ice.instances, snapshot.instances);
- }
- void armem::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
- {
- detail::fromIceItem(ice, snapshot);
-
- fromIce(ice.instances, snapshot.instances);
- }
-
- void armem::toIce(data::Entity& ice, const Entity& entity)
- {
- detail::toIceItem(ice, entity);
-
- toIce(ice.history, entity.history);
- }
- void armem::fromIce(const data::Entity& ice, Entity& entity)
- {
- detail::fromIceItem(ice, entity);
-
- fromIce(ice.history, entity.history);
- }
-
-
- void armem::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
- {
- detail::toIceItem(ice, providerSegment);
-
- if (providerSegment.hasAronType())
- {
- ice.aronType = providerSegment.aronType()->getResult();
- }
- toIce(ice.entities, providerSegment.entities);
- }
- void armem::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
- {
- detail::fromIceItem(ice, providerSegment);
-
- if (ice.aronType)
- {
- providerSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(
- aron::typenavigator::Navigator::FromAronType(ice.aronType)
- );
- }
- fromIce(ice.entities, providerSegment.entities);
- }
-
- void armem::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
- {
- detail::toIceItem(ice, coreSegment);
-
- if (coreSegment.hasAronType())
- {
- ice.aronType = coreSegment.aronType()->getResult();
- }
- toIce(ice.providerSegments, coreSegment.providerSegments);
- }
- void armem::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
- {
- detail::fromIceItem(ice, coreSegment);
-
- if (ice.aronType)
- {
- coreSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(
- aron::typenavigator::Navigator::FromAronType(ice.aronType)
- );
- }
- fromIce(ice.providerSegments, coreSegment.providerSegments);
- }
-
- void armem::toIce(data::Memory& ice, const Memory& memory)
- {
- detail::toIceItem(ice, memory);
-
- toIce(ice.coreSegments, memory.coreSegments);
- }
- void armem::fromIce(const data::Memory& ice, Memory& memory)
- {
- detail::fromIceItem(ice, memory);
-
- fromIce(ice.coreSegments, memory.coreSegments);
- }
-
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index f615e7c3bfd05861b81a3debedc0b8500b6e1365..874684cac2a6a52b8742c36dcc2bdea8346e1586 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -6,7 +6,7 @@
#include "ice_conversions_templates.h"
#include "Commit.h"
-#include "Memory.h"
+#include "base/detail/MemoryItem.h"
namespace IceUtil
@@ -52,35 +52,10 @@ namespace armarx::armem
void fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived);
-
- void toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata);
- void fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata);
-
- void toIce(data::EntityInstance& ice, const EntityInstance& data);
- void fromIce(const data::EntityInstance& ice, EntityInstance& data);
-
-
- void toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot);
- void fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot);
-
- void toIce(data::Entity& ice, const Entity& entity);
- void fromIce(const data::Entity& ice, Entity& entity);
-
-
- void toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment);
- void fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment);
-
- void toIce(data::CoreSegment& ice, const CoreSegment& coreSegment);
- void fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment);
-
- void toIce(data::Memory& ice, const Memory& memory);
- void fromIce(const data::Memory& ice, Memory& memory);
-
-
namespace detail
{
- void toIceItem(data::detail::MemoryItem& ice, const MemoryItem& item);
- void fromIceItem(const data::detail::MemoryItem& ice, MemoryItem& item);
+ void toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item);
+ void fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
index 2f729f2026aa2fc20ce616b18ed408bf3d1f3206..e0c71faac37cd899d85d174c33196499a29df770 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
@@ -22,8 +22,24 @@ namespace armarx::armem
}
- // General return version
+ // Ice Handle
+ template <class IceT, class CppT>
+ void toIce(::IceInternal::Handle<IceT>& ice, const CppT& cpp)
+ {
+ ice = new IceT();
+ toIce(*ice, cpp);
+ }
+ template <class IceT, class CppT>
+ void fromIce(const ::IceInternal::Handle<IceT>& ice, CppT& cpp)
+ {
+ if (ice)
+ {
+ fromIce(*ice, cpp);
+ }
+ }
+
+ // General return version
template <class IceT, class CppT>
IceT toIce(const CppT& cpp)
{
@@ -39,7 +55,6 @@ namespace armarx::armem
return cpp;
}
-
// std::unique_ptr
template <class IceT, class CppT>
@@ -58,24 +73,6 @@ namespace armarx::armem
}
- // Ice Handle
-
- template <class IceT, class CppT>
- void toIce(::IceInternal::Handle<IceT>& ice, const CppT& cpp)
- {
- ice = new IceT();
- toIce(*ice, cpp);
- }
- template <class IceT, class CppT>
- void fromIce(const ::IceInternal::Handle<IceT>& ice, CppT& cpp)
- {
- if (ice)
- {
- fromIce(*ice, cpp);
- }
- }
-
-
// Ice Handle <-> std::unique_ptr
template <class IceT, class CppT>
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp b/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp
deleted file mode 100644
index 08584369d6f745f7c0203497a531cd2e133319c2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#include "DiskReader.h"
-
-#include <sstream>
-#include <fstream>
-#include <iostream>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-
-#include <RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::io
-{
-
- FileSystemLookupMemory DiskReader::readMemoryStructureFromDisk()
- {
- FileSystemLookupMemory ret;
-
- for (const auto& memoryFolder : std::filesystem::directory_iterator(rootPath))
- {
- if (!std::filesystem::is_directory(memoryFolder))
- {
- continue;
- }
-
- std::string folderName = memoryFolder.path().filename();
- FileSystemLookupMemory tmp = readMemoryStructureFromDisk(folderName);
- ret.merge(tmp);
- }
- return ret;
- }
-
- FileSystemLookupMemory DiskReader::readMemoryStructureFromDisk(const std::string& memoryFolderName)
- {
- FileSystemLookupMemory ret;
-
- std::filesystem::path p = rootPath / memoryFolderName;
- if (!std::filesystem::is_directory(p))
- {
- return ret;
- }
-
- for (const auto& coreSegmentFolder : std::filesystem::directory_iterator(p))
- {
- if (!std::filesystem::is_directory(coreSegmentFolder))
- {
- continue;
- }
- std::string coreSegmentName = coreSegmentFolder.path().filename();
- for (const auto& providerSegmentFolder : std::filesystem::directory_iterator(coreSegmentFolder.path()))
- {
- if (!std::filesystem::is_directory(providerSegmentFolder))
- {
- continue;
- }
- std::string providerSegmentName = providerSegmentFolder.path().filename();
- for (const auto& entityFolder : std::filesystem::directory_iterator(providerSegmentFolder.path()))
- {
- if (!std::filesystem::is_directory(entityFolder))
- {
- continue;
- }
- std::string entityName = entityFolder.path().filename();
- for (const auto& entityHistoryFolder : std::filesystem::directory_iterator(entityFolder.path()))
- {
- if (!std::filesystem::is_directory(entityHistoryFolder))
- {
- continue;
- }
- Time entityTimestamp = armem::timeFromStringMicroSeconds(entityHistoryFolder.path().filename());
-
- // TODO: make this relative to amount of files in folder
- for (unsigned int i = 0; i < 10000; ++i)
- {
- std::filesystem::path entityInstance = entityHistoryFolder / std::filesystem::path(std::to_string(i) + getEntityInstanceSuffix());
- if (!std::filesystem::is_regular_file(entityInstance))
- {
- break;
- }
- DiskMemory& mem = ret.memoryLookupTable[memoryFolderName];
- DiskCoreSegment& mem_core = mem[coreSegmentName];
- DiskProviderSegment& mem_prov = mem_core[providerSegmentName];
- DiskEntity& mem_entity = mem_prov[entityName];
- DiskEntitySnapshot& mem_entity_history = mem_entity[entityTimestamp];
-
- mem_entity_history.push_back(entityInstance);
- }
- }
- }
- }
- }
- return ret;
- }
-
- EntityInstance DiskReader::readSingleInstanceFromDisk(const MemoryID& id, const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- std::filesystem::path p = rootPath / (id.str() + getEntityInstanceSuffix());
- return readSingleInstanceFromDisk(p, expectedStructure);
- }
-
- EntityInstance DiskReader::readSingleInstanceFromDisk(const std::filesystem::path& p, const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- if (!std::filesystem::is_regular_file(p))
- {
- throw error::PathNotARegularFile(p.string(), "During readSingleInstanceFromDisk tried to read an entityInstance from a non-file path.");
- }
-
- std::ifstream ifs(p);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- aron::datanavigator::DictNavigatorPtr dictdata = getStringAsDataNavigator(file_content, expectedStructure);
- return unwrapData(dictdata);
- }
-
- aron::typenavigator::ObjectNavigatorPtr DiskReader::readSingleTypeInformationFromDisk(const MemoryID& id) const
- {
- std::filesystem::path p = rootPath / id.str() / (id.getLeafItem() + getTypeSuffix());
- return readSingleTypeInformationFromDisk(p);
- }
-
- aron::typenavigator::ObjectNavigatorPtr DiskReader::readSingleTypeInformationFromDisk(const std::filesystem::path& p) const
- {
- if (!std::filesystem::is_regular_file(p))
- {
- return nullptr;
- }
-
- std::ifstream ifs(p);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- aron::typenavigator::ObjectNavigatorPtr objecttype = getStringAsTypeNavigator(file_content);
- return objecttype;
- }
-
- EntityInstance DiskReader::unwrapData(const aron::datanavigator::DictNavigatorPtr& dataWrapped) const
- {
- EntityInstance e;
- EntityInstanceMetadata& metadata = e.metadata();
-
- aron::datanavigator::DictNavigatorPtr data = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_DATA_FIELD));
- e.setData(data);
-
- // not used right now
- //aron::datanavigator::LongNavigatorPtr timeWrapped = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_TIME_WRAPPED_FIELD));
-
- aron::datanavigator::LongNavigatorPtr timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_TIME_CREATED_FIELD));
- metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
-
- aron::datanavigator::LongNavigatorPtr timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_TIME_SENT_FIELD));
- metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
-
- aron::datanavigator::LongNavigatorPtr timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_TIME_ARRIVED_FIELD));
- metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
-
- aron::datanavigator::DoubleNavigatorPtr confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DISK_READER_WRITER_CONFIDENCE_FIELD));
- metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
-
- return e;
- }
-
- aron::typenavigator::ObjectNavigatorPtr DiskReader::unwrapType(const aron::typenavigator::ObjectNavigatorPtr& t) const
- {
- return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(t->getMemberType(DISK_READER_WRITER_DATA_FIELD));
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.h b/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.h
deleted file mode 100644
index 2ae252521b823e270533d2305a4af272b76bf944..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <filesystem>
-#include <memory>
-#include <string>
-
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-#include <RobotAPI/libraries/armem/core/EntityInstance.h>
-
-#include "../DiskReaderWriter.h"
-#include "../FileSystemLookupMemory.h"
-
-
-namespace armarx::armem::io
-{
- class DiskReader;
- using DiskReaderPtr = std::shared_ptr<DiskReader>;
-
- class DiskReader : virtual public DiskReaderWriter
- {
- public:
-
- using DiskReaderWriter::DiskReaderWriter;
-
- FileSystemLookupMemory readMemoryStructureFromDisk();
- FileSystemLookupMemory readMemoryStructureFromDisk(const std::string&);
-
- EntityInstance readSingleInstanceFromDisk(const std::filesystem::path&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr) const;
- EntityInstance readSingleInstanceFromDisk(const MemoryID&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr) const;
-
- aron::typenavigator::ObjectNavigatorPtr readSingleTypeInformationFromDisk(const std::filesystem::path&) const;
- aron::typenavigator::ObjectNavigatorPtr readSingleTypeInformationFromDisk(const MemoryID&) const;
-
- protected:
- EntityInstance unwrapData(const aron::datanavigator::DictNavigatorPtr&) const;
- aron::typenavigator::ObjectNavigatorPtr unwrapType(const aron::typenavigator::ObjectNavigatorPtr&) const;
-
- virtual aron::datanavigator::DictNavigatorPtr getStringAsDataNavigator(const std::string&, const aron::typenavigator::NavigatorPtr&) const = 0;
- virtual aron::typenavigator::ObjectNavigatorPtr getStringAsTypeNavigator(const std::string& s) const = 0;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.cpp b/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.cpp
deleted file mode 100644
index 3742996cda7cf4f03a15615a5ba7a296dfeba591..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#include "NlohmannJSONDiskReader.h"
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.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>
-
-
-namespace armarx::armem::io
-{
-
- NlohmannJSONDiskReader::NlohmannJSONDiskReader(bool createFolder) :
- DiskReaderWriter(createFolder),
- DiskReader(createFolder)
- {
- }
-
- NlohmannJSONDiskReader::NlohmannJSONDiskReader(const std::string& rootPath, bool createFolder) :
- DiskReaderWriter(rootPath, createFolder),
- DiskReader(rootPath, createFolder)
- {
- }
-
- aron::datanavigator::DictNavigatorPtr NlohmannJSONDiskReader::getStringAsDataNavigator(const std::string& s, const aron::typenavigator::NavigatorPtr& expectedStructure) const
- {
- aron::dataIO::reader::NlohmannJSONReader dataReader(s);
- aron::dataIO::writer::NavigatorWriter navWriter;
- aron::dataIO::Converter::ConvertFromReader(dataReader, navWriter, expectedStructure);
- return aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
- }
-
- aron::typenavigator::ObjectNavigatorPtr NlohmannJSONDiskReader::getStringAsTypeNavigator(const std::string& s) const
- {
- aron::typeIO::reader::NlohmannJSONReader typeReader(s);
- aron::typeIO::writer::NavigatorWriter navWriter;
- aron::typeIO::Converter::ConvertFromReader(typeReader, navWriter);
- return aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(navWriter.getResult());
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h b/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
deleted file mode 100644
index 6358d751b07fe395de0345bf06aa4af374b85289..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include "../DiskReader.h"
-
-
-namespace armarx::armem::io
-{
- class NlohmannJSONDiskReader;
- using NlohmannJSONDiskReaderPtr = std::shared_ptr<NlohmannJSONDiskReader>;
-
- class NlohmannJSONDiskReader : virtual public DiskReader
- {
- public:
- NlohmannJSONDiskReader(bool createFolder);
- NlohmannJSONDiskReader(const std::string& rootPath, bool createFolder);
-
-
- protected:
- aron::datanavigator::DictNavigatorPtr getStringAsDataNavigator(
- const std::string&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr) const override;
- aron::typenavigator::ObjectNavigatorPtr getStringAsTypeNavigator(const std::string& s) const override;
-
- std::string getEntityInstanceSuffix() const override
- {
- return ".data.json";
- }
-
- std::string getTypeSuffix() const override
- {
- return ".type.json";
- }
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.cpp b/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.cpp
deleted file mode 100644
index 0929a3fc4d85a0efa44178cca02d218104956d48..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#include "DiskReaderWriter.h"
-
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::io
-{
-
- DiskReaderWriter::DiskReaderWriter(bool createIfNotExistent) :
- DiskReaderWriter("/tmp/MemoryExport", createIfNotExistent)
- {}
-
- DiskReaderWriter::DiskReaderWriter(const std::filesystem::path& rootPath, bool createIfNotExistent) :
- rootPath(rootPath)
- {
- if (!std::filesystem::exists(rootPath.parent_path()))
- {
- throw error::PathDoesNotExist(rootPath.parent_path().string(),
- "The parent path does not exist. At least this directory must exist!");
- }
-
- if (!std::filesystem::is_directory(rootPath.parent_path()))
- {
- throw error::PathDoesNotExist(rootPath.parent_path().string(), "The parent path is not a directory. It must be a directory.");
- }
-
- if (!std::filesystem::exists(rootPath))
- {
- if (createIfNotExistent)
- {
- std::filesystem::create_directory(rootPath);
- }
- else
- {
- throw error::PathDoesNotExist(rootPath.string(), "The path does not exist and a new folder should not be created!");
- }
- }
-
- if (!std::filesystem::is_directory(rootPath))
- {
- throw error::PathNotADirectory(rootPath.string(), "The Path is not valid. It points to a file instead of a folder!");
- }
- }
-
- DiskReaderWriter::~DiskReaderWriter()
- {
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.h b/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.h
deleted file mode 100644
index eaed6011e162b5244e8ed4f97877d2b43c4f53c9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskReaderWriter.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <string>
-#include <filesystem>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-
-namespace armarx::armem::io
-{
- class DiskReaderWriter
- {
- public:
- DiskReaderWriter(bool createIfNotExistent);
- DiskReaderWriter(const std::filesystem::path& rootPath, bool createIfNotExistent);
-
- virtual ~DiskReaderWriter();
-
-
- protected:
- virtual std::string getEntityInstanceSuffix() const = 0;
- virtual std::string getTypeSuffix() const = 0;
-
- protected:
- std::filesystem::path rootPath;
-
- const std::string DISK_READER_WRITER_DATA_FIELD = "DATA";
- const std::string DISK_READER_WRITER_TIME_STORED_FIELD = "STORAGE_MANAGER_METADATA__TIME_STORED";
- const std::string DISK_READER_WRITER_TIME_CREATED_FIELD = "ENTITY_METADATA__TIME_CREATED";
- const std::string DISK_READER_WRITER_TIME_SENT_FIELD = "ENTITY_METADATA__TIME_SENT";
- const std::string DISK_READER_WRITER_TIME_ARRIVED_FIELD = "ENTITY_METADATA__TIME_ARRIVED";
- const std::string DISK_READER_WRITER_CONFIDENCE_FIELD = "ENTITY_METADATA__CONFIDENCE";
-
- };
-
- using DiskReaderWriterPtr = std::shared_ptr<DiskReaderWriter>;
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp b/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp
deleted file mode 100644
index 9168f8adf64313f17a21700a50d93113db60954c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp
+++ /dev/null
@@ -1,303 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include "DiskWriter.h"
-
-#include <fstream>
-#include <iostream>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/Writer.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-
-
-// BaseClass
-#include "../DiskReaderWriter.h"
-#include "../FileSystemLookupMemory.h"
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/core/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/Entity.h>
-
-
-namespace armarx::armem::io
-{
-
- DiskWriterReturnInformation DiskWriter::writeOnDisk(const Memory& m)
- {
- MemoryID id = m.id().getMemoryID();
- DiskWriterReturnInformation ret(id.memoryName);
-
- std::filesystem::path p = rootPath / id.str();
- if (!ensureDirectoryPathExists(p))
- {
- ret.hasError = true;
- return ret;
- }
-
- for (const auto& [coreKey, coreSegment] : m.coreSegments)
- {
- MemoryID storeCore(id);
- storeCore.coreSegmentName = coreKey;
- DiskWriterReturnInformation other = writeOnDisk(storeCore, coreSegment);
- ret.append(other);
- }
- return ret;
- }
-
- DiskWriterReturnInformation DiskWriter::writeOnDisk(const MemoryID& i, const CoreSegment& s)
- {
- MemoryID id = i.getCoreSegmentID();
- DiskWriterReturnInformation ret(id.memoryName);
-
- std::filesystem::path p = rootPath / id.str();
- if (!ensureDirectoryPathExists(p))
- {
- ret.coreSegmentsError.push_back(id.str());
- ret.hasError = true;
- return ret;
- }
-
- if (s.hasAronType())
- {
- std::string val = getTypeAsString(wrapType(s.aronType()));
-
- std::filesystem::path coreSegmentProviderPath = rootPath / id.str() / (id.coreSegmentName + getTypeSuffix());
- std::ofstream ofs;
- ofs.open(coreSegmentProviderPath);
- ofs << val;
- ofs.close();
- }
-
- for (const auto& [providerKey, providerSegment] : s.providerSegments)
- {
- MemoryID storeProvider(id);
- storeProvider.providerSegmentName = providerKey;
- DiskWriterReturnInformation other = writeOnDisk(storeProvider, providerSegment);
- ret.append(other);
- }
- return ret;
- }
-
- DiskWriterReturnInformation DiskWriter::writeOnDisk(const MemoryID& i, const ProviderSegment& s)
- {
- MemoryID id = i.getProviderSegmentID();
- DiskWriterReturnInformation ret(id.memoryName);
-
- std::filesystem::path p = rootPath / id.str();
- if (!ensureDirectoryPathExists(p))
- {
- ret.providerSegmentsError.push_back(id.str());
- ret.hasError = true;
- return ret;
- }
-
- if (s.hasAronType())
- {
- std::string val = getTypeAsString(wrapType(s.aronType()));
-
- std::filesystem::path providerSegmentProviderPath = rootPath / id.str() / (id.providerSegmentName + getTypeSuffix());
- std::ofstream ofs;
- ofs.open(providerSegmentProviderPath);
- ofs << val;
- ofs.close();
- }
-
- for (const auto& [entityKey, entity] : s.entities)
- {
- MemoryID storeEntity(id);
- storeEntity.entityName = entityKey;
- DiskWriterReturnInformation other = writeOnDisk(storeEntity, entity);
- ret.append(other);
- }
- return ret;
- }
-
- DiskWriterReturnInformation DiskWriter::writeOnDisk(const MemoryID& i, const Entity& s)
- {
- MemoryID id = i.getEntityID();
- DiskWriterReturnInformation ret(id.memoryName);
-
- std::filesystem::path p = rootPath / id.str();
- if (!ensureDirectoryPathExists(p))
- {
- ret.entitiesError.push_back(id.str());
- ret.hasError = true;
- return ret;
- }
-
- for (const auto& [ts, snapshot] : s.history)
- {
- MemoryID storeSnapshot(id);
- storeSnapshot.timestamp = ts;
- DiskWriterReturnInformation other = writeOnDisk(storeSnapshot, snapshot);
- ret.append(other);
- }
- return ret;
- }
-
- DiskWriterReturnInformation DiskWriter::writeOnDisk(const MemoryID& _id, const EntitySnapshot& s)
- {
- MemoryID id = _id.getEntitySnapshotID();
- DiskWriterReturnInformation ret(id.memoryName);
-
- std::filesystem::path p = rootPath / id.str();
- if (!ensureDirectoryPathExists(p))
- {
- ret.historyTimestampsError.push_back(id.str());
- ret.hasError = true;
- return ret;
- }
-
- int i = 0;
- for (const EntityInstance& instance : s.instances)
- {
- MemoryID storeInstance(id);
- storeInstance.instanceIndex = i;
-
- std::filesystem::path entityElementPath = rootPath / (storeInstance.str() + getEntityInstanceSuffix());
- std::string val = getDataAsString(wrapData(instance));
-
- if (filePathExists(entityElementPath))
- {
- if (std::filesystem::is_directory(entityElementPath))
- {
- ret.entityInstancesError.push_back(id.str());
- ret.hasError = true;
- return ret;
- }
- //std::ifstream ifs(entityElementPath);
- //std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
- //if (file_content == val)
- //{
- // // already written. skip
- // continue;
- //}
- }
-
- std::ofstream ofs;
- ofs.open(entityElementPath);
- ofs << val;
- ofs.close();
- ret.storedElements.memoryLookupTable[id.memoryName]
- [id.coreSegmentName]
- [id.providerSegmentName]
- [id.entityName]
- [id.timestamp].push_back(entityElementPath);
-
- i++;
- }
- return ret;
- }
-
- aron::datanavigator::DictNavigatorPtr DiskWriter::wrapData(const EntityInstance& e) const
- {
- aron::datanavigator::DictNavigatorPtr dataWrapped(new aron::datanavigator::DictNavigator());
- dataWrapped->addElement(DISK_READER_WRITER_DATA_FIELD, e.data());
-
- aron::datanavigator::LongNavigatorPtr timeWrapped(new aron::datanavigator::LongNavigator());
- timeWrapped->setValue(Time::now().toMicroSeconds());
- dataWrapped->addElement(DISK_READER_WRITER_TIME_STORED_FIELD, timeWrapped);
-
-
- const EntityInstanceMetadata& metadata = e.metadata();
- aron::datanavigator::LongNavigatorPtr timeCreated(new aron::datanavigator::LongNavigator());
- timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
- dataWrapped->addElement(DISK_READER_WRITER_TIME_CREATED_FIELD, timeCreated);
-
- aron::datanavigator::LongNavigatorPtr timeSent(new aron::datanavigator::LongNavigator());
- timeSent->setValue(metadata.timeSent.toMicroSeconds());
- dataWrapped->addElement(DISK_READER_WRITER_TIME_SENT_FIELD, timeSent);
-
- aron::datanavigator::LongNavigatorPtr timeArrived(new aron::datanavigator::LongNavigator());
- timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
- dataWrapped->addElement(DISK_READER_WRITER_TIME_ARRIVED_FIELD, timeArrived);
-
- aron::datanavigator::DoubleNavigatorPtr confidence(new aron::datanavigator::DoubleNavigator());
- confidence->setValue(metadata.confidence);
- dataWrapped->addElement(DISK_READER_WRITER_CONFIDENCE_FIELD, confidence);
-
- return dataWrapped;
- }
-
- aron::typenavigator::ObjectNavigatorPtr DiskWriter::wrapType(const aron::typenavigator::ObjectNavigatorPtr& t) const
- {
- aron::typenavigator::ObjectNavigatorPtr typeWrapped(new aron::typenavigator::ObjectNavigator());
- typeWrapped->setObjectName(t->getName() + "__ltm_export");
- typeWrapped->addMemberType(DISK_READER_WRITER_DATA_FIELD, t);
-
- typeWrapped->addMemberType(DISK_READER_WRITER_TIME_STORED_FIELD, aron::typenavigator::LongNavigatorPtr(new aron::typenavigator::LongNavigator()));
- typeWrapped->addMemberType(DISK_READER_WRITER_TIME_CREATED_FIELD, aron::typenavigator::LongNavigatorPtr(new aron::typenavigator::LongNavigator()));
- typeWrapped->addMemberType(DISK_READER_WRITER_TIME_SENT_FIELD, aron::typenavigator::LongNavigatorPtr(new aron::typenavigator::LongNavigator()));
- typeWrapped->addMemberType(DISK_READER_WRITER_TIME_ARRIVED_FIELD, aron::typenavigator::LongNavigatorPtr(new aron::typenavigator::LongNavigator()));
- typeWrapped->addMemberType(DISK_READER_WRITER_CONFIDENCE_FIELD, aron::typenavigator::DoubleNavigatorPtr(new aron::typenavigator::DoubleNavigator()));
-
- return typeWrapped;
- }
-
- bool DiskWriter::directoryPathExists(const std::filesystem::path& p) const
- {
- return std::filesystem::exists(p) && std::filesystem::is_directory(p);
- }
-
- bool DiskWriter::ensureDirectoryPathExists(const std::filesystem::path& p) const
- {
- if (!std::filesystem::exists(p))
- {
- return std::filesystem::create_directory(p);
- }
- return directoryPathExists(p);
- }
-
- bool DiskWriter::filePathExists(const std::filesystem::path& p) const
- {
- return std::filesystem::exists(p) && std::filesystem::is_regular_file(p);
- }
-
-
- io::DiskWriterReturnInformation::DiskWriterReturnInformation(const std::string& m) :
- memoryName(m)
- {
- }
-
- void io::DiskWriterReturnInformation::append(const io::DiskWriterReturnInformation& o)
- {
- if (memoryName != o.memoryName)
- {
- // The memory name must be equal
- return;
- }
-
- storedElements.merge(o.storedElements);
-
- coreSegmentsError.insert(coreSegmentsError.end(), o.coreSegmentsError.begin(), o.coreSegmentsError.end());
- providerSegmentsError.insert(providerSegmentsError.end(), o.providerSegmentsError.begin(), o.providerSegmentsError.end());
- entitiesError.insert(entitiesError.end(), o.entitiesError.begin(), o.entitiesError.end());
- historyTimestampsError.insert(historyTimestampsError.end(), o.historyTimestampsError.begin(), o.historyTimestampsError.end());
- entityInstancesError.insert(entityInstancesError.end(), o.entityInstancesError.begin(), o.entityInstancesError.end());
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.h b/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.h
deleted file mode 100644
index c8b4e4541e341e23bab18f45cb956620178771c8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-#include <filesystem>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-
-// BaseClass
-#include "../DiskReaderWriter.h"
-#include "../FileSystemLookupMemory.h"
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-
-
-namespace armarx::armem::io
-{
- struct DiskWriterReturnInformation;
-
-
- class DiskWriter;
- using DiskWriterPtr = std::shared_ptr<DiskWriter>;
-
- class DiskWriter : virtual public DiskReaderWriter
- {
- public:
-
- using DiskReaderWriter::DiskReaderWriter;
-
- DiskWriterReturnInformation writeOnDisk(const Memory& m);
- DiskWriterReturnInformation writeOnDisk(const MemoryID&, const CoreSegment&);
- DiskWriterReturnInformation writeOnDisk(const MemoryID&, const ProviderSegment&);
- DiskWriterReturnInformation writeOnDisk(const MemoryID&, const Entity&);
- DiskWriterReturnInformation writeOnDisk(const MemoryID&, const EntitySnapshot&);
-
- protected:
- aron::datanavigator::DictNavigatorPtr wrapData(const EntityInstance& e) const;
- aron::typenavigator::ObjectNavigatorPtr wrapType(const aron::typenavigator::ObjectNavigatorPtr&) const;
-
- virtual std::string getDataAsString(const aron::datanavigator::DictNavigatorPtr&) const = 0;
- virtual std::string getTypeAsString(const aron::typenavigator::ObjectNavigatorPtr&) const = 0;
-
- bool directoryPathExists(const std::filesystem::path& p) const;
- bool ensureDirectoryPathExists(const std::filesystem::path& p) const;
-
- bool filePathExists(const std::filesystem::path& p) const;
- };
-
-
- struct DiskWriterReturnInformation
- {
- std::string memoryName = "";
- bool hasError = false;
-
- FileSystemLookupMemory storedElements;
- std::vector<std::string> coreSegmentsError;
- std::vector<std::string> providerSegmentsError;
- std::vector<std::string> entitiesError;
- std::vector<std::string> historyTimestampsError;
- std::vector<std::string> entityInstancesError;
-
- DiskWriterReturnInformation(const std::string& m);
-
- void append(const DiskWriterReturnInformation& o);
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.cpp b/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.cpp
deleted file mode 100644
index a52253d8cdfe867195e3f595832746a7e312f0fc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include "NlohmannJSONDiskWriter.h"
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
-namespace armarx::armem::io
-{
- NlohmannJSONDiskWriter::NlohmannJSONDiskWriter(bool createFolder) :
- DiskReaderWriter(createFolder),
- DiskWriter(createFolder)
- {
- }
-
- NlohmannJSONDiskWriter::NlohmannJSONDiskWriter(const std::string& rootPath, bool createFolder) :
- DiskReaderWriter(rootPath, createFolder),
- DiskWriter(rootPath, createFolder)
- {
- }
-
- std::string NlohmannJSONDiskWriter::getDataAsString(const aron::datanavigator::DictNavigatorPtr& aronDataNav) const
- {
- aron::dataIO::writer::NlohmannJSONWriter dataWriter;
- aron::dataIO::Visitor::SetupWriterFromAronDataPtr(dataWriter, aronDataNav->getResult());
- return dataWriter.getResult().dump(2);
- }
-
- std::string NlohmannJSONDiskWriter::getTypeAsString(const aron::typenavigator::ObjectNavigatorPtr& aronTypeNav) const
- {
- aron::typeIO::writer::NlohmannJSONWriter typeWriter;
- aron::typeIO::Visitor::SetupWriterFromAronTypePtr(typeWriter, aronTypeNav->getResult());
- return typeWriter.getResult().dump(2);
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.h b/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.h
deleted file mode 100644
index 67d35f01aa874f3c203d4ffffd5bd5691b237bb0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include "../DiskWriter.h"
-
-
-namespace armarx::armem::io
-{
- class NlohmannJSONDiskWriter;
- using NlohmannJSONDiskWriterPtr = std::shared_ptr<NlohmannJSONDiskWriter>;
-
- class NlohmannJSONDiskWriter : virtual public DiskWriter
- {
- public:
- NlohmannJSONDiskWriter(bool createFolder);
- NlohmannJSONDiskWriter(const std::string& rootPath, bool createFolder);
-
-
- protected:
- std::string getDataAsString(const aron::datanavigator::DictNavigatorPtr&) const override;
- std::string getTypeAsString(const aron::typenavigator::ObjectNavigatorPtr&) const override;
-
- std::string getEntityInstanceSuffix() const override
- {
- return ".data.json";
- }
-
- std::string getTypeSuffix() const override
- {
- return ".type.json";
- }
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.cpp b/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.cpp
deleted file mode 100644
index ea9dc07dd048426f3cf4d57fa22790b27b15921d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.cpp
+++ /dev/null
@@ -1,217 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#include "FileSystemLookupMemory.h"
-
-
-namespace armarx::armem::io
-{
-
- DiskMemory FileSystemLookupMemory::getDiskMemory(const std::string& n) const
- {
- if (!hasMemory(n))
- {
- throw error::ArMemError("Could not find a memory with name '" + n + "' in lookup memory.");
- }
- return memoryLookupTable.at(n);
- }
-
- DiskMemory FileSystemLookupMemory::getDiskMemory(const MemoryID& id) const
- {
- if (!hasMemory(id))
- {
- throw error::InvalidMemoryID(id, "MemoryName does not exist in lookup memory.");
- }
- return memoryLookupTable.at(id.memoryName);
- }
-
- DiskCoreSegment FileSystemLookupMemory::getDiskCoreSegment(const MemoryID& id) const
- {
- if (!hasCoreSegment(id))
- {
- throw error::InvalidMemoryID(id, "CoreSegmentName does not exist in lookup memory.");
- }
- return getDiskMemory(id).at(id.coreSegmentName);
- }
-
- DiskProviderSegment FileSystemLookupMemory::getDiskProviderSegment(const MemoryID& id) const
- {
- if (!hasProviderSegment(id))
- {
- throw error::InvalidMemoryID(id, "ProviderSegmentName does not exist in lookup memory.");
- }
- return getDiskCoreSegment(id).at(id.providerSegmentName);
- }
-
- DiskEntity FileSystemLookupMemory::getDiskEntity(const MemoryID& id) const
- {
- if (!hasEntity(id))
- {
- throw error::InvalidMemoryID(id, "EntityName does not exist in lookup memory.");
- }
- return getDiskProviderSegment(id).at(id.entityName);
- }
-
- DiskEntitySnapshot FileSystemLookupMemory::getDiskEntitySnapshot(const MemoryID& id) const
- {
- if (!hasEntitySnapshot(id))
- {
- throw error::InvalidMemoryID(id, "EntitySnapshotTimestamp does not exist in lookup memory.");
- }
- return getDiskEntity(id).at(id.timestamp);
- }
-
- DiskEntityInstance FileSystemLookupMemory::getDiskEntityInstance(const MemoryID& id) const
- {
- if (!hasEntityInstance(id))
- {
- throw error::InvalidMemoryID(id, "EntityInstanceIndex does not exist in lookup memory.");
- }
- return getDiskEntitySnapshot(id).at(id.instanceIndex);
- }
-
-
- bool FileSystemLookupMemory::hasMemory(const std::string& n) const
- {
- MemoryID id;
- id.memoryName = n;
- return hasMemory(id);
- }
-
- bool FileSystemLookupMemory::hasMemory(const MemoryID& id) const
- {
- return id.hasMemoryName() && memoryLookupTable.find(id.memoryName) != memoryLookupTable.end();
- }
-
- bool FileSystemLookupMemory::hasCoreSegment(const MemoryID& id) const
- {
- if (hasMemory(id))
- {
- const auto& m = getDiskMemory(id);
- return id.hasCoreSegmentName() && m.find(id.coreSegmentName) != m.end();
- }
- return false;
- }
-
- bool FileSystemLookupMemory::hasProviderSegment(const MemoryID& id) const
- {
- if (hasCoreSegment(id))
- {
- const auto& m = getDiskCoreSegment(id);
- return id.hasProviderSegmentName() && m.find(id.providerSegmentName) != m.end();
- }
- return false;
- }
-
- bool FileSystemLookupMemory::hasEntity(const MemoryID& id) const
- {
- if (hasProviderSegment(id))
- {
- const auto& m = getDiskProviderSegment(id);
- return id.hasEntityName() && m.find(id.entityName) != m.end();
- }
- return false;
- }
-
- bool FileSystemLookupMemory::hasEntitySnapshot(const MemoryID& id) const
- {
- if (hasEntity(id))
- {
- const auto& m = getDiskEntity(id);
- return id.hasTimestamp() && m.find(id.timestamp) != m.end();
- }
- return false;
- }
-
- bool FileSystemLookupMemory::hasEntityInstance(const MemoryID& id) const
- {
- if (hasEntitySnapshot(id))
- {
- const auto& m = getDiskEntitySnapshot(id);
- return id.hasInstanceIndex() && m.size() >= (size_t) id.instanceIndex;
- }
- return false;
- }
-
-
- void FileSystemLookupMemory::merge(const FileSystemLookupMemory& m)
- {
- for (const auto& [memoryName, memory] : m.memoryLookupTable)
- {
- if (memoryLookupTable.find(memoryName) == memoryLookupTable.end())
- {
- memoryLookupTable[memoryName] = memory;
- continue;
- }
-
- DiskMemory& lutMemory = memoryLookupTable[memoryName];
- for (const auto& [coreKey, coreSegment] : memory)
- {
- if (lutMemory.find(coreKey) == lutMemory.end())
- {
- lutMemory[coreKey] = coreSegment;
- continue;
- }
-
- DiskCoreSegment& lutCoreSegment = lutMemory[coreKey];
- for (const auto& [providerKey, providerSegment] : coreSegment)
- {
- if (lutCoreSegment.find(providerKey) == lutCoreSegment.end())
- {
- lutCoreSegment[providerKey] = providerSegment;
- continue;
- }
-
- DiskProviderSegment& lutProviderSegment = lutCoreSegment[providerKey];
- for (const auto& [entityKey, entity] : providerSegment)
- {
- if (lutProviderSegment.find(entityKey) == lutProviderSegment.end())
- {
- lutProviderSegment[entityKey] = entity;
- continue;
- }
-
- DiskEntity& lutEntity = lutProviderSegment[entityKey];
- for (const auto& [entityHistoryTimestamp, entitySnapshot] : entity)
- {
- if (lutEntity.find(entityHistoryTimestamp) == lutEntity.end())
- {
- lutEntity[entityHistoryTimestamp] = entitySnapshot;
- continue;
- }
-
- DiskEntitySnapshot& lutEntitySnapshot = lutEntity[entityHistoryTimestamp];
- //for (unsigned int i = 0; i < entitySnapshot.size(); ++i)
- //{
- // // clear internal stored memory of that snapshot
- // internalMemoryEntitySnapshot.clear();
- // internalMemoryEntitySnapshot.push_back(entitySnapshot[i]);
- //}
- // for now, simply overwrite all values since one history timestamp should not have two different snapshots
- lutEntitySnapshot = entitySnapshot;
- }
- }
- }
- }
- }
- }
-
-
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.h b/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.h
deleted file mode 100644
index c3aa952f6e7895221db20f157f6fee83796722ea..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/FileSystemLookupMemory.h
+++ /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/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <filesystem>
-#include <map>
-#include <string>
-#include <vector>
-
-#include "../Time.h"
-#include "../Memory.h"
-#include "../error.h"
-
-
-namespace armarx::armem::io
-{
- using DiskEntityInstance = std::filesystem::path; // absolute paths to stored files
- using DiskEntitySnapshot = std::vector<DiskEntityInstance>;
- using DiskEntity = std::map<Time, DiskEntitySnapshot>;
- using DiskProviderSegment = std::map<std::string, DiskEntity>;
- using DiskCoreSegment = std::map<std::string, DiskProviderSegment>;
- using DiskMemory = std::map<std::string, DiskCoreSegment>;
-
-
- class FileSystemLookupMemory
- {
- public:
- FileSystemLookupMemory() = default;
- void merge(const FileSystemLookupMemory& m);
-
- DiskMemory getDiskMemory(const std::string&) const;
- DiskMemory getDiskMemory(const MemoryID&) const;
- DiskCoreSegment getDiskCoreSegment(const MemoryID&) const;
- DiskProviderSegment getDiskProviderSegment(const MemoryID&) const;
- DiskEntity getDiskEntity(const MemoryID&) const;
- DiskEntitySnapshot getDiskEntitySnapshot(const MemoryID&) const;
- DiskEntityInstance getDiskEntityInstance(const MemoryID&) const;
-
- bool hasMemory(const std::string&) const;
- bool hasMemory(const MemoryID&) const;
- bool hasCoreSegment(const MemoryID&) const;
- bool hasProviderSegment(const MemoryID&) const;
- bool hasEntity(const MemoryID&) const;
- bool hasEntitySnapshot(const MemoryID&) const;
- bool hasEntityInstance(const MemoryID&) const;
-
- std::map<std::string, DiskMemory> memoryLookupTable;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.cpp b/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.cpp
deleted file mode 100644
index d39acab1af52a9ed4ec40b01d887a9168960df80..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.cpp
+++ /dev/null
@@ -1,291 +0,0 @@
-#include "MemoryFileSystemStorage.h"
-
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::io
-{
- FileSystemMemoryManager::FileSystemMemoryManager(const std::filesystem::path& rootPath, bool createFolder) :
- FileSystemMemoryManager(std::make_shared<io::NlohmannJSONDiskWriter>(rootPath, createFolder),
- std::make_shared<io::NlohmannJSONDiskReader>(rootPath, createFolder))
- {
- }
-
- FileSystemMemoryManager::FileSystemMemoryManager(const io::DiskWriterPtr& w, const io::DiskReaderPtr& r) :
- writer(w),
- reader(r)
- {
- }
-
- void FileSystemMemoryManager::writeOnDisk(const Memory& m)
- {
- io::DiskWriterReturnInformation info = writer->writeOnDisk(m);
- merge(info.storedElements);
- }
-
- void FileSystemMemoryManager::writeOnDisk(const MemoryID& id, const CoreSegment& s)
- {
- io::DiskWriterReturnInformation info = writer->writeOnDisk(id, s);
- merge(info.storedElements);
- }
-
- void FileSystemMemoryManager::writeOnDisk(const MemoryID& id, const ProviderSegment& s)
- {
- io::DiskWriterReturnInformation info = writer->writeOnDisk(id, s);
- merge(info.storedElements);
- }
-
- void FileSystemMemoryManager::writeOnDisk(const MemoryID& id, const Entity& s)
- {
- io::DiskWriterReturnInformation info = writer->writeOnDisk(id, s);
- merge(info.storedElements);
- }
-
- void FileSystemMemoryManager::writeOnDisk(const MemoryID& id, const EntitySnapshot& s)
- {
- io::DiskWriterReturnInformation info = writer->writeOnDisk(id, s);
- merge(info.storedElements);
- }
-
- Memory FileSystemMemoryManager::readMemoryFromDisk(const std::string& n) const
- {
- MemoryID id;
- id.memoryName = n;
- return readMemoryFromDisk(id);
- }
-
- Memory FileSystemMemoryManager::readMemoryFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasMemoryName())
- {
- throw error::InvalidMemoryID(_id, "MemoryName is missing.");
- }
- Memory ret(_id.memoryName);
- if (internalMemory.hasMemory(_id))
- {
- MemoryID id = _id.getMemoryID();
- for (const auto& [coreKey, _] : internalMemory.getDiskMemory(_id))
- {
- id.coreSegmentName = coreKey;
- CoreSegment s = readCoreSegmentFromDisk(id);
- ret.addCoreSegment(s);
- }
- }
- return ret;
- }
-
- CoreSegment FileSystemMemoryManager::readCoreSegmentFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasCoreSegmentName())
- {
- throw error::InvalidMemoryID(_id, "CoreSegmentName is missing.");
- }
-
- CoreSegment ret(_id.coreSegmentName);
- if (internalMemory.hasCoreSegment(_id))
- {
- MemoryID id = _id.getCoreSegmentID();
- for (const auto& [providerKey, _] : internalMemory.getDiskCoreSegment(_id))
- {
- id.providerSegmentName = providerKey;
- ProviderSegment s = readProviderSegmentFromDisk(id);
- ret.addProviderSegment(s);
- }
- }
- return ret;
- }
-
- ProviderSegment FileSystemMemoryManager::readProviderSegmentFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasProviderSegmentName())
- {
- throw error::InvalidMemoryID(_id, "ProviderSegmentName is missing.");
- }
-
- ProviderSegment ret(_id.providerSegmentName);
- if (internalMemory.hasProviderSegment(_id))
- {
- MemoryID id = _id.getProviderSegmentID();
- for (const auto& [entityKey, _] : internalMemory.getDiskProviderSegment(_id))
- {
- id.entityName = entityKey;
- Entity s = readEntityFromDisk(id);
- ret.addEntity(s);
- }
- }
- return ret;
- }
-
- Entity FileSystemMemoryManager::readEntityFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasEntityName())
- {
- throw error::InvalidMemoryID(_id, "EntityName is missing.");
- }
-
- Entity ret(_id.entityName);
- if (internalMemory.hasEntity(_id))
- {
- MemoryID id = _id.getEntityID();
- for (const auto& [ts, _] : internalMemory.getDiskEntity(_id))
- {
- id.timestamp = ts;
- EntitySnapshot s = readEntitySnapshotFromDisk(id);
- ret.addSnapshot(s);
- }
- }
- return ret;
- }
-
- EntitySnapshot FileSystemMemoryManager::readEntitySnapshotFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasTimestamp())
- {
- throw error::InvalidMemoryID(_id, "Timestamp is missing.");
- }
-
- EntitySnapshot ret(_id.timestamp);
- if (internalMemory.hasEntitySnapshot(_id))
- {
- MemoryID id = _id.getEntitySnapshotID();
- for (unsigned int i = 0; i < internalMemory.getDiskEntitySnapshot(_id).size(); ++i)
- {
- id.instanceIndex = static_cast<int>(i);
- EntityInstance s = readEntityInstanceFromDisk(id);
- ret.addInstance(s);
- }
- }
- return ret;
- }
-
- EntityInstance FileSystemMemoryManager::readEntityInstanceFromDisk(const MemoryID& _id) const
- {
- if (!_id.hasInstanceIndex())
- {
- throw error::InvalidMemoryID(_id, "InstanceIndex is missing.");
- }
-
- EntityInstance ret(_id.instanceIndex);
- if (internalMemory.hasEntityInstance(_id))
- {
- aron::typenavigator::ObjectNavigatorPtr typeInformation = readClosestTypeInformation(_id);
- const DiskEntityInstance& path = internalMemory.getDiskEntityInstance(_id);
- return reader->readSingleInstanceFromDisk(path, typeInformation);
- }
- return ret;
- }
-
- aron::typenavigator::ObjectNavigatorPtr FileSystemMemoryManager::readClosestTypeInformation(const MemoryID& _id) const
- {
- if (_id.hasInstanceIndex())
- {
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(_id);
- if (t != nullptr)
- {
- return t;
- }
- }
- if (_id.hasTimestamp())
- {
- MemoryID id = _id.getEntitySnapshotID();
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(id);
- if (t != nullptr)
- {
- return t;
- }
- }
- if (_id.hasEntityName())
- {
- MemoryID id = _id.getEntityID();
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(id);
- if (t != nullptr)
- {
- return t;
- }
- }
- if (_id.hasProviderSegmentName())
- {
- MemoryID id = _id.getProviderSegmentID();
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(id);
- if (t != nullptr)
- {
- return t;
- }
- }
- if (_id.hasCoreSegmentName())
- {
- MemoryID id = _id.getCoreSegmentID();
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(id);
- if (t != nullptr)
- {
- return t;
- }
- }
- if (_id.hasMemoryName())
- {
- MemoryID id = _id.getMemoryID();
- aron::typenavigator::ObjectNavigatorPtr t = reader->readSingleTypeInformationFromDisk(id);
- if (t != nullptr)
- {
- return t;
- }
- }
-
- return nullptr;
- }
-
- void FileSystemMemoryManager::update()
- {
- FileSystemLookupMemory up = reader->readMemoryStructureFromDisk();
- merge(up);
- }
-
- std::string FileSystemMemoryManager::toString()
- {
- std::stringstream ss;
- std::string tabs = "";
- for (const auto& [memoryName, memory] : internalMemory.memoryLookupTable)
- {
- ss << memoryName << ": " << std::endl;
- std::string tabs = "\t";
- for (const auto& [coreKey, coreSegment] : memory)
- {
- ss << tabs << coreKey << ": " << std::endl;
- std::string tabs = "\t\t";
- for (const auto& [providerKey, providerSegment] : coreSegment)
- {
- ss << tabs << providerKey << ": " << std::endl;
- std::string tabs = "\t\t\t";
- for (const auto& [entityKey, entity] : providerSegment)
- {
- ss << tabs << entityKey << ": " << std::endl;
- std::string tabs = "\t\t\t\t";
- for (const auto& [entityHistoryTimestamp, entitySnapshot] : entity)
- {
- ss << tabs << entityHistoryTimestamp.toMicroSeconds() << ":" << std::endl;
- ss << tabs << "[" << std::endl;
- for (unsigned int i = 0; i < entitySnapshot.size(); ++i)
- {
- const std::string& newlyGenerate = entitySnapshot[i];
- ss << tabs << "\t(" << i << " => " << newlyGenerate << ")";
- if (i < entitySnapshot.size() - 1)
- {
- ss << ", ";
- }
- ss << std::endl;
- }
- ss << tabs << "]" << std::endl;
- }
- }
- }
- }
- }
- return ss.str();
- }
-
- void FileSystemMemoryManager::merge(const FileSystemLookupMemory& info)
- {
- internalMemory.merge(info);
- }
-}
diff --git a/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.h b/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.h
deleted file mode 100644
index 9f44bb7a037aeed8dcf32acda6c5b8374514c45c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <filesystem>
-#include <string>
-#include <utility> // std::pair
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-
-#include "DiskWriter/DiskWriter.h"
-#include "DiskWriter/NlohmannJSONDiskWriter/NlohmannJSONDiskWriter.h"
-#include "DiskReader/DiskReader.h"
-#include "DiskReader/NlohmannJSONDiskReader/NlohmannJSONDiskReader.h"
-
-
-namespace armarx::armem::io
-{
- class FileSystemMemoryManager
- {
- public:
- FileSystemMemoryManager() = delete;
- FileSystemMemoryManager(const std::filesystem::path& rootPath, bool createFolder = false);
- FileSystemMemoryManager(const io::DiskWriterPtr&, const io::DiskReaderPtr&);
-
- void writeOnDisk(const Memory& memory);
- void writeOnDisk(const MemoryID& coreSegmentID, const CoreSegment& coreSegment);
- void writeOnDisk(const MemoryID& providerSegmentID, const ProviderSegment& providerSegment);
- void writeOnDisk(const MemoryID& entityID, const Entity& entity);
- // void writeOnDisk(const MemoryID&, const Entity&, aron::typenavigator::ObjectNavigatorPtr = nullptr);
- void writeOnDisk(const MemoryID& entitySnapshotID, const EntitySnapshot& entitySnapshot);
-
- void update();
- void merge(const FileSystemLookupMemory& info);
-
- Memory readMemoryFromDisk(const std::string& memoryName) const;
- Memory readMemoryFromDisk(const MemoryID& memoryID) const;
- CoreSegment readCoreSegmentFromDisk(const MemoryID& coreSegmentID) const;
- ProviderSegment readProviderSegmentFromDisk(const MemoryID& providerSegmentID) const;
- Entity readEntityFromDisk(const MemoryID& entityID) const;
- EntitySnapshot readEntitySnapshotFromDisk(const MemoryID& entitySnapshotID) const;
- EntityInstance readEntityInstanceFromDisk(const MemoryID& entityInstanceID) const;
-
- aron::typenavigator::ObjectNavigatorPtr readClosestTypeInformation(const MemoryID&) const;
-
- //std::pair<Entity, aron::typenavigator::ObjectNavigatorPtr> readTypedEntityFromDisk();
- //std::pair<EntitySnapshot, aron::typenavigator::ObjectNavigatorPtr> readTypedEntitySnapshotFromDisk();
-
- std::string toString();
-
- private:
- FileSystemLookupMemory internalMemory;
-
- DiskWriterPtr writer;
- DiskReaderPtr reader;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78f275fe1a8b1285b852431acaa67dc13963874d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -0,0 +1,36 @@
+#include "CoreSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ wm::CoreSegment CoreSegment::convert() const
+ {
+ wm::CoreSegment m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addProviderSegment(s.convert());
+ }
+ return m;
+ }
+
+ void CoreSegment::append(const wm::CoreSegment& m)
+ {
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..67f9da6f465820534a4214cbb423e14916819daf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "../base/CoreSegmentBase.h"
+
+#include "ProviderSegment.h"
+
+#include "../workingmemory/CoreSegment.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ /**
+ * @brief Data of a core segment containing multiple provider segments.
+ */
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ using Base::CoreSegmentBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::CoreSegment convert() const;
+
+ // MongoDB connection
+ void append(const wm::CoreSegment&);
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15e232c01fb0edb2ce6e0bffec0f2f0b2c0a3110
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -0,0 +1,31 @@
+#include "Entity.h"
+
+namespace armarx::armem::ltm
+{
+
+ wm::Entity Entity::convert() const
+ {
+ wm::Entity m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addSnapshot(s.convert());
+ }
+ return m;
+ }
+
+ void Entity::append(const wm::Entity& m)
+ {
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..ecdf297c13fae4d24b070f2af13fd6c84ed93392
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "../base/EntityBase.h"
+
+#include "EntitySnapshot.h"
+
+#include "../workingmemory/Entity.h"
+
+
+namespace armarx::armem::ltm
+{
+ /**
+ * @brief An entity over a period of time.
+ *
+ * An entity should be a physical thing or abstract concept existing
+ * (and potentially evolving) over some time.
+ *
+ * Examples are:
+ * - objects (the green box)
+ * - agents (robot, human)
+ * - locations (frige, sink)
+ * - grasp affordances (general, or for a specific object)
+ * - images
+ * - point clouds
+ * - other sensory values
+ *
+ * At each point in time (`EntitySnapshot`), the entity can have a
+ * (potentially variable) number of instances (`EntityInstance`),
+ * each containing a single `AronData` object of a specific `AronType`.
+ */
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>
+ {
+ using Base = base::EntityBase<EntitySnapshot, Entity>;
+
+ public:
+
+ using Base::EntityBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::Entity convert() const;
+
+ // MongoDB connection
+ void append(const wm::Entity&);
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/EntityInstance.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
similarity index 55%
rename from source/RobotAPI/libraries/armem/core/EntityInstance.cpp
rename to source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
index 416e56d740acb7701557e8ade3b98a4d1eb410b4..6332be7ea52f3c547101ae3ee196a524a36aab17 100644
--- a/source/RobotAPI/libraries/armem/core/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.cpp
@@ -2,27 +2,20 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx::armem
+namespace armarx::armem::ltm
{
- EntityInstance::EntityInstance()
- {
- }
-
- EntityInstance::EntityInstance(int index, const MemoryID& parentID) :
- EntityInstance(parentID.withInstanceIndex(index))
- {
- }
-
- EntityInstance::EntityInstance(const MemoryID& id) : MemoryItem(id)
+ bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
{
+ return timeCreated == other.timeCreated
+ && timeSent == other.timeSent
+ && timeArrived == other.timeArrived
+ && std::abs(confidence - other.confidence) < 1e-6f;
}
-
bool EntityInstance::equalsDeep(const EntityInstance& other) const
{
- return _metadata == other.metadata() && _data->equalsDeep(other.data());
+ return id() == other.id() && _metadata == other.metadata();
}
void EntityInstance::update(const EntityUpdate& update, int index)
@@ -30,43 +23,34 @@ namespace armarx::armem
ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
this->index() = index;
- setData(update.instancesData.at(size_t(index)));
this->_metadata.confidence = update.confidence;
-
this->_metadata.timeCreated = update.timeCreated;
this->_metadata.timeSent = update.timeSent;
this->_metadata.timeArrived = update.timeArrived;
}
- EntityInstance EntityInstance::copy(bool withData) const
+ EntityInstance EntityInstance::copy() const
{
- EntityInstance other(id());
- other._metadata = _metadata;
- if (withData)
- {
- other.setData(_data);
- }
- return other;
+ EntityInstance d;
+ this->_copySelf(d);
+ return d;
}
- std::string EntityInstance::getKeyString() const
+ void EntityInstance::_copySelf(EntityInstance& other) const
{
- return std::to_string(index());
+ Base::_copySelf(other);
+ other._metadata = _metadata;
}
- std::string EntityInstance::getLevelName() const
+ wm::EntityInstance EntityInstance::convert() const
{
- return "entity instance";
+ wm::EntityInstance m;
+ return m;
}
-
- bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
+ void EntityInstance::setTo(const wm::EntityInstance& m)
{
- return timeCreated == other.timeCreated
- && timeSent == other.timeSent
- && timeArrived == other.timeArrived
- && std::abs(confidence - other.confidence) < 1e-6f;
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..f78668e5e1a4e1506c2b63c8d2721126e293d6c9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntityInstance.h
@@ -0,0 +1,86 @@
+#pragma once
+
+#include "../base/EntityInstanceBase.h"
+
+#include "../workingmemory/EntityInstance.h"
+
+namespace armarx::armem::ltm
+{
+
+ /**
+ * @brief Metadata of an entity instance.
+ */
+ struct EntityInstanceMetadata
+ {
+ /// Time when this value was created.
+ Time timeCreated;
+ /// Time when this value was sent to the memory.
+ Time timeSent;
+ /// Time when this value has arrived at the memory.
+ Time timeArrived;
+
+ /// An optional confidence, may be used for things like decay.
+ float confidence = 1.0;
+
+
+ bool operator==(const EntityInstanceMetadata& other) const;
+ inline bool operator!=(const EntityInstanceMetadata& other) const
+ {
+ return !(*this == other);
+ }
+ };
+
+ /**
+ * @brief Data of a single entity instance.
+ */
+ class EntityInstance :
+ public base::EntityInstanceBase<EntityInstance>
+ {
+ using Base = base::EntityInstanceBase<EntityInstance>;
+
+ public:
+
+ 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()
+ {
+ return _metadata;
+ }
+ inline const EntityInstanceMetadata& metadata() const
+ {
+ return _metadata;
+ }
+
+
+ /**
+ * @brief Fill `*this` with the update's values.
+ * @param update The update.
+ * @param index The instances index.
+ */
+ virtual void update(const EntityUpdate& update, int index) override;
+
+ virtual bool equalsDeep(const EntityInstance& other) const override;
+
+ virtual EntityInstance copy() const override;
+
+ // Conversion
+ wm::EntityInstance convert() const;
+
+ // MongoDB connection
+ void setTo(const wm::EntityInstance&);
+
+ protected:
+ virtual void _copySelf(EntityInstance& other) const override;
+
+
+ private:
+ /// The metadata.
+ EntityInstanceMetadata _metadata;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73d4932155562ffe213504ce3b7bccfc93690b51
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -0,0 +1,33 @@
+#include "EntitySnapshot.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ wm::EntitySnapshot EntitySnapshot::convert() const
+ {
+ wm::EntitySnapshot m;
+ for (const auto& s : _container)
+ {
+ m.addInstance(s.convert());
+ }
+ return m;
+ }
+
+ void EntitySnapshot::setTo(const wm::EntitySnapshot& m)
+ {
+ // We remove the contente here and reset it with new values
+ _container.clear();
+
+ int i = 0;
+ for (const auto& s : m.container())
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..79c0341643c4619270e6b264454ce50dffd0a202
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "../base/EntitySnapshotBase.h"
+
+#include "EntityInstance.h"
+
+#include "../workingmemory/EntitySnapshot.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ /**
+ * @brief Data of an entity at one point in time.
+ */
+ class EntitySnapshot :
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
+ {
+ using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
+
+ public:
+
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::EntitySnapshot convert() const;
+
+ // MongoDB connection
+ void setTo(const wm::EntitySnapshot&);
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac204816011ee0b760fa65a9fecea330edcadf97
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -0,0 +1,43 @@
+#include "Memory.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ wm::Memory Memory::convert() const
+ {
+ wm::Memory m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addCoreSegment(s.convert());
+ }
+ return m;
+ }
+
+ void Memory::reload(const MongoDBConnectionManager::MongoDBSettings& settings)
+ {
+ dbsettings = settings;
+ std::cout << "Setting connection to: " << settings.uniqueString() << std::endl;
+ }
+
+ void Memory::append(const wm::Memory& m)
+ {
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..a381972a8c6670f10f286b5ecee82757c1be87d1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "../base/MemoryBase.h"
+
+#include "CoreSegment.h"
+
+#include "../workingmemory/Memory.h"
+#include "mongodb/MongoDBConnectionManager.h"
+
+namespace armarx::armem::ltm
+{
+ /**
+ * @brief Data of a memory consisting of multiple core segments.
+ */
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>
+ {
+ using Base = base::MemoryBase<CoreSegment, Memory>;
+
+ public:
+
+ using Base::MemoryBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::Memory convert() const;
+
+ // MongoDB connection
+ void reload(const MongoDBConnectionManager::MongoDBSettings&);
+ void append(const wm::Memory&);
+
+
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..38f2a142989324cab5aa9a47f2f80d7e91fa9b24
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -0,0 +1,36 @@
+#include "ProviderSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ wm::ProviderSegment ProviderSegment::convert() const
+ {
+ wm::ProviderSegment m;
+ for (const auto& [_, s] : _container)
+ {
+ m.addEntity(s.convert());
+ }
+ return m;
+ }
+
+ void ProviderSegment::append(const wm::ProviderSegment& m)
+ {
+ for (const auto& [k, s] : m.container())
+ {
+ 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)));
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..4e81db8a9270f58d44845d08f298b37d59424323
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "../base/ProviderSegmentBase.h"
+
+#include "Entity.h"
+
+#include "../workingmemory/ProviderSegment.h"
+
+
+namespace armarx::armem::ltm
+{
+
+ /**
+ * @brief Data of a provider segment containing multiple entities.
+ */
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>
+ {
+ using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
+
+ public:
+
+ using Base::ProviderSegmentBase;
+ using Base::operator=;
+
+
+ // Conversion
+ wm::ProviderSegment convert() const;
+
+ // MongoDB connection
+ void append(const wm::ProviderSegment&);
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..02c25965f9c6ae0c12f528c9e2a5c241a10a26d3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/mongodb/MongoDBConnectionManager.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <map>
+#include <memory>
+
+namespace armarx::armem::ltm
+{
+
+ /**
+ * @brief Data of a memory consisting of multiple core segments.
+ */
+ class MongoDBConnectionManager
+ {
+ public:
+ struct MongoDBSettings
+ {
+ std::string host = "localhost";
+ std::string user = "root";
+ std::string password = "";
+
+ bool isSet() const
+ {
+ // we always need a user and a host
+ return !host.empty() and !user.empty();
+ }
+
+ std::string uniqueString() const
+ {
+ return host + "::" + user;
+ }
+ };
+
+ static int EstablishConnection(const MongoDBSettings& settings)
+ {
+ const auto str_rep = settings.uniqueString();
+ const auto& it = Connections.find(str_rep);
+ if (it == Connections.end())
+ {
+ auto con = Connections.emplace(str_rep, 0);
+ return con.first->second;
+ }
+ else
+ {
+ // A connection already exists. We do not need to open another one.
+ return it->second;
+ }
+ }
+
+ private:
+ static std::map<std::string, int> Connections;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4341c9da6b9bf6c1c9be6e1141ff712d9e00132
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -0,0 +1,19 @@
+#include "CoreSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::wm
+{
+ void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
+ {
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
+ {
+ other.addProviderSegment(s.copyWithoutData());
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..1eaf327338929b1290e476951aa69bdde0d3723d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "../base/CoreSegmentBase.h"
+
+#include "ProviderSegment.h"
+#include "detail/CopyWithoutData.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief Data of a core segment containing multiple provider segments.
+ */
+ class CoreSegment :
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::CopyWithoutData<CoreSegment>
+ {
+ using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
+
+ public:
+
+ 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;
+
+
+ protected:
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..022864c19d9ea98583d9d3998bec46643921cdc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
@@ -0,0 +1,14 @@
+#include "Entity.h"
+
+namespace armarx::armem::wm
+{
+ void Entity::_copySelfWithoutData(Entity& other) const
+ {
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
+ {
+ other.addSnapshot(s.copyWithoutData());
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a31286722e0e78d53423f6aa6b517583f83ca09
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "../base/EntityBase.h"
+
+#include "EntitySnapshot.h"
+#include "detail/CopyWithoutData.h"
+
+
+namespace armarx::armem::wm
+{
+ /**
+ * @brief An entity over a period of time.
+ *
+ * An entity should be a physical thing or abstract concept existing
+ * (and potentially evolving) over some time.
+ *
+ * Examples are:
+ * - objects (the green box)
+ * - agents (robot, human)
+ * - locations (frige, sink)
+ * - grasp affordances (general, or for a specific object)
+ * - images
+ * - point clouds
+ * - other sensory values
+ *
+ * At each point in time (`EntitySnapshot`), the entity can have a
+ * (potentially variable) number of instances (`EntityInstance`),
+ * each containing a single `AronData` object of a specific `AronType`.
+ */
+ class Entity :
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::CopyWithoutData<Entity>
+ {
+ using Base = base::EntityBase<EntitySnapshot, Entity>;
+
+ public:
+
+ 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;
+
+
+ protected:
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..95e508bde1412a87c865b2720bd227ec58595114
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.cpp
@@ -0,0 +1,69 @@
+#include "EntityInstance.h"
+
+#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
+{
+
+ bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
+ {
+ return timeCreated == other.timeCreated
+ && timeSent == other.timeSent
+ && timeArrived == other.timeArrived
+ && std::abs(confidence - other.confidence) < 1e-6f;
+ }
+
+
+ bool EntityInstance::equalsDeep(const EntityInstance& other) const
+ {
+ return id() == other.id()
+ && _metadata == other.metadata()
+ && _data->equalsDeep(other.data());
+ }
+
+ void EntityInstance::update(const EntityUpdate& update, int index)
+ {
+ ARMARX_CHECK_FITS_SIZE(index, update.instancesData.size());
+
+ this->index() = index;
+ setData(update.instancesData.at(size_t(index)));
+
+ this->_metadata.confidence = update.confidence;
+
+ this->_metadata.timeCreated = update.timeCreated;
+ this->_metadata.timeSent = update.timeSent;
+ this->_metadata.timeArrived = update.timeArrived;
+ }
+
+ EntityInstance EntityInstance::copy() const
+ {
+ EntityInstance d;
+ this->_copySelf(d);
+ return d;
+ }
+
+ void EntityInstance::_copySelf(EntityInstance& other) const
+ {
+ Base::_copySelf(other);
+ other._metadata = _metadata;
+ other._data = _data;
+ }
+
+ void EntityInstance::_copySelfWithoutData(EntityInstance& other) const
+ {
+ Base::_copySelf(other);
+ other._metadata = _metadata;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/EntityInstance.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
similarity index 59%
rename from source/RobotAPI/libraries/armem/core/EntityInstance.h
rename to source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
index 25311339e6429d62b4f7de77476804a553574a7f..8e6eb1afe7e673baeab933397ebd16aa1781c18d 100644
--- a/source/RobotAPI/libraries/armem/core/EntityInstance.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h
@@ -1,15 +1,11 @@
#pragma once
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include "../base/EntityInstanceBase.h"
-#include "../core/Time.h"
+#include "detail/CopyWithoutData.h"
-#include "Commit.h"
-#include "detail/MemoryItem.h"
-
-namespace armarx::armem
+namespace armarx::armem::wm
{
/**
@@ -35,27 +31,27 @@ namespace armarx::armem
}
};
+ std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
+
/**
* @brief Data of a single entity instance.
*/
- class EntityInstance : public detail::MemoryItem
+ class EntityInstance :
+ public base::EntityInstanceBase<EntityInstance>,
+ public detail::CopyWithoutData<EntityInstance>
{
+ using Base = base::EntityInstanceBase<EntityInstance>;
+
public:
- EntityInstance();
- EntityInstance(int index, const MemoryID& parentID = {});
- EntityInstance(const MemoryID& id);
+ using Base::EntityInstanceBase;
+ EntityInstance(const EntityInstance& other) = default;
+ EntityInstance(EntityInstance&& other) = default;
+ EntityInstance& operator=(const EntityInstance& other) = default;
+ EntityInstance& operator=(EntityInstance&& other) = default;
- inline int& index()
- {
- return id().instanceIndex;
- }
- inline int index() const
- {
- return id().instanceIndex;
- }
EntityInstanceMetadata& metadata()
{
@@ -70,6 +66,7 @@ namespace armarx::armem
{
return _data;
}
+
void setData(const aron::datanavigator::DictNavigatorPtr& data)
{
this->_data = data;
@@ -81,18 +78,17 @@ namespace armarx::armem
* @param update The update.
* @param index The instances index.
*/
- void update(const EntityUpdate& update, int index);
+ virtual void update(const EntityUpdate& update, int index) override;
+ virtual bool equalsDeep(const EntityInstance& other) const override;
- EntityInstance copy(bool withData = true) const;
+ virtual EntityInstance copy() const override;
- bool equalsDeep(const EntityInstance& other) const;
+ protected:
- // MemoryItem interface
- public:
- std::string getKeyString() const override;
- std::string getLevelName() const override;
+ virtual void _copySelf(EntityInstance& other) const override;
+ virtual void _copySelfWithoutData(EntityInstance& other) const override;
private:
@@ -104,5 +100,4 @@ namespace armarx::armem
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
new file mode 100644
index 0000000000000000000000000000000000000000..fb68d050a0ad19e1e2627e4cb5732a23a2ce4493
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
@@ -0,0 +1,18 @@
+#include "EntitySnapshot.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::wm
+{
+ void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
+ {
+ other.id() = _id;
+ for (const auto& s : _container)
+ {
+ other.addInstance(s.copyWithoutData());
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee0c78c5d201ed2e9fb1fe5c333efc728a9dccfc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "../base/EntitySnapshotBase.h"
+
+#include "EntityInstance.h"
+#include "detail/CopyWithoutData.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief Data of an entity at one point in time.
+ */
+ class EntitySnapshot :
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
+ public detail::CopyWithoutData<EntitySnapshot>
+ {
+ using Base = base::EntitySnapshotBase<EntityInstance, EntitySnapshot>;
+
+ public:
+
+ using Base::EntitySnapshotBase;
+ using Base::operator=;
+
+
+ protected:
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..b7af1a6456687b1741710d46fa157b9091295db7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -0,0 +1,14 @@
+#include "Memory.h"
+
+
+namespace armarx::armem::wm
+{
+ void Memory::_copySelfWithoutData(Memory& other) const
+ {
+ other.id() = _id;
+ for (const auto& [k, s] : _container)
+ {
+ other.addCoreSegment(s.copyWithoutData());
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b7cb35349837820c3dd47f2d584ef6b9760461
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "../base/MemoryBase.h"
+
+#include "CoreSegment.h"
+#include "detail/CopyWithoutData.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief Data of a memory consisting of multiple core segments.
+ */
+ class Memory :
+ public base::MemoryBase<CoreSegment, Memory>,
+ public detail::CopyWithoutData<Memory>
+ {
+ using Base = base::MemoryBase<CoreSegment, Memory>;
+
+ public:
+
+ using Base::MemoryBase;
+
+ Memory(const Memory& other) = default;
+ Memory(Memory&& other) = default;
+ Memory& operator=(const Memory& other) = default;
+ Memory& operator=(Memory&& other) = default;
+
+
+ protected:
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..517b35bfad666cdac02041694e787523c4c1e894
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
@@ -0,0 +1,21 @@
+#include "ProviderSegment.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "error.h"
+
+
+namespace armarx::armem::wm
+{
+
+ void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
+ {
+ other.id() = _id;
+ other.setMaxHistorySize(_maxHistorySize);
+ for (const auto& [k, s] : _container)
+ {
+ other.addEntity(s.copyWithoutData());
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..0b703617d9b1b642153da505882e71aab8aa3723
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include "../base/ProviderSegmentBase.h"
+
+#include "Entity.h"
+#include "detail/CopyWithoutData.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief Data of a provider segment containing multiple entities.
+ */
+ class ProviderSegment :
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::CopyWithoutData<ProviderSegment>
+
+ {
+ using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
+
+ public:
+
+ 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;
+
+
+ protected:
+
+ 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/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dded894331809986a991351f0eb7c7675b11812
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
@@ -0,0 +1,124 @@
+#include "ice_conversions.h"
+
+namespace armarx::armem
+{
+ void toIce(armarx::armem::data::EntityInstanceMetadata& ice, const armarx::armem::wm::EntityInstanceMetadata& metadata)
+ {
+ ice.confidence = metadata.confidence;
+ toIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
+ toIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
+ toIce(ice.timeSentMicroSeconds, metadata.timeSent);
+ }
+ void fromIce(const armarx::armem::data::EntityInstanceMetadata& ice, armarx::armem::wm::EntityInstanceMetadata& metadata)
+ {
+ metadata.confidence = ice.confidence;
+ fromIce(ice.timeArrivedMicroSeconds, metadata.timeArrived);
+ fromIce(ice.timeCreatedMicroSeconds, metadata.timeCreated);
+ fromIce(ice.timeSentMicroSeconds, metadata.timeSent);
+ }
+
+ void toIce(data::EntityInstance& ice, const wm::EntityInstance& data)
+ {
+ detail::toIceItem(ice, data);
+
+ if (data.data())
+ {
+ ice.data = data.data()->toAronDictPtr();
+ }
+ toIce(ice.metadata, data.metadata());
+ }
+ void fromIce(const data::EntityInstance& ice, wm::EntityInstance& data)
+ {
+ detail::fromIceItem(ice, data);
+
+ if (ice.data)
+ {
+ data.setData(aron::datanavigator::DictNavigator::FromAronDictPtr(ice.data));
+ }
+ fromIce(ice.metadata, data.metadata());
+ }
+
+
+ void toIce(data::EntitySnapshot& ice, const wm::EntitySnapshot& snapshot)
+ {
+ detail::toIceItem(ice, snapshot);
+
+ toIce(ice.instances, snapshot.instances());
+ }
+ void fromIce(const data::EntitySnapshot& ice, wm::EntitySnapshot& snapshot)
+ {
+ detail::fromIceItem(ice, snapshot);
+
+ fromIce(ice.instances, snapshot.instances());
+ }
+
+ void toIce(data::Entity& ice, const wm::Entity& entity)
+ {
+ detail::toIceItem(ice, entity);
+
+ toIce(ice.history, entity.history());
+ }
+ void fromIce(const data::Entity& ice, wm::Entity& entity)
+ {
+ detail::fromIceItem(ice, entity);
+
+ fromIce(ice.history, entity.history());
+ }
+
+
+ void toIce(data::ProviderSegment& ice, const wm::ProviderSegment& providerSegment)
+ {
+ detail::toIceItem(ice, providerSegment);
+
+ if (providerSegment.hasAronType())
+ {
+ ice.aronType = providerSegment.aronType()->getResult();
+ }
+ toIce(ice.entities, providerSegment.entities());
+ }
+ void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment)
+ {
+ detail::fromIceItem(ice, providerSegment);
+
+ if (ice.aronType)
+ {
+ providerSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
+ }
+ fromIce(ice.entities, providerSegment.entities());
+ }
+
+ void toIce(data::CoreSegment& ice, const wm::CoreSegment& coreSegment)
+ {
+ detail::toIceItem(ice, coreSegment);
+
+ if (coreSegment.hasAronType())
+ {
+ ice.aronType = coreSegment.aronType()->getResult();
+ }
+ toIce(ice.providerSegments, coreSegment.providerSegments());
+ }
+ void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment)
+ {
+ detail::fromIceItem(ice, coreSegment);
+
+ if (ice.aronType)
+ {
+ coreSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
+ }
+ fromIce(ice.providerSegments, coreSegment.providerSegments());
+ }
+
+ void toIce(data::Memory& ice, const wm::Memory& memory)
+ {
+ detail::toIceItem(ice, memory);
+
+ toIce(ice.coreSegments, memory.coreSegments());
+ }
+ void fromIce(const data::Memory& ice, wm::Memory& memory)
+ {
+ detail::fromIceItem(ice, memory);
+
+ fromIce(ice.coreSegments, memory.coreSegments());
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d14e6b0d04e9086e701cc38a159e516e533d95c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/commit.h>
+#include <RobotAPI/interface/armem/memory.h>
+
+#include "Memory.h"
+
+
+namespace armarx::armem
+{
+ void toIce(data::EntityInstanceMetadata& ice, const wm::EntityInstanceMetadata& metadata);
+ void fromIce(const data::EntityInstanceMetadata& ice, wm::EntityInstanceMetadata& metadata);
+
+ void toIce(data::EntityInstance& ice, const wm::EntityInstance& data);
+ void fromIce(const data::EntityInstance& ice, wm::EntityInstance& data);
+
+
+ void toIce(data::EntitySnapshot& ice, const wm::EntitySnapshot& snapshot);
+ void fromIce(const data::EntitySnapshot& ice, wm::EntitySnapshot& snapshot);
+
+ void toIce(data::Entity& ice, const wm::Entity& entity);
+ void fromIce(const data::Entity& ice, wm::Entity& entity);
+
+
+ void toIce(data::ProviderSegment& ice, const wm::ProviderSegment& providerSegment);
+ void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment);
+
+ void toIce(data::CoreSegment& ice, const wm::CoreSegment& coreSegment);
+ void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment);
+
+ void toIce(data::Memory& ice, const wm::Memory& memory);
+ void fromIce(const data::Memory& ice, wm::Memory& memory);
+}
+
+// Must be included after the prototypes. Otherwise the compiler cannot find the correct methods in ice_coversion_templates.h
+#include "../ice_conversions.h"
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 de811a33125e672a0a57e266ef5ca9b59971b083..536c72f49d6ccb4c656f37c345dc6a4ec70b4f17 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -6,10 +6,10 @@
#include "MemoryToIceAdapter.h"
+//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
namespace armarx::armem::server::plugins
{
-
ComponentPlugin::~ComponentPlugin()
{}
@@ -18,17 +18,30 @@ namespace armarx::armem::server::plugins
{
ClientPlugin::postCreatePropertyDefinitions(properties);
properties->topic(memoryListener, this->parent<ComponentPluginUser>().memoryListenerDefaultName);
+
+ properties->optional(longTermMemoryDatabaseHost, this->parent<ComponentPluginUser>().longTermMemoryDatabaseHostDefault);
+ properties->optional(longTermMemoryDatabaseUser, this->parent<ComponentPluginUser>().longTermMemoryDatabaseUserDefault);
+ properties->optional(longTermMemoryDatabasePassword, this->parent<ComponentPluginUser>().longTermMemoryDatabasePasswordDefault);
}
void ComponentPlugin::postOnConnectComponent()
{
ComponentPluginUser& parent = this->parent<ComponentPluginUser>();
+
if (isMemoryNameSystemEnabled() && parent.memoryNameSystem)
{
registerMemory(parent);
}
parent.iceMemory.setMemoryListener(memoryListener);
+
+ // establishing connection to ltm and mongodb
+ ltm::MongoDBConnectionManager::MongoDBSettings settings;
+ settings.host = longTermMemoryDatabaseHost;
+ settings.user = longTermMemoryDatabaseUser;
+ settings.password = longTermMemoryDatabasePassword;
+
+ parent.longtermMemory.reload(settings);
}
@@ -45,7 +58,7 @@ namespace armarx::armem::server::plugins
data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
{
data::RegisterMemoryInput input;
- input.name = parent.memory.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);
@@ -68,7 +81,7 @@ namespace armarx::armem::server::plugins
try
{
data::RemoveMemoryInput input;
- input.name = parent.memory.name();
+ input.name = parent.workingMemory.name();
result = parent.memoryNameSystem->removeMemory(input);
if (result.success)
{
@@ -94,7 +107,6 @@ namespace armarx::armem::server::plugins
namespace armarx::armem::server
{
-
ComponentPluginUser::ComponentPluginUser()
{
addPlugin(plugin);
@@ -102,7 +114,6 @@ namespace armarx::armem::server
// WRITING
-
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, const Ice::Current&)
{
bool addCoreSegmentOnUsage = false;
@@ -111,7 +122,7 @@ namespace armarx::armem::server
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
return result;
}
@@ -119,17 +130,32 @@ namespace armarx::armem::server
data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
return iceMemory.commit(commitIce);
}
// READING
-
armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(memoryMutex);
+ 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);
+ return iceMemory.store(input);
+ }
+
+
+ // LTM STORING
+ armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
+ {
+ 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 ca0958b3c93203ef519c2c5311b71c1b0f54cd98..fc2154f59a0d640a2b6d6e07f5df51465caa2812 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -8,7 +8,9 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include "../core/Memory.h"
+#include "../core/workingmemory/Memory.h"
+#include "../core/longtermmemory/Memory.h"
+
#include "../mns/ClientPlugin.h"
#include "MemoryToIceAdapter.h"
@@ -47,6 +49,10 @@ namespace armarx::armem::server::plugins
data::RemoveMemoryResult removeMemory(ComponentPluginUser& parent);
client::MemoryListenerInterfacePrx memoryListener;
+
+ std::string longTermMemoryDatabaseHost;
+ std::string longTermMemoryDatabaseUser;
+ std::string longTermMemoryDatabasePassword;
};
}
@@ -80,24 +86,35 @@ namespace armarx::armem::server
virtual armem::query::data::Result query(const armem::query::data::Input& input, const Ice::Current& = Ice::emptyCurrent) override;
- public:
+ // StoringInterface interface
+ virtual data::StoreResult store(const data::StoreInput&, const Ice::Current& = Ice::emptyCurrent) override;
+
+
+ // LoadingInterface interface
+ virtual armem::query::data::Result load(const armem::query::data::Input&, const Ice::Current& = Ice::emptyCurrent) override;
+
+ public:
/// The actual memory.
- Memory memory;
- std::mutex memoryMutex;
+ wm::Memory workingMemory;
+ std::mutex workingMemoryMutex;
+
+ ltm::Memory longtermMemory;
+ std::mutex longtermMemoryMutex;
- /// property defauls
+ /// property defaults
std::string memoryListenerDefaultName = "MemoryUpdates";
+ std::string longTermMemoryDatabaseHostDefault = "";
+ std::string longTermMemoryDatabaseUserDefault = "";
+ std::string longTermMemoryDatabasePasswordDefault = "";
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceMemory { &memory };
+ MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
private:
-
plugins::ComponentPlugin* plugin = nullptr;
-
};
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index b2ada310d271e5c9161d2f3f74e12ca16ee66ae6..9ab34efd946541a37ae825d6338a355aec0dd722 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -13,16 +13,16 @@
namespace armarx::armem::server
{
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const Memory& memory) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::Memory& memory) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Memory", memory.name(), memory.coreSegments.size()));
+ group.setLabel(makeGroupLabel("Memory", memory.name(), memory.coreSegments().size()));
- if (memory.coreSegments.empty())
+ if (memory.coreSegments().empty())
{
group.addChild(Label(makeNoItemsMessage("core segments")));
}
- for (const auto& [name, coreSegment] : memory.coreSegments)
+ for (const auto& [name, coreSegment] : memory.coreSegments())
{
group.addChild(makeGroupBox(coreSegment));
}
@@ -32,16 +32,16 @@ namespace armarx::armem::server
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const CoreSegment& coreSegment) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::CoreSegment& coreSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.providerSegments.size()));
+ group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.providerSegments().size()));
- if (coreSegment.providerSegments.empty())
+ if (coreSegment.providerSegments().empty())
{
group.addChild(Label(makeNoItemsMessage("provider segments")));
}
- for (const auto& [name, providerSegment] : coreSegment.providerSegments)
+ for (const auto& [name, providerSegment] : coreSegment.providerSegments())
{
group.addChild(makeGroupBox(providerSegment));
}
@@ -51,16 +51,16 @@ namespace armarx::armem::server
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const ProviderSegment& providerSegment) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::ProviderSegment& providerSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.entities.size()));
+ group.setLabel(makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.entities().size()));
- if (providerSegment.entities.empty())
+ if (providerSegment.entities().empty())
{
group.addChild(Label(makeNoItemsMessage("entities")));
}
- for (const auto& [name, entity] : providerSegment.entities)
+ for (const auto& [name, entity] : providerSegment.entities())
{
group.addChild(makeGroupBox(entity));
}
@@ -70,18 +70,18 @@ namespace armarx::armem::server
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const Entity& entity) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::Entity& entity) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Entity", entity.name(), entity.history.size()));
+ group.setLabel(makeGroupLabel("Entity", entity.name(), entity.history().size()));
- if (entity.history.empty())
+ if (entity.history().empty())
{
group.addChild(Label(makeNoItemsMessage("snapshots")));
}
- if (int(entity.history.size()) <= maxHistorySize)
+ if (int(entity.history().size()) <= maxHistorySize)
{
- for (const auto& [time, snapshot] : entity.history)
+ for (const auto& [time, snapshot] : entity.history())
{
group.addChild(makeGroupBox(snapshot));
}
@@ -89,8 +89,8 @@ namespace armarx::armem::server
else
{
int margin = 2;
- auto it = entity.history.begin();
- auto rit = entity.history.end();
+ auto it = entity.history().begin();
+ auto rit = entity.history().end();
--rit;
for (int i = 0; i < margin; ++i, ++it)
{
@@ -98,7 +98,7 @@ namespace armarx::armem::server
--rit;
}
group.addChild(Label("..."));
- for (; rit != entity.history.end(); ++rit)
+ for (; rit != entity.history().end(); ++rit)
{
group.addChild(makeGroupBox(rit->second));
}
@@ -110,17 +110,17 @@ namespace armarx::armem::server
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const EntitySnapshot& snapshot) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntitySnapshot& snapshot) const
{
GroupBox group;
group.setLabel(makeGroupLabel("t", armem::toDateTimeMilliSeconds(snapshot.time()),
- snapshot.instances.size(), " = ", ""));
+ snapshot.instances().size(), " = ", ""));
- if (snapshot.instances.empty())
+ if (snapshot.instances().empty())
{
group.addChild(Label(makeNoItemsMessage("instances")));
}
- for (const EntityInstance& instance : snapshot.instances)
+ for (const wm::EntityInstance& instance : snapshot.instances())
{
group.addChild(makeGroupBox(instance));
}
@@ -129,7 +129,7 @@ namespace armarx::armem::server
return group;
}
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const EntityInstance& instance) const
+ MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const wm::EntityInstance& instance) const
{
GroupBox group;
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
index 4ce866f15b52a9c2505f5081adcbe87451695966..f4a7fad95f1efa38d8ad7044d661e77fef60be31 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
@@ -2,7 +2,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-#include "../core/Memory.h"
+#include "../core/workingmemory/Memory.h"
namespace armarx::armem::server
@@ -17,12 +17,12 @@ namespace armarx::armem::server
using GroupBox = armarx::RemoteGui::Client::GroupBox;
using Label = armarx::RemoteGui::Client::Label;
- GroupBox makeGroupBox(const Memory& memory) const;
- GroupBox makeGroupBox(const CoreSegment& coreSegment) const;
- GroupBox makeGroupBox(const ProviderSegment& providerSegment) const;
- GroupBox makeGroupBox(const Entity& entity) const;
- GroupBox makeGroupBox(const EntitySnapshot& entitySnapshot) const;
- GroupBox makeGroupBox(const EntityInstance& instance) const;
+ GroupBox makeGroupBox(const wm::Memory& memory) const;
+ GroupBox makeGroupBox(const wm::CoreSegment& coreSegment) const;
+ GroupBox makeGroupBox(const wm::ProviderSegment& providerSegment) const;
+ GroupBox makeGroupBox(const wm::Entity& entity) const;
+ GroupBox makeGroupBox(const wm::EntitySnapshot& entitySnapshot) const;
+ GroupBox makeGroupBox(const wm::EntityInstance& instance) const;
std::string makeGroupLabel(const std::string& term, const std::string& name, size_t size,
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 22c47eea2cd3c2e0121297f6d5b2e88f369ea3a3..6895cf4e5b27ecec5872a38890d8111b41405fa7 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -3,14 +3,17 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "../error.h"
-#include "../core/ice_conversions.h"
-#include "query_proc/MemoryQueryProcessor.h"
+#include "../core/workingmemory/ice_conversions.h"
+#include "query_proc/workingmemory/MemoryQueryProcessor.h"
+//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
+//#include "../core/io/diskReader/NlohmannJSON/NlohmannJSONDiskReader.h"
namespace armarx::armem::server
{
- MemoryToIceAdapter::MemoryToIceAdapter(Memory* memory) : memory(memory)
+ MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, ltm::Memory* longtermMemory) :
+ workingMemory(workingMemory), longtermMemory(longtermMemory)
{
}
@@ -18,30 +21,29 @@ namespace armarx::armem::server
void
MemoryToIceAdapter::setMemoryListener(client::MemoryListenerInterfacePrx memoryListener)
{
- this->memoryListener = memoryListener;
+ this->memoryListenerTopic = memoryListener;
}
// WRITING
-
data::AddSegmentResult
MemoryToIceAdapter::addSegment(const data::AddSegmentInput& input, bool addCoreSegments)
{
ARMARX_DEBUG << "Adding segment '" << input.coreSegmentName << "/" << input.providerSegmentName << "'.";
- ARMARX_CHECK_NOT_NULL(memory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentResult output;
- armem::CoreSegment* coreSegment = nullptr;
+ armem::wm::CoreSegment* coreSegment = nullptr;
try
{
- coreSegment = &memory->getCoreSegment(input.coreSegmentName);
+ coreSegment = &workingMemory->getCoreSegment(input.coreSegmentName);
}
catch (const armem::error::MissingEntry& e)
{
if (addCoreSegments)
{
- coreSegment = &memory->addCoreSegment(input.coreSegmentName);
+ coreSegment = &workingMemory->addCoreSegment(input.coreSegmentName);
}
else
{
@@ -63,14 +65,14 @@ namespace armarx::armem::server
// This is ok.
if (input.clearWhenExists)
{
- ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
+ wm::ProviderSegment& provider = coreSegment->getProviderSegment(input.providerSegmentName);
provider.clear();
}
}
}
armem::MemoryID segmentID;
- segmentID.memoryName = memory->name();
+ segmentID.memoryName = workingMemory->name();
segmentID.coreSegmentName = input.coreSegmentName;
segmentID.providerSegmentName = input.providerSegmentName;
@@ -83,7 +85,7 @@ namespace armarx::armem::server
data::AddSegmentsResult
MemoryToIceAdapter::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- ARMARX_CHECK_NOT_NULL(memory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
data::AddSegmentsResult output;
for (const auto& i : input)
@@ -96,7 +98,7 @@ namespace armarx::armem::server
data::CommitResult
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
- ARMARX_CHECK_NOT_NULL(memory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
armem::Commit commit;
armem::fromIce(commitIce, commit, timeArrived);
@@ -120,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)
@@ -128,7 +130,7 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- MemoryID snapshotID = memory->update(update);
+ MemoryID snapshotID = workingMemory->update(update);
result.success = true;
result.snapshotID = snapshotID;
@@ -149,7 +151,7 @@ namespace armarx::armem::server
if (publishUpdates)
{
- memoryListener->memoryUpdated(updatedIDs);
+ memoryListenerTopic->memoryUpdated(updatedIDs);
}
return commitResult;
@@ -157,16 +159,53 @@ namespace armarx::armem::server
// READING
-
armem::query::data::Result
MemoryToIceAdapter::query(const armem::query::data::Input& input)
{
- ARMARX_CHECK_NOT_NULL(memory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::query_proc::MemoryQueryProcessor processor(
+ armem::wm::query_proc::MemoryQueryProcessor processor(
input.withData ? armem::DataMode::WithData
: armem::DataMode::NoData);
- return processor.process(input, *memory);
+ return processor.process(input, *workingMemory);
+ }
+
+ // LTM LOADING
+ query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
+ {
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
+ query::data::Result output;
+
+ output.success = true;
+ return output;
+ }
+
+ // LTM STORING
+ data::StoreResult MemoryToIceAdapter::store(const armem::data::StoreInput& input)
+ {
+ ARMARX_CHECK_NOT_NULL(workingMemory);
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
+ data::StoreResult output;
+
+ armem::query::data::Result queryResult = this->query(input.query);
+ if (queryResult.success)
+ {
+ wm::Memory m;
+ fromIce(queryResult.memory, m);
+ longtermMemory->append(m);
+ }
+ else
+ {
+ output.success = false;
+ }
+
+ ARMARX_IMPORTANT << "Finsihed";
+ 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 c6c61c6a18af5b8855e9b97b05b5d79cb73d3a49..919c5fc8dc74ec3f742ac1c33365bc2c680ac768 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -3,7 +3,9 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include "../core/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
@@ -21,13 +23,12 @@ namespace armarx::armem::server
public:
/// Construct an MemoryToIceAdapter from an existing Memory.
- MemoryToIceAdapter(Memory* memory = nullptr);
+ MemoryToIceAdapter(wm::Memory* workingMemory = nullptr, ltm::Memory* longtermMemory = nullptr);
- void setMemoryListener(client::MemoryListenerInterfacePrx memoryListener);
+ void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
// WRITING
-
data::AddSegmentResult addSegment(
const data::AddSegmentInput& input, bool addCoreSegments = false);
@@ -39,17 +40,22 @@ namespace armarx::armem::server
data::CommitResult commit(const data::Commit& commitIce);
armem::CommitResult commit(const armem::Commit& commit);
-
// 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);
+ // LTM STORING
+ data::StoreResult store(const armem::data::StoreInput& input);
public:
- Memory* memory;
+ wm::Memory* workingMemory;
+ ltm::Memory* longtermMemory;
- client::MemoryListenerInterfacePrx memoryListener;
+ client::MemoryListenerInterfacePrx memoryListenerTopic;
};
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.cpp
deleted file mode 100644
index c9f25c5d279a896f34f4d72e252a427b24549e07..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "BaseQueryProcessor.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
-namespace armarx::armem::query_proc
-{
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.cpp
deleted file mode 100644
index 9cdb9050876af48a2c48f670b0772507aa688670..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-#include "CoreSegmentQueryProcessor.h"
-
-#include <regex>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
-namespace armarx::armem::query_proc
-{
-
- void CoreSegmentQueryProcessor::process(CoreSegment& result, const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
- {
- if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
- {
- process(result, *q, coreSegment);
- }
- else if (auto q = dynamic_cast<const armem::query::data::core::Single*>(&query))
- {
- process(result, *q, coreSegment);
- }
- else if (auto q = dynamic_cast<const armem::query::data::core::Regex*>(&query))
- {
- process(result, *q, coreSegment);
- }
- else
- {
- throw armem::error::UnknownQueryType("core segment", query);
- }
- }
-
-
- void CoreSegmentQueryProcessor::process(CoreSegment& result,
- const armem::query::data::core::All& query, const CoreSegment& coreSegment) const
- {
- for (const auto& [name, providerSegment] : coreSegment.providerSegments)
- {
- result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, providerSegment));
- }
- }
-
- void CoreSegmentQueryProcessor::process(CoreSegment& result,
- const armem::query::data::core::Single& query, const CoreSegment& coreSegment) const
- {
- try
- {
- const ProviderSegment& providerSegment = coreSegment.getProviderSegment(query.providerSegmentName);
- result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, providerSegment));
- }
- catch (const error::MissingEntry&)
- {
- // Leave empty.
- }
- }
-
- void CoreSegmentQueryProcessor::process(CoreSegment& result,
- const armem::query::data::core::Regex& query, const CoreSegment& coreSegment) const
- {
- std::regex regex(query.providerSegmentNameRegex);
- for (const auto& [name, providerSegment] : coreSegment.providerSegments)
- {
- if (std::regex_search(providerSegment.name(), regex))
- {
- result.addProviderSegment(providerSegmentProcessor.process(query.providerSegmentQueries, providerSegment));
- }
- }
- }
-
-
- data::CoreSegment CoreSegmentQueryProcessor::processToIce(
- const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const
- {
- return toIce<data::CoreSegment>(process(query, coreSegment));
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.h
deleted file mode 100644
index 4a59e546da57ff720a9dcce627319bc8edcb1e35..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/CoreSegmentQueryProcessor.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-
-#include "BaseQueryProcessor.h"
-#include "ProviderSegmentQueryProcessor.h"
-
-
-
-namespace armarx::armem::query_proc
-{
-
- /**
- * @brief Handles memory queries.
- */
- class CoreSegmentQueryProcessor : public BaseQueryProcessor<CoreSegment, armem::query::data::CoreSegmentQuery>
- {
- public:
-
- CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- BaseQueryProcessor(dataMode), providerSegmentProcessor(dataMode)
- {}
-
-
- using BaseQueryProcessor::process;
-
- void process(CoreSegment& result, const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const;
-
- void process(CoreSegment& result, const armem::query::data::core::All& query, const CoreSegment& coreSegment) const;
- void process(CoreSegment& result, const armem::query::data::core::Single& query, const CoreSegment& coreSegment) const;
- void process(CoreSegment& result, const armem::query::data::core::Regex& query, const CoreSegment& coreSegment) const;
-
-
- data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const CoreSegment& coreSegment) const;
-
-
- private:
-
- ProviderSegmentQueryProcessor providerSegmentProcessor;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp
deleted file mode 100644
index c062c089e31f8cf5c1314e4e4fcd7608ee5f2a48..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp
+++ /dev/null
@@ -1,152 +0,0 @@
-#include "EntityQueryProcessor.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
-namespace armarx::armem::query_proc
-{
-
- void EntityQueryProcessor::process(Entity& result, const armem::query::data::EntityQuery& query, const Entity& entity) const
- {
- if (auto q = dynamic_cast<const armem::query::data::entity::All*>(&query))
- {
- process(result, *q, entity);
- }
- else if (auto q = dynamic_cast<const armem::query::data::entity::Single*>(&query))
- {
- process(result, *q, entity);
- }
- else if (auto q = dynamic_cast<const armem::query::data::entity::TimeRange*>(&query))
- {
- process(result, *q, entity);
- }
- else if (auto q = dynamic_cast<const armem::query::data::entity::IndexRange*>(&query))
- {
- process(result, *q, entity);
- }
- else
- {
- throw armem::error::UnknownQueryType("entity snapshot", query);
- }
- }
-
- void EntityQueryProcessor::process(Entity& result, const armem::query::data::entity::All& query, const Entity& entity) const
- {
- (void) query;
- // Copy this entitiy and its contents.
-
- for (const auto& [time, snapshot] : entity)
- {
- addResultSnapshot(result, snapshot);
- }
- }
-
- void EntityQueryProcessor::process(Entity& result, const armem::query::data::entity::Single& query, const Entity& entity) const
- {
- if (query.timestamp < 0)
- {
- try
- {
- addResultSnapshot(result, entity.getLatestSnapshot());
- }
- catch (const armem::error::EntityHistoryEmpty&)
- {
- // Leave empty.
- }
- }
- else
- {
- Time time = fromIce<Time>(query.timestamp);
- try
- {
- addResultSnapshot(result, entity.getSnapshot(time));
- }
- catch (const armem::error::MissingEntry&)
- {
- // Leave empty.
- }
- }
- }
-
- void EntityQueryProcessor::process(Entity& result, const armem::query::data::entity::IndexRange& query, const Entity& entity) const
- {
- if (entity.empty())
- {
- return;
- }
-
- size_t first = negativeIndexSemantics(query.first, entity.history.size());
- size_t last = negativeIndexSemantics(query.last, entity.history.size());
-
- if (first <= last)
- {
- auto it = entity.begin();
- std::advance(it, first);
-
- size_t num = last - first + 1; // +1 to make last inclusive
- for (size_t i = 0; i < num; ++i, ++it)
- {
- addResultSnapshot(result, it);
- }
- }
- }
-
- void EntityQueryProcessor::process(Entity& result, const armem::query::data::entity::TimeRange& query, const Entity& entity) const
- {
- if (query.minTimestamp <= query.maxTimestamp || query.minTimestamp < 0 || query.maxTimestamp < 0)
- {
- Time min = fromIce<Time>(query.minTimestamp);
- Time max = fromIce<Time>(query.maxTimestamp);
- process(result, min, max, entity, query);
- }
- }
-
- void EntityQueryProcessor::process(Entity& result, const Time& min, const Time& max, const Entity& entity,
- const armem::query::data::EntityQuery& query) const
- {
- (void) query;
-
- // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
- auto begin = min.toMicroSeconds() > 0 ? entity.history.lower_bound(min) : entity.history.begin();
- // Returns an iterator pointing to the first element that is *greater than* key.
- auto end = max.toMicroSeconds() > 0 ? entity.history.upper_bound(max) : entity.history.end();
-
- for (auto it = begin; it != end && it != entity.history.end(); ++it)
- {
- addResultSnapshot(result, it);
- }
- }
-
- data::Entity EntityQueryProcessor::processToIce(const armem::query::data::EntityQuery& query, const Entity& entity) const
- {
- return toIce<data::Entity>(process(query, entity));
- }
-
- size_t EntityQueryProcessor::negativeIndexSemantics(long index, size_t size)
- {
- const size_t max = size > 0 ? size - 1 : 0;
- if (index >= 0)
- {
- return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
- }
- else
- {
- return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
- }
- }
-
- void EntityQueryProcessor::addResultSnapshot(Entity& result, Entity::ContainerT::const_iterator it) const
- {
- addResultSnapshot(result, it->second);
- }
-
- void EntityQueryProcessor::addResultSnapshot(Entity& result, const EntitySnapshot& snapshot) const
- {
- bool withData = (dataMode == DataMode::WithData);
- result.addSnapshot(snapshot.copy(withData));
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.h
deleted file mode 100644
index 594022ffb82e227a87c793db29c54f462e66aaaa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-
-#include "BaseQueryProcessor.h"
-
-
-namespace armarx::armem::query_proc
-{
-
- /**
- * @brief Handles memory queries.
- */
- class EntityQueryProcessor : public BaseQueryProcessor<Entity, armem::query::data::EntityQuery>
- {
- public:
-
- EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
- BaseQueryProcessor(dataMode)
- {}
-
-
- using BaseQueryProcessor::process;
-
- void process(Entity& result, const armem::query::data::EntityQuery& query, const Entity& entity) const override;
-
- void process(Entity& result, const armem::query::data::entity::All& query, const Entity& entity) const;
- void process(Entity& result, const armem::query::data::entity::Single& query, const Entity& entity) const;
- void process(Entity& result, const armem::query::data::entity::TimeRange& query, const Entity& entity) const;
- void process(Entity& result, const armem::query::data::entity::IndexRange& query, const Entity& entity) const;
-
- void process(Entity& result, const Time& min, const Time& max, const Entity& entity,
- const armem::query::data::EntityQuery& query) const;
-
-
- data::Entity processToIce(const armem::query::data::EntityQuery& query, const Entity& entity) const;
-
-
- static size_t negativeIndexSemantics(long index, size_t size);
-
- private:
-
- void addResultSnapshot(Entity& result, Entity::ContainerT::const_iterator it) const;
- void addResultSnapshot(Entity& result, const EntitySnapshot& snapshot) const;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.cpp
deleted file mode 100644
index 129892339524ca76cc602dbea1b8cfe0524b3a1a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-#include "MemoryQueryProcessor.h"
-
-#include <regex>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
-namespace armarx::armem::query_proc
-{
-
- armem::query::data::Result MemoryQueryProcessor::process(const armem::query::data::Input& input, const Memory& memory) const
- {
- armem::query::data::Result result;
- result.memory = processToIce(input.memoryQueries, memory);
- result.success = true;
- return result;
- }
-
- data::MemoryPtr MemoryQueryProcessor::processToIce(
- const armem::query::data::MemoryQuery& query, const Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(query, memory));
- }
-
- data::MemoryPtr MemoryQueryProcessor::processToIce(const armem::query::data::MemoryQuerySeq& queries, const Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(queries, memory));
- }
-
- void MemoryQueryProcessor::process(Memory& result, const armem::query::data::MemoryQuery& query, const Memory& memory) const
- {
- if (auto q = dynamic_cast<const armem::query::data::memory::All*>(&query))
- {
- process(result, *q, memory);
- }
- else if (auto q = dynamic_cast<const armem::query::data::memory::Single*>(&query))
- {
- process(result, *q, memory);
- }
- else if (auto q = dynamic_cast<const armem::query::data::memory::Regex*>(&query))
- {
- process(result, *q, memory);
- }
- else
- {
- throw armem::error::UnknownQueryType("memory segment", query);
- }
- }
-
-
- void MemoryQueryProcessor::process(Memory& result, const armem::query::data::memory::All& query, const Memory& memory) const
- {
- for (const auto& [name, coreSegment] : memory.coreSegments)
- {
- result.addCoreSegment(coreSegmentProcessor.process(query.coreSegmentQueries, coreSegment));
- }
- }
-
- void MemoryQueryProcessor::process(Memory& result, const armem::query::data::memory::Single& query, const Memory& memory) const
- {
- try
- {
- const CoreSegment& coreSegment = memory.getCoreSegment(query.coreSegmentName);
- result.addCoreSegment(coreSegmentProcessor.process(query.coreSegmentQueries, coreSegment));
- }
- catch (const error::MissingEntry&)
- {
- // Leave empty.
- }
- }
-
- void MemoryQueryProcessor::process(Memory& result, const armem::query::data::memory::Regex& query, const Memory& memory) const
- {
- std::regex regex(query.coreSegmentNameRegex);
- for (const auto& [name, coreSegment] : memory.coreSegments)
- {
- if (std::regex_search(coreSegment.name(), regex))
- {
- result.addCoreSegment(coreSegmentProcessor.process(query.coreSegmentQueries, coreSegment));
- }
- }
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.h
deleted file mode 100644
index de16c9433bdf5af97fee33e929930519cc490187..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/MemoryQueryProcessor.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-
-#include "BaseQueryProcessor.h"
-#include "CoreSegmentQueryProcessor.h"
-
-
-
-namespace armarx::armem::query_proc
-{
-
- /**
- * @brief Handles memory queries.
- */
- class MemoryQueryProcessor : public BaseQueryProcessor<Memory, armem::query::data::MemoryQuery>
- {
- public:
-
- MemoryQueryProcessor(DataMode dataMode = DataMode::WithData) :
- BaseQueryProcessor(dataMode), coreSegmentProcessor(dataMode)
- {}
-
-
- using BaseQueryProcessor::process;
-
- armem::query::data::Result process(const armem::query::data::Input& input, const Memory& memory) const;
-
-
- void process(Memory& result, const armem::query::data::MemoryQuery& query, const Memory& memory) const;
-
- void process(Memory& result, const armem::query::data::memory::All& query, const Memory& memory) const;
- void process(Memory& result, const armem::query::data::memory::Single& query, const Memory& memory) const;
- void process(Memory& result, const armem::query::data::memory::Regex& query, const Memory& memory) const;
-
-
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuery& query, const Memory& memory) const;
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuerySeq& query, const Memory& memory) const;
-
-
- private:
-
- CoreSegmentQueryProcessor coreSegmentProcessor;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.cpp
deleted file mode 100644
index 36bfe8765fe2fedb092b93e4a6d607ab1a026cda..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-#include "ProviderSegmentQueryProcessor.h"
-
-#include <regex>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
-namespace armarx::armem::query_proc
-{
-
- void ProviderSegmentQueryProcessor::process(ProviderSegment& result, const armem::query::data::ProviderSegmentQuery& query, const ProviderSegment& providerSegment) const
- {
- if (auto q = dynamic_cast<const armem::query::data::provider::All*>(&query))
- {
- process(result, *q, providerSegment);
- }
- else if (auto q = dynamic_cast<const armem::query::data::provider::Single*>(&query))
- {
- process(result, *q, providerSegment);
- }
- else if (auto q = dynamic_cast<const armem::query::data::provider::Regex*>(&query))
- {
- process(result, *q, providerSegment);
- }
- else
- {
- throw armem::error::UnknownQueryType("provider segment", query);
- }
- }
-
-
- void ProviderSegmentQueryProcessor::process(ProviderSegment& result,
- const armem::query::data::provider::All& query, const ProviderSegment& providerSegment) const
- {
- for (const auto& [name, entity] : providerSegment.entities)
- {
- result.addEntity(entityProcessor.process(query.entityQueries, entity));
- }
- }
-
- void ProviderSegmentQueryProcessor::process(ProviderSegment& result,
- const armem::query::data::provider::Single& query, const ProviderSegment& providerSegment) const
- {
- try
- {
- const Entity& entity = providerSegment.getEntity(query.entityName);
- result.addEntity(entityProcessor.process(query.entityQueries, entity));
- }
- catch (const error::MissingEntry&)
- {
- // Leave empty.
- }
- }
-
- void ProviderSegmentQueryProcessor::process(ProviderSegment& result,
- const armem::query::data::provider::Regex& query, const ProviderSegment& providerSegment) const
- {
- std::regex regex(query.entityNameRegex);
- for (const auto& [name, entity] : providerSegment.entities)
- {
- if (std::regex_search(entity.name(), regex))
- {
- result.addEntity(entityProcessor.process(query.entityQueries, entity));
- }
- }
- }
-
-
- data::ProviderSegment ProviderSegmentQueryProcessor::processToIce(
- const armem::query::data::ProviderSegmentQuery& query, const ProviderSegment& providerSegment) const
- {
- return toIce<data::ProviderSegment>(process(query, providerSegment));
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.h
deleted file mode 100644
index da5c200bfbe66752aff5bb88324e44220bb7faea..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/ProviderSegmentQueryProcessor.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#pragma once
-
-#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-
-#include "BaseQueryProcessor.h"
-#include "EntityQueryProcessor.h"
-
-
-
-namespace armarx::armem::query_proc
-{
-
- /**
- * @brief Handles memory queries.
- */
- class ProviderSegmentQueryProcessor : public BaseQueryProcessor<ProviderSegment, armem::query::data::ProviderSegmentQuery>
- {
- public:
-
- ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
- BaseQueryProcessor(dataMode), entityProcessor(dataMode)
- {}
-
-
- using BaseQueryProcessor::process;
-
- void process(ProviderSegment& result, const armem::query::data::ProviderSegmentQuery& query, const ProviderSegment& providerSegment) const;
-
- void process(ProviderSegment& result, const armem::query::data::provider::All& query, const ProviderSegment& providerSegment) const;
- void process(ProviderSegment& result, const armem::query::data::provider::Single& query, const ProviderSegment& providerSegment) const;
- void process(ProviderSegment& result, const armem::query::data::provider::Regex& query, const ProviderSegment& providerSegment) const;
-
-
- data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const ProviderSegment& providerSegment) const;
-
-
- private:
-
- EntityQueryProcessor entityProcessor;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce437fd67bdd2ba3aa6ac9943093525db99a581e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "BaseQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
similarity index 82%
rename from source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.h
rename to source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 7858b2b7120fb5239c375d0c398334d8381c538b..3ea2b0bdd45230dab810bd628cf338d2b685662b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -5,37 +5,28 @@
#include <RobotAPI/libraries/armem/core/DataMode.h>
-namespace armarx::armem::query_proc
+namespace armarx::armem::base::query_proc
{
/**
* @brief Base class for memory query processors.
*/
template <class DataT, class QueryT>
- class BaseQueryProcessor
+ class BaseQueryProcessorBase
{
public:
-
using QueryPtrT = ::IceInternal::Handle<QueryT>;
using QuerySeqT = std::vector<QueryPtrT>;
-
public:
- BaseQueryProcessor(DataMode dataMode = DataMode::WithData) :
- dataMode(dataMode)
- {}
-
- virtual ~BaseQueryProcessor()
- {}
-
-
DataT process(const QueryT& query, const DataT& data) const
{
DataT result = data.copyEmpty();
this->process(result, query, data);
return result;
}
+
DataT process(const QueryPtrT& query, const DataT& data) const
{
return this->process(*query, *data);
@@ -48,7 +39,6 @@ namespace armarx::armem::query_proc
return result;
}
-
void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
{
for (const auto& query : queries)
@@ -58,12 +48,6 @@ namespace armarx::armem::query_proc
}
virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
-
-
- protected:
-
- DataMode dataMode;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63e7ff349207a6891d26c0dde081572f0e53d3a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "CoreSegmentQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..f8ab8c113e510e57a1ab2b53af6a06e60e393ce1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -0,0 +1,100 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/query.h>
+
+#include "BaseQueryProcessorBase.h"
+#include "ProviderSegmentQueryProcessorBase.h"
+
+#include <regex>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+
+namespace armarx::armem::base::query_proc
+{
+
+ /**
+ * @brief Handles memory queries.
+ */
+ template <class _CoreSegmentT>
+ class CoreSegmentQueryProcessorBase :
+ virtual public BaseQueryProcessorBase<_CoreSegmentT, armem::query::data::CoreSegmentQuery>
+ {
+ using Base = BaseQueryProcessorBase<_CoreSegmentT, armem::query::data::CoreSegmentQuery>;
+
+ public:
+ using CoreSegmentT = _CoreSegmentT;
+ using ProviderSegmentT = typename _CoreSegmentT::ProviderSegmentT;
+ using EntityT = typename ProviderSegmentT::EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ void process(_CoreSegmentT& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const _CoreSegmentT& coreSegment) const override
+ {
+ if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
+ {
+ process(result, *q, coreSegment);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::core::Single*>(&query))
+ {
+ process(result, *q, coreSegment);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::core::Regex*>(&query))
+ {
+ process(result, *q, coreSegment);
+ }
+ else
+ {
+ throw armem::error::UnknownQueryType("core segment", query);
+ }
+ }
+
+ void process(_CoreSegmentT& result,
+ const armem::query::data::core::All& query,
+ const _CoreSegmentT& coreSegment) const
+ {
+ for (const auto& [name, providerSegment] : coreSegment.providerSegments())
+ {
+ result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ }
+ }
+
+ void process(_CoreSegmentT& result,
+ const armem::query::data::core::Single& query,
+ const _CoreSegmentT& coreSegment) const
+ {
+ try
+ {
+ const ProviderSegmentT& providerSegment = coreSegment.getProviderSegment(query.providerSegmentName);
+ result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ }
+ catch (const error::MissingEntry&)
+ {
+ // Leave empty.
+ }
+ }
+
+ void process(_CoreSegmentT& result,
+ const armem::query::data::core::Regex& query,
+ const _CoreSegmentT& coreSegment) const
+ {
+ std::regex regex(query.providerSegmentNameRegex);
+ for (const auto& [name, providerSegment] : coreSegment.providerSegments())
+ {
+ if (std::regex_search(providerSegment.name(), regex))
+ {
+ result.addProviderSegment(providerSegmentProcessorProcess(query.providerSegmentQueries, providerSegment));
+ }
+ }
+ }
+
+ protected:
+ virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& a, const ProviderSegmentT& o) const = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5cbac85a8dc5f8899e7c72434516063121a2b4c1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "EntityQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..447bd259a2c24a3ca80c91c59a518daed56c18f1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -0,0 +1,174 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/query.h>
+
+#include "BaseQueryProcessorBase.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+
+namespace armarx::armem::base::query_proc
+{
+
+ /**
+ * @brief Handles memory queries.
+ */
+ template <class _EntityT>
+ class EntityQueryProcessorBase :
+ virtual public BaseQueryProcessorBase<_EntityT, armem::query::data::EntityQuery>
+ {
+ using Base = BaseQueryProcessorBase<_EntityT, armem::query::data::EntityQuery>;
+
+ public:
+ using EntityT = _EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ void process(_EntityT& result,
+ const armem::query::data::EntityQuery& query,
+ const _EntityT& entity) const override
+ {
+ if (auto q = dynamic_cast<const armem::query::data::entity::All*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::Single*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::TimeRange*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::IndexRange*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else
+ {
+ throw armem::error::UnknownQueryType("entity snapshot", query);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::All& query,
+ const _EntityT& entity) const
+ {
+ (void) query;
+ // Copy this entitiy and its contents.
+
+ for (const auto& [time, snapshot] : entity)
+ {
+ addResultSnapshot(result, snapshot);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::Single& query,
+ const _EntityT& entity) const
+ {
+ if (query.timestamp < 0)
+ {
+ try
+ {
+ addResultSnapshot(result, entity.getLatestSnapshot());
+ }
+ catch (const armem::error::EntityHistoryEmpty&)
+ {
+ // Leave empty.
+ }
+ }
+ else
+ {
+ Time time = fromIce<Time>(query.timestamp);
+ try
+ {
+ addResultSnapshot(result, entity.getSnapshot(time));
+ }
+ catch (const armem::error::MissingEntry&)
+ {
+ // Leave empty.
+ }
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::TimeRange& query,
+ const _EntityT& entity) const
+ {
+ if (query.minTimestamp <= query.maxTimestamp || query.minTimestamp < 0 || query.maxTimestamp < 0)
+ {
+ Time min = fromIce<Time>(query.minTimestamp);
+ Time max = fromIce<Time>(query.maxTimestamp);
+ process(result, min, max, entity, query);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::IndexRange& query,
+ const _EntityT& entity) const
+ {
+ if (entity.empty())
+ {
+ return;
+ }
+
+ size_t first = negativeIndexSemantics(query.first, entity.history().size());
+ size_t last = negativeIndexSemantics(query.last, entity.history().size());
+
+ if (first <= last)
+ {
+ auto it = entity.begin();
+ std::advance(it, first);
+
+ size_t num = last - first + 1; // +1 to make last inclusive
+ for (size_t i = 0; i < num; ++i, ++it)
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+ }
+
+ void process(_EntityT& result,
+ const Time& min,
+ const Time& max,
+ const _EntityT& entity,
+ const armem::query::data::EntityQuery& query) const
+ {
+ (void) query;
+
+ // Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
+ auto begin = min.toMicroSeconds() > 0 ? entity.history().lower_bound(min) : entity.history().begin();
+ // Returns an iterator pointing to the first element that is *greater than* key.
+ auto end = max.toMicroSeconds() > 0 ? entity.history().upper_bound(max) : entity.history().end();
+
+ for (auto it = begin; it != end && it != entity.history().end(); ++it)
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+
+
+ static size_t negativeIndexSemantics(long index, size_t size)
+ {
+ const size_t max = size > 0 ? size - 1 : 0;
+ if (index >= 0)
+ {
+ return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
+ }
+ else
+ {
+ return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
+ }
+ }
+
+ protected:
+ virtual void addResultSnapshot(_EntityT& result, typename _EntityT::ContainerT::const_iterator it) const = 0;
+ virtual void addResultSnapshot(_EntityT& result, const typename _EntityT::EntitySnapshotT& snapshot) const = 0;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c51a6336c440e44fe637df89700136687d03f294
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "MemoryQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..c50086b553164b70f10ed1559a18420649eb3892
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -0,0 +1,98 @@
+#pragma once
+
+#include <regex>
+
+#include <RobotAPI/interface/armem/query.h>
+
+#include "BaseQueryProcessorBase.h"
+#include "CoreSegmentQueryProcessorBase.h"
+
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::base::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ template <class _MemoryT>
+ class MemoryQueryProcessorBase :
+ virtual public BaseQueryProcessorBase<_MemoryT, armem::query::data::MemoryQuery>
+ {
+ using Base = BaseQueryProcessorBase<_MemoryT, armem::query::data::MemoryQuery>;
+
+ public:
+ using MemoryT = _MemoryT;
+ using CoreSegmentT = typename MemoryT::CoreSegmentT;
+ using ProviderSegmentT = typename CoreSegmentT::ProviderSegmentT;
+ using EntityT = typename ProviderSegmentT::EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ void process(_MemoryT& result,
+ const armem::query::data::MemoryQuery& query,
+ const _MemoryT& memory) const override
+ {
+ if (auto q = dynamic_cast<const armem::query::data::memory::All*>(&query))
+ {
+ process(result, *q, memory);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::memory::Single*>(&query))
+ {
+ process(result, *q, memory);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::memory::Regex*>(&query))
+ {
+ process(result, *q, memory);
+ }
+ else
+ {
+ throw armem::error::UnknownQueryType("memory segment", query);
+ }
+ }
+
+ void process(_MemoryT& result,
+ const armem::query::data::memory::All& query,
+ const _MemoryT& memory) const
+ {
+ for (const auto& [name, coreSegment] : memory.coreSegments())
+ {
+ result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
+ }
+ }
+
+ void process(_MemoryT& result,
+ const armem::query::data::memory::Single& query,
+ const _MemoryT& memory) const
+ {
+ try
+ {
+ const CoreSegmentT& coreSegment = memory.getCoreSegment(query.coreSegmentName);
+ result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
+ }
+ catch (const error::MissingEntry&)
+ {
+ }
+ }
+
+ void process(_MemoryT& result,
+ const armem::query::data::memory::Regex& query,
+ const _MemoryT& memory) const
+ {
+ std::regex regex(query.coreSegmentNameRegex);
+ for (const auto& [name, coreSegment] : memory.coreSegments())
+ {
+ if (std::regex_search(coreSegment.name(), regex))
+ {
+ result.addCoreSegment(coreSegmentProcessorProcess(query.coreSegmentQueries, coreSegment));
+ }
+ }
+ }
+
+ protected:
+ virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& a, const CoreSegmentT& o) const = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..67db9e1429d8aa9c797790f4290a9feb512d41c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "ProviderSegmentQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3b7da7ffa9cc1d06aeadabef58c5aebba3974de
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -0,0 +1,98 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/query.h>
+
+#include "BaseQueryProcessorBase.h"
+#include "EntityQueryProcessorBase.h"
+
+#include <regex>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::base::query_proc
+{
+
+ /**
+ * @brief Handles memory queries.
+ */
+ template <class _ProviderSegmentT>
+ class ProviderSegmentQueryProcessorBase :
+ virtual public BaseQueryProcessorBase<_ProviderSegmentT, armem::query::data::ProviderSegmentQuery>
+ {
+ using Base = BaseQueryProcessorBase<_ProviderSegmentT, armem::query::data::ProviderSegmentQuery>;
+
+ public:
+ using ProviderSegmentT = _ProviderSegmentT;
+ using EntityT = typename ProviderSegmentT::EntityT;
+ using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ void process(_ProviderSegmentT& result,
+ const armem::query::data::ProviderSegmentQuery& query,
+ const _ProviderSegmentT& providerSegment) const override
+ {
+ if (auto q = dynamic_cast<const armem::query::data::provider::All*>(&query))
+ {
+ process(result, *q, providerSegment);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::provider::Single*>(&query))
+ {
+ process(result, *q, providerSegment);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::provider::Regex*>(&query))
+ {
+ process(result, *q, providerSegment);
+ }
+ else
+ {
+ throw armem::error::UnknownQueryType("provider segment", query);
+ }
+ }
+
+ void process(_ProviderSegmentT& result,
+ const armem::query::data::provider::All& query,
+ const _ProviderSegmentT& providerSegment) const
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ result.addEntity(entityProcessorProcess(query.entityQueries, entity));
+ }
+ }
+
+ void process(_ProviderSegmentT& result,
+ const armem::query::data::provider::Single& query,
+ const _ProviderSegmentT& providerSegment) const
+ {
+ try
+ {
+ const EntityT& entity = providerSegment.getEntity(query.entityName);
+ result.addEntity(entityProcessorProcess(query.entityQueries, entity));
+ }
+ catch (const error::MissingEntry&)
+ {
+ // Leave empty.
+ }
+ }
+
+ void process(_ProviderSegmentT& result,
+ const armem::query::data::provider::Regex& query,
+ const _ProviderSegmentT& providerSegment) const
+ {
+ std::regex regex(query.entityNameRegex);
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (std::regex_search(entity.name(), regex))
+ {
+ result.addEntity(entityProcessorProcess(query.entityQueries, entity));
+ }
+ }
+ }
+
+ protected:
+ virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& a, const EntityT& o) const = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..68821bcd95cdf2aaccf7126d4055495d39d0393d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "BaseQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..458128e5305139db33a6c25d6ff19e3e92772fd5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/DataMode.h>
+
+#include "../base/BaseQueryProcessorBase.h"
+
+
+namespace armarx::armem::ltm::query_proc
+{
+ /**
+ * @brief Base class for memory query processors.
+ */
+ template <class DataT, class QueryT>
+ class BaseQueryProcessor :
+ virtual public base::query_proc::BaseQueryProcessorBase<DataT, QueryT>
+ {
+ using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
+
+ public:
+ BaseQueryProcessor()
+ {}
+
+ protected:
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "CoreSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc379f1d88fa82bbe886e412e0341079fa43fce9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/CoreSegmentQueryProcessorBase.h"
+
+#include "../../../core/longtermmemory/CoreSegment.h"
+
+#include "ProviderSegmentQueryProcessor.h"
+
+
+namespace armarx::armem::ltm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class CoreSegmentQueryProcessor :
+ virtual public BaseQueryProcessor<ltm::CoreSegment, armem::query::data::CoreSegmentQuery>,
+ virtual public base::query_proc::CoreSegmentQueryProcessorBase<ltm::CoreSegment>
+ {
+ using Base = BaseQueryProcessor<ltm::CoreSegment, armem::query::data::CoreSegmentQuery>;
+
+ public:
+ CoreSegmentQueryProcessor() :
+ Base()
+ {}
+
+ protected:
+ virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
+ {
+ return providerSegmentProcessor.process(q, s);
+ }
+
+ private:
+ ProviderSegmentQueryProcessor providerSegmentProcessor;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1c321b026b173c6552758fb9d8b9fdf722ea5a4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "EntityQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ec4c5dde481e16f0af30589d57ff2a447d81221
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/EntityQueryProcessorBase.h"
+
+#include "../../../core/longtermmemory/Entity.h"
+
+#include "EntityQueryProcessor.h"
+
+namespace armarx::armem::ltm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class EntityQueryProcessor :
+ virtual public BaseQueryProcessor<ltm::Entity, armem::query::data::EntityQuery>,
+ virtual public base::query_proc::EntityQueryProcessorBase<ltm::Entity>
+ {
+ using Base = BaseQueryProcessor<ltm::Entity, armem::query::data::EntityQuery>;
+
+ public:
+ EntityQueryProcessor() :
+ Base()
+ {}
+
+ private:
+ void addResultSnapshot(ltm::Entity& result, ltm::Entity::ContainerT::const_iterator it) const override
+ {
+ addResultSnapshot(result, it->second);
+ }
+
+ void addResultSnapshot(ltm::Entity& result, const ltm::EntitySnapshot& snapshot) const override
+ {
+ result.addSnapshot(snapshot.copy());
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69b04de6c9e623286a5bda836dddfdc8b551b64a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "MemoryQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5129417262515975efa247398602272865ee5ec
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/MemoryQueryProcessorBase.h"
+
+#include "../../../core/longtermmemory/Memory.h"
+
+#include "CoreSegmentQueryProcessor.h"
+
+namespace armarx::armem::ltm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class MemoryQueryProcessor :
+ virtual public BaseQueryProcessor<ltm::Memory, armem::query::data::MemoryQuery>,
+ virtual public base::query_proc::MemoryQueryProcessorBase<ltm::Memory>
+ {
+ using Base = BaseQueryProcessor<ltm::Memory, armem::query::data::MemoryQuery>;
+
+ public:
+ MemoryQueryProcessor() :
+ Base()
+ {}
+
+ protected:
+ virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
+ {
+ return coreSegmentProcessor.process(q, s);
+ }
+
+ private:
+ CoreSegmentQueryProcessor coreSegmentProcessor;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a2a4405001f0904b74fc6afcf96813eef0879cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0c6db9e3ef06e6241e9af8c572bb7fc78eec452
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/ProviderSegmentQueryProcessorBase.h"
+
+#include "../../../core/longtermmemory/ProviderSegment.h"
+
+#include "EntityQueryProcessor.h"
+
+namespace armarx::armem::ltm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class ProviderSegmentQueryProcessor :
+ virtual public BaseQueryProcessor<ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>,
+ virtual public base::query_proc::ProviderSegmentQueryProcessorBase<ltm::ProviderSegment>
+ {
+ using Base = BaseQueryProcessor<ltm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
+
+ public:
+ ProviderSegmentQueryProcessor() :
+ Base()
+ {}
+
+ protected:
+ virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
+ {
+ return entityProcessor.process(q, s);
+ }
+
+ private:
+ EntityQueryProcessor entityProcessor;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..68821bcd95cdf2aaccf7126d4055495d39d0393d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "BaseQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..99811dcc72d168dfecb0c5864f0760c4d5634a6e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+
+#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/DataMode.h>
+
+#include "../base/BaseQueryProcessorBase.h"
+
+
+namespace armarx::armem::wm::query_proc
+{
+ /**
+ * @brief Base class for memory query processors.
+ */
+ template <class DataT, class QueryT>
+ class BaseQueryProcessor :
+ virtual public base::query_proc::BaseQueryProcessorBase<DataT, QueryT>
+ {
+ using Base = base::query_proc::BaseQueryProcessorBase<DataT, QueryT>;
+
+ public:
+ BaseQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ dataMode(dataMode)
+ {}
+
+ protected:
+ DataMode dataMode;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..afbe35ad15eea0342b0b0d4df0200aaf0d32aa2a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "CoreSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..079f63ac59928f2bd4ace9ab7ab033bef93c04a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/CoreSegmentQueryProcessorBase.h"
+
+#include "ProviderSegmentQueryProcessor.h"
+
+
+namespace armarx::armem::wm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class CoreSegmentQueryProcessor :
+ virtual public BaseQueryProcessor<wm::CoreSegment, armem::query::data::CoreSegmentQuery>,
+ virtual public base::query_proc::CoreSegmentQueryProcessorBase<wm::CoreSegment>
+ {
+ using Base = BaseQueryProcessor<wm::CoreSegment, armem::query::data::CoreSegmentQuery>;
+
+ public:
+ CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ Base(dataMode), providerSegmentProcessor(dataMode)
+ {}
+
+ using Base::process;
+ data::CoreSegment processToIce(const armem::query::data::CoreSegmentQuery& query, const wm::CoreSegment& coreSegment) const
+ {
+ return toIce<data::CoreSegment>(process(query, coreSegment));
+ }
+
+ protected:
+ virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
+ {
+ return providerSegmentProcessor.process(q, s);
+ }
+
+ private:
+ ProviderSegmentQueryProcessor providerSegmentProcessor;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1c321b026b173c6552758fb9d8b9fdf722ea5a4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "EntityQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..94dc905e489be501257be508cff03c7efee8f8b8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/EntityQueryProcessorBase.h"
+
+#include "EntityQueryProcessor.h"
+
+namespace armarx::armem::wm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class EntityQueryProcessor :
+ virtual public BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>,
+ virtual public base::query_proc::EntityQueryProcessorBase<wm::Entity>
+ {
+ using Base = BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>;
+
+ public:
+ EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ Base(dataMode)
+ {}
+
+ data::Entity processToIce(const armem::query::data::EntityQuery& query, const wm::Entity& entity) const
+ {
+ return toIce<data::Entity>(process(query, entity));
+ }
+
+ private:
+ void addResultSnapshot(wm::Entity& result, wm::Entity::ContainerT::const_iterator it) const override
+ {
+ addResultSnapshot(result, it->second);
+ }
+
+ void addResultSnapshot(wm::Entity& result, const wm::EntitySnapshot& snapshot) const override
+ {
+ bool withData = (dataMode == DataMode::WithData);
+ if (withData)
+ {
+ result.addSnapshot(snapshot.copy());
+ }
+ else
+ {
+ result.addSnapshot(snapshot.copyWithoutData());
+ }
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69b04de6c9e623286a5bda836dddfdc8b551b64a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "MemoryQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..b017c3b561569c1da5d3d7f661ee604bbb1cae12
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/MemoryQueryProcessorBase.h"
+
+#include "CoreSegmentQueryProcessor.h"
+
+namespace armarx::armem::wm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class MemoryQueryProcessor :
+ virtual public BaseQueryProcessor<wm::Memory, armem::query::data::MemoryQuery>,
+ virtual public base::query_proc::MemoryQueryProcessorBase<wm::Memory>
+ {
+ using Base = BaseQueryProcessor<wm::Memory, armem::query::data::MemoryQuery>;
+
+ public:
+ MemoryQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ Base(dataMode), coreSegmentProcessor(dataMode)
+ {}
+
+ armem::query::data::Result process(const armem::query::data::Input& input,
+ const wm::Memory& memory) const
+ {
+ armem::query::data::Result result;
+ result.memory = processToIce(input.memoryQueries, memory);
+ result.success = true;
+ return result;
+ }
+
+ using Base::process;
+ data::MemoryPtr processToIce(const armem::query::data::MemoryQuery& query,
+ const wm::Memory& memory) const
+ {
+ return toIce<data::MemoryPtr>(process(query, memory));
+ }
+
+ data::MemoryPtr processToIce(const armem::query::data::MemoryQuerySeq& queries,
+ const wm::Memory& memory) const
+ {
+ return toIce<data::MemoryPtr>(process(queries, memory));
+ }
+
+ protected:
+ virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
+ {
+ return coreSegmentProcessor.process(q, s);
+ }
+
+
+ private:
+ CoreSegmentQueryProcessor coreSegmentProcessor;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a2a4405001f0904b74fc6afcf96813eef0879cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
@@ -0,0 +1 @@
+#include "ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b2e9ba97850ed1f62f2d761a51996f3b2bb2665
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "BaseQueryProcessor.h"
+#include "../base/ProviderSegmentQueryProcessorBase.h"
+
+#include "EntityQueryProcessor.h"
+
+namespace armarx::armem::wm::query_proc
+{
+ /**
+ * @brief Handles memory queries.
+ */
+ class ProviderSegmentQueryProcessor :
+ virtual public BaseQueryProcessor<wm::ProviderSegment, armem::query::data::ProviderSegmentQuery>,
+ virtual public base::query_proc::ProviderSegmentQueryProcessorBase<wm::ProviderSegment>
+ {
+ using Base = BaseQueryProcessor<wm::ProviderSegment, armem::query::data::ProviderSegmentQuery>;
+
+ public:
+ ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData) :
+ Base(dataMode), entityProcessor(dataMode)
+ {}
+
+ using Base::process;
+ data::ProviderSegment processToIce(const armem::query::data::ProviderSegmentQuery& query, const wm::ProviderSegment& providerSegment) const
+ {
+ return toIce<data::ProviderSegment>(process(query, providerSegment));
+ }
+
+ protected:
+ virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
+ {
+ return entityProcessor.process(q, s);
+ }
+
+ private:
+ EntityQueryProcessor entityProcessor;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
index e139931d828f69494299b69fb4bd87677fd40465..a835f39bef74f745b22c4ee167e4755a031fec50 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
@@ -25,7 +25,8 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../core/ice_conversions.h"
+#include "../core/workingmemory/ice_conversions.h"
+
#include <iostream>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -37,11 +38,11 @@ namespace aron = armarx::aron;
BOOST_AUTO_TEST_CASE(test_entity)
{
- armem::Entity entity("entity");
+ armem::wm::Entity entity("entity");
std::vector<armem::Time> expectedTimestamps;
- armem::EntitySnapshot snapshot;
+ armem::wm::EntitySnapshot snapshot;
snapshot.time() = armem::Time::milliSeconds(100);
expectedTimestamps.push_back(snapshot.time());
entity.addSnapshot(snapshot);
@@ -53,13 +54,19 @@ BOOST_AUTO_TEST_CASE(test_entity)
armem::data::EntityPtr ice;
armem::toIce(ice, entity);
- armem::Entity entityOut;
+ 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());
+
+ armem::wm::Entity entityOut;
armem::fromIce(ice, entityOut);
- BOOST_CHECK_EQUAL(entityOut.history.size(), entity.history.size());
+ BOOST_CHECK_EQUAL(entityOut.history().size(), entity.history().size());
std::vector<armem::Time> timestamps = entityOut.getTimestamps();
- BOOST_CHECK_EQUAL_COLLECTIONS(timestamps.begin(), timestamps.end(),
- expectedTimestamps.begin(), expectedTimestamps.end());
+ BOOST_CHECK_EQUAL_COLLECTIONS(timestamps.begin(), timestamps.end(), expectedTimestamps.begin(), expectedTimestamps.end());
}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
index dea566c5ee235868f23334f381095e1949f4f044..f6271553eddfe787162841ba7c428f7f74aa1626 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
@@ -25,16 +25,19 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../core/Memory.h"
+#include "../core/workingmemory/Memory.h"
#include "../core/error.h"
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/armem/core/io/MemoryFileSystemStorage.h>
#include <RobotAPI/libraries/aron/core/Randomizer.h>
#include <RobotAPI/libraries/aron/core/Debug.h>
+#include "../core/longtermmemory/Memory.h"
+
+//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
+
#include <filesystem>
#include <iostream>
@@ -46,15 +49,26 @@ namespace ArMemLTMTest
{
struct Fixture
{
- fs::path storagePath = "./TestMemoryExport";
+ fs::path storagePath = "/tmp/TestMemoryExport";
Fixture()
{
clearStoragePath();
+ assureStoragePath();
}
~Fixture()
{
- clearStoragePath();
+ //clearStoragePath();
+ }
+
+ void assureStoragePath()
+ {
+ if (!fs::is_directory(storagePath))
+ {
+ fs::create_directory(storagePath);
+ }
+ BOOST_TEST_INFO("Storage path: " << storagePath);
+ BOOST_REQUIRE(fs::exists(storagePath) && fs::is_directory(storagePath));
}
void clearStoragePath()
@@ -68,7 +82,7 @@ namespace ArMemLTMTest
}
- static armem::Memory setupMemoryWithType(
+ static armem::wm::Memory setupMemoryWithType(
const std::string& memoryName,
const aron::typenavigator::ObjectNavigatorPtr& t1,
const aron::typenavigator::ObjectNavigatorPtr& t2,
@@ -77,19 +91,19 @@ namespace ArMemLTMTest
{
aron::Randomizer r;
- armem::Memory memory(memoryName);
+ armem::wm::Memory memory(memoryName);
BOOST_CHECK_EQUAL(memory.name(), memoryName);
- armem::CoreSegment& coreSegment = memory.addCoreSegment("TestCoreSegment");
+ armem::wm::CoreSegment& coreSegment = memory.addCoreSegment("TestCoreSegment");
BOOST_CHECK_EQUAL(coreSegment.name(), "TestCoreSegment");
BOOST_CHECK(memory.hasCoreSegment(coreSegment.name()));
- armem::ProviderSegment& providerSegment = coreSegment.addProviderSegment("TestProvider");
+ armem::wm::ProviderSegment& providerSegment = coreSegment.addProviderSegment("TestProvider");
BOOST_CHECK_EQUAL(providerSegment.name(), "TestProvider");
BOOST_CHECK(coreSegment.hasProviderSegment(providerSegment.name()));
- coreSegment.aronType() = t1;
- providerSegment.aronType() = t2;
+ //coreSegment.aronType() = t1;
+ //providerSegment.aronType() = t2;
aron::typenavigator::ObjectNavigatorPtr t = t2 != nullptr ? t2 : t1;
@@ -108,7 +122,7 @@ namespace ArMemLTMTest
update.instancesData = q;
update.timeCreated = armem::Time::now();
BOOST_CHECK_NO_THROW(providerSegment.update(update));
- BOOST_CHECK_EQUAL(providerSegment.entities.size(), 1);
+ BOOST_CHECK_EQUAL(providerSegment.entities().size(), 1);
}
BOOST_CHECK(providerSegment.hasEntity("TestEntity"));
@@ -128,186 +142,72 @@ namespace ArMemLTMTest
return t;
}
- };
-}
-BOOST_FIXTURE_TEST_SUITE(ArMemLTMTest, Fixture)
+ template <class TypeNavigatorT>
+ void run(const std::string& memoryName, const std::string& memberNamePrefix)
+ {
+ /*aron::typenavigator::ObjectNavigatorPtr t = makeType<TypeNavigatorT>(memberNamePrefix);
+ armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-/*
-BOOST_AUTO_TEST_CASE(test_memory_export__easy_int_setup)
-{
- std::string memoryName = "TestMemory_IntSetup";
+ // create reader and writer
+ armem::io::DiskWriterPtr w = std::make_shared<armem::io::NlohmannJSONDiskWriter>();
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronIntTypeNavigator>("theInt");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
+ // export memory
+ BOOST_REQUIRE(fs::exists(storagePath));
+ armem::io::DiskMemory dMem(memory);
+ std::string mfs_str = dMem.toString();
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
+ armem::io::DiskMemory dMem2();
+ std::string mfs2_str = dMem2.toString();
- std::string mfs_str = mfs.toString();
+ //std::cout << "MFS1: " << std::endl;
+ //std::cout << mfs_str << std::endl;
+ //std::cout << "MFS2: " << std::endl;
+ //std::cout << mfs2_str << std::endl;
+ BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
+ armem::Memory memory2 = dMem2.toMemory();
+ memory2.name() = memoryName;
- std::string mfs2_str = mfs2.toString();
+ BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);*/
+ }
+ };
+}
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
+BOOST_FIXTURE_TEST_SUITE(ArMemLTMTest, Fixture)
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+
+BOOST_AUTO_TEST_CASE(test_memory_export__easy_int_setup)
+{
+ run<aron::typenavigator::IntNavigator>("TestMemory_IntSetup", "theInt");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_long_setup)
{
- std::string memoryName = "TestMemory_LongSetup";
-
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronLongTypeNavigator>("theLong");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
-
- std::string mfs_str = mfs.toString();
-
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
-
- std::string mfs2_str = mfs2.toString();
-
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
-
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+ run<aron::typenavigator::LongNavigator>("TestMemory_LongSetup", "theLong");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_float_setup)
{
- std::string memoryName = "TestMemory_FloatSetup";
-
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronFloatTypeNavigator>("theFloat");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
-
- std::string mfs_str = mfs.toString();
-
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
-
- std::string mfs2_str = mfs2.toString();
-
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
-
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+ run<aron::typenavigator::FloatNavigator>("TestMemory_FloatSetup", "theFloat");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_double_setup)
{
- std::string memoryName = "TestMemory_DoubleSetup";
-
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronDoubleTypeNavigator>("theDouble");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
-
- std::string mfs_str = mfs.toString();
-
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
-
- std::string mfs2_str = mfs2.toString();
-
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
-
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+ run<aron::typenavigator::DoubleNavigator>("TestMemory_DoubleSetup", "theDouble");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_string_setup)
{
- std::string memoryName = "TestMemory_StringSetup";
-
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronStringTypeNavigator>("theString");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
-
- std::string mfs_str = mfs.toString();
-
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
-
- std::string mfs2_str = mfs2.toString();
-
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
-
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+ run<aron::typenavigator::StringNavigator>("TestMemory_StringSetup", "theString");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_bool_setup)
{
- std::string memoryName = "TestMemory_BoolSetup";
-
- aron::typenavigator::ObjectNavigatorPtr t = makeType<aron::typenavigator::AronBoolTypeNavigator>("theBool");
- armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
-
- // export memory
- BOOST_REQUIRE(!fs::exists(storagePath));
- armem::io::FileSystemMemoryManager mfs(storagePath, true);
- mfs.writeOnDisk(memory);
-
- std::string mfs_str = mfs.toString();
-
- armem::io::FileSystemMemoryManager mfs2(storagePath);
- mfs2.update();
-
- std::string mfs2_str = mfs2.toString();
-
- //std::cout << "MFS1: " << std::endl;
- //std::cout << mfs_str << std::endl;
- //std::cout << "MFS2: " << std::endl;
- //std::cout << mfs2_str << std::endl;
- BOOST_CHECK_EQUAL(mfs_str == mfs2_str, true);
-
- armem::Memory memory2 = mfs.readMemoryFromDisk(memoryName);
- BOOST_CHECK_EQUAL(memory.equalsDeep(memory2), true);
+ run<aron::typenavigator::BoolNavigator>("TestMemory_BoolSetup", "theBool");
}
+/*
BOOST_AUTO_TEST_CASE(test_memory_export__easy_rainer_setup)
{
std::string memoryName = "TestMemory_RainerSetup";
@@ -316,17 +216,17 @@ BOOST_AUTO_TEST_CASE(test_memory_export__easy_rainer_setup)
t->setObjectName("TestRainerType1");
aron::typenavigator::DictNavigatorPtr tm1(new aron::typenavigator::DictNavigator(aron::Path()));
- aron::typenavigator::AronFloatTypeNavigatorPtr tm1m1(new aron::typenavigator::AronFloatTypeNavigator(aron::Path()));
+ aron::typenavigator::FloatNavigatorPtr tm1m1(new aron::typenavigator::FloatNavigator(aron::Path()));
tm1->setAcceptedType(tm1m1);
- aron::typenavigator::AronStringTypeNavigatorPtr tm2(new aron::typenavigator::AronStringTypeNavigator(aron::Path()));
+ aron::typenavigator::StringNavigatorPtr tm2(new aron::typenavigator::StringNavigator(aron::Path()));
aron::typenavigator::DictNavigatorPtr tm3(new aron::typenavigator::DictNavigator(aron::Path()));
- aron::typenavigator::AronStringTypeNavigatorPtr tm3m1(new aron::typenavigator::AronStringTypeNavigator(aron::Path()));
+ aron::typenavigator::StringNavigatorPtr tm3m1(new aron::typenavigator::StringNavigator(aron::Path()));
tm3->setAcceptedType(tm3m1);
- aron::typenavigator::AronStringTypeNavigatorPtr tm4(new aron::typenavigator::AronStringTypeNavigator(aron::Path()));
- t->addAcceptedType("float_params", tm1);
- t->addAcceptedType("name", tm2);
- //t->addAcceptedType("string_params", tm3);
- t->addAcceptedType("type", tm4);
+ aron::typenavigator::StringNavigatorPtr tm4(new aron::typenavigator::StringNavigator(aron::Path()));
+ t->addMemberType("float_params", tm1);
+ t->addMemberType("name", tm2);
+ //t->addMemberType("string_params", tm3);
+ t->addMemberType("type", tm4);
armem::Memory memory = setupMemoryWithType(memoryName, t, nullptr, 15, 1);
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 b7ba95c633d7d561024b848c1afa862e0bc51caa..7588230d8bab0f9fc42093c01382497ddcfc402b 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/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,11 +78,541 @@ 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);
+
+ if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ {
+ in.aronType() = std::make_shared<aron::typenavigator::ObjectNavigator>();
+ BOOST_CHECK(in.aronType());
+ }
+ }
+
+ 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);
+
+ if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ {
+ BOOST_CHECK(in.aronType());
+ BOOST_CHECK(out.aronType());
+ BOOST_CHECK_EQUAL(in.aronType(), out.aronType());
+ }
+
+ 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);
+
+ if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ {
+ BOOST_CHECK(in.aronType());
+ BOOST_CHECK(out.aronType());
+ BOOST_CHECK_EQUAL(in.aronType(), out.aronType());
+ }
+
+ 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);
+
+ if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ {
+ BOOST_CHECK(out.aronType());
+ BOOST_CHECK_EQUAL(in.aronType(), nullptr);
+ }
+
+ 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);
+
+ if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ {
+ BOOST_CHECK(out.aronType());
+ BOOST_CHECK_EQUAL(in.aronType(), nullptr);
+ }
+
+ 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;
- armem::Memory memory("Memory");
+ armem::wm::Memory memory("Memory");
BOOST_CHECK_EQUAL(memory.name(), "Memory");
{
update.entityID = armem::MemoryID::fromString("OtherMemory/SomeSegment");
@@ -89,7 +621,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
BOOST_CHECK_THROW(memory.update(update), armem::error::MissingEntry);
}
- armem::CoreSegment& coreSegment = memory.addCoreSegment("ImageRGB");
+ armem::wm::CoreSegment& coreSegment = memory.addCoreSegment("ImageRGB");
BOOST_CHECK_EQUAL(coreSegment.name(), "ImageRGB");
BOOST_CHECK(memory.hasCoreSegment(coreSegment.name()));
{
@@ -99,7 +631,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
BOOST_CHECK_THROW(coreSegment.update(update), armem::error::MissingEntry);
}
- armem::ProviderSegment& providerSegment = coreSegment.addProviderSegment("SomeRGBImageProvider");
+ armem::wm::ProviderSegment& providerSegment = coreSegment.addProviderSegment("SomeRGBImageProvider");
BOOST_CHECK_EQUAL(providerSegment.name(), "SomeRGBImageProvider");
BOOST_CHECK(coreSegment.hasProviderSegment(providerSegment.name()));
{
@@ -119,17 +651,17 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
update.timeCreated = armem::Time::milliSeconds(1000);
BOOST_CHECK_NO_THROW(providerSegment.update(update));
- BOOST_CHECK_EQUAL(providerSegment.entities.size(), 1);
+ BOOST_CHECK_EQUAL(providerSegment.entities().size(), 1);
BOOST_CHECK(providerSegment.hasEntity("image"));
BOOST_CHECK(!providerSegment.hasEntity("other_image"));
- armem::Entity& entity = providerSegment.getEntity("image");
+ armem::wm::Entity& entity = providerSegment.getEntity("image");
BOOST_CHECK_EQUAL(entity.name(), "image");
- BOOST_CHECK_EQUAL(entity.history.size(), 1);
- BOOST_CHECK_EQUAL(entity.history.count(update.timeCreated), 1);
+ BOOST_CHECK_EQUAL(entity.history().size(), 1);
+ BOOST_CHECK_EQUAL(entity.history().count(update.timeCreated), 1);
- armem::EntitySnapshot& entitySnapshot = entity.history.at(update.timeCreated);
- BOOST_CHECK_EQUAL(entitySnapshot.instances.size(), update.instancesData.size());
+ armem::wm::EntitySnapshot& entitySnapshot = entity.history().at(update.timeCreated);
+ BOOST_CHECK_EQUAL(entitySnapshot.instances().size(), update.instancesData.size());
// Another update (on memory).
@@ -137,16 +669,16 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
update.timeCreated = armem::Time::milliSeconds(2000);
memory.update(update);
- BOOST_CHECK_EQUAL(entity.history.size(), 2);
- BOOST_CHECK_EQUAL(entity.history.count(update.timeCreated), 1);
- BOOST_CHECK_EQUAL(entity.history.at(update.timeCreated).instances.size(), update.instancesData.size());
+ BOOST_CHECK_EQUAL(entity.history().size(), 2);
+ BOOST_CHECK_EQUAL(entity.history().count(update.timeCreated), 1);
+ BOOST_CHECK_EQUAL(entity.history().at(update.timeCreated).instances().size(), update.instancesData.size());
// A third update (on entity).
update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
update.timeCreated = armem::Time::milliSeconds(3000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history.size(), 3);
+ BOOST_CHECK_EQUAL(entity.history().size(), 3);
}
@@ -154,7 +686,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
{
- armem::Entity entity("entity");
+ armem::wm::Entity entity("entity");
armem::EntityUpdate update;
update.entityID.entityName = entity.name();
@@ -166,38 +698,38 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
entity.update(update);
update.timeCreated = armem::Time::milliSeconds(3000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history.size(), 3);
+ BOOST_CHECK_EQUAL(entity.history().size(), 3);
// Now with maximum history size.
entity.setMaxHistorySize(2);
- BOOST_CHECK_EQUAL(entity.history.size(), 2);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(1000)), 0);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(2000)), 1);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(3000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().size(), 2);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(1000)), 0);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(2000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
update.timeCreated = armem::Time::milliSeconds(4000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history.size(), 2);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(2000)), 0);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(3000)), 1);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(4000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().size(), 2);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(2000)), 0);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(4000)), 1);
// Disable maximum history size.
entity.setMaxHistorySize(-1);
update.timeCreated = armem::Time::milliSeconds(5000);
entity.update(update);
- BOOST_CHECK_EQUAL(entity.history.size(), 3);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(3000)), 1);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(4000)), 1);
- BOOST_CHECK_EQUAL(entity.history.count(armem::Time::milliSeconds(5000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().size(), 3);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(3000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(4000)), 1);
+ BOOST_CHECK_EQUAL(entity.history().count(armem::Time::milliSeconds(5000)), 1);
}
BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
{
- armem::ProviderSegment providerSegment("SomeRGBImageProvider");
+ armem::wm::ProviderSegment providerSegment("SomeRGBImageProvider");
armem::EntityUpdate update;
update.entityID.providerSegmentName = providerSegment.name();
@@ -220,22 +752,22 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
update.timeCreated = armem::Time::milliSeconds(4000);
providerSegment.update(update);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history.size(), 3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history.size(), 4);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 4);
// Employ maximum history size.
providerSegment.setMaxHistorySize(3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history.size(), 3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history.size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 3);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 3);
providerSegment.setMaxHistorySize(2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history.size(), 2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history.size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 2);
providerSegment.setMaxHistorySize(3);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history.size(), 2);
- BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history.size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("A").history().size(), 2);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("B").history().size(), 2);
// Add new entity.
providerSegment.setMaxHistorySize(2);
@@ -249,9 +781,9 @@ 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);
+ BOOST_CHECK_EQUAL(providerSegment.getEntity("C").history().size(), 2);
// Remove maximum.
providerSegment.setMaxHistorySize(-1);
@@ -262,6 +794,58 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
update.entityID.entityName = name;
update.timeCreated = armem::Time::milliSeconds(5000);
providerSegment.update(update);
- BOOST_CHECK_EQUAL(providerSegment.getEntity(name).history.size(), 3);
+ 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/test/ArMemQueryBuilderTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
index 5b9a09f097cc55f3e8343b55c58d0aa09109e890..535735b09eeb61d41cdf0dbaf658cebcdb08f4db 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
@@ -27,7 +27,7 @@
#include <RobotAPI/Test.h>
#include "../client/query/Builder.h"
#include "../client/query/query_fns.h"
-#include "../core/ice_conversions.h"
+#include "../core/workingmemory/ice_conversions.h"
#include <iostream>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index b35fb39452fd014e7d9a7bec6353ca181b5a14e2..42d357b005ae9c4c4663bc72fa91ed4fc459b78a 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -25,7 +25,7 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../server/query_proc/EntityQueryProcessor.h"
+#include "../server/query_proc/workingmemory/EntityQueryProcessor.h"
#include "../error.h"
@@ -38,7 +38,7 @@
namespace armem = armarx::armem;
namespace aron = armarx::aron;
namespace query = armarx::armem::query::data;
-using EntityQueryProcessor = armarx::armem::query_proc::EntityQueryProcessor;
+using EntityQueryProcessor = armarx::armem::wm::query_proc::EntityQueryProcessor;
namespace ArMemQueryTest
@@ -50,19 +50,19 @@ namespace ArMemQueryTest
struct Fixture
{
- armem::Entity entity;
+ armem::wm::Entity entity;
- armem::query_proc::EntityQueryProcessor processor;
+ armem::wm::query_proc::EntityQueryProcessor processor;
query::EntityQueryPtr entityQuery;
- std::vector<armem::Entity> results;
+ std::vector<armem::wm::Entity> results;
Fixture()
{
- entity = armem::Entity("entity");
+ entity = armem::wm::Entity("entity");
- armem::EntitySnapshot snapshot;
+ armem::wm::EntitySnapshot snapshot;
snapshot.time() = armem::Time::microSeconds(1000);
entity.addSnapshot(snapshot);
snapshot.time() = armem::Time::microSeconds(2000);
@@ -119,12 +119,12 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
addResults(query::entity::Single());
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 1);
- BOOST_CHECK_EQUAL(result.history.begin()->second.time(), entity.getLatestSnapshot().time());
- BOOST_CHECK_NE(&result.history.begin()->second, &entity.history.rbegin()->second);
+ BOOST_CHECK_EQUAL(result.history().begin()->second.time(), entity.getLatestSnapshot().time());
+ BOOST_CHECK_NE(&result.history().begin()->second, &entity.history().rbegin()->second);
}
}
@@ -134,11 +134,11 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
addResults(query::entity::Single { 3000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 1);
- BOOST_CHECK_EQUAL(result.history.begin()->second.time(), armem::Time::microSeconds(3000));
+ BOOST_CHECK_EQUAL(result.history().begin()->second.time(), armem::Time::microSeconds(3000));
}
}
@@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
addResults(query::entity::Single { 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 0);
@@ -161,12 +161,12 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
addResults(query::entity::All());
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), entity.size());
- auto jt = entity.history.begin();
- for (auto it = result.history.begin(); it != result.history.end(); ++it, ++jt)
+ auto jt = entity.history().begin();
+ for (auto it = result.history().begin(); it != result.history().end(); ++it, ++jt)
{
BOOST_CHECK_EQUAL(it->first, jt->first);
BOOST_CHECK_EQUAL(it->second.time(), jt->second.time());
@@ -182,13 +182,13 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
addResults(query::entity::TimeRange{ 1500, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 2);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
@@ -203,13 +203,13 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
addResults(query::entity::TimeRange{ 2000, 4000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
@@ -224,14 +224,14 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
addResults(query::entity::TimeRange{ -1, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), entity.size());
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
- std::vector<armem::Time> expected = simox::alg::get_keys(entity.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> expected = simox::alg::get_keys(entity.history());
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
@@ -243,7 +243,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
addResults(query::entity::TimeRange{ 6000, 1000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
@@ -257,13 +257,13 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
addResults(query::entity::TimeRange{ -1, 2500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 2);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
std::vector<armem::Time> expected
{
armem::Time::microSeconds(1000), armem::Time::microSeconds(2000)
@@ -278,13 +278,13 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
addResults(query::entity::TimeRange{ 2500, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
std::vector<armem::Time> expected
{
armem::Time::microSeconds(3000), armem::Time::microSeconds(4000), armem::Time::microSeconds(5000)
@@ -332,14 +332,14 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
addResults(query::entity::IndexRange(0, -1));
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), entity.size());
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
- std::vector<armem::Time> expected = simox::alg::get_keys(entity.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ std::vector<armem::Time> expected = simox::alg::get_keys(entity.history());
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -355,13 +355,13 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
addResults(query::entity::IndexRange{ -4, -2 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 3);
- std::vector<armem::Time> times = simox::alg::get_keys(result.history);
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
std::vector<armem::Time> expected
{
armem::Time::microSeconds(2000), armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
@@ -383,7 +383,7 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
@@ -405,7 +405,7 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
BOOST_REQUIRE_GT(results.size(), 0);
- for (const armem::Entity& result : results)
+ for (const armem::wm::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 0);
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 4fa457ab679a0f970974101a53ba04a92b3fc241..91a31651cf055572dd6c3e8fcbb27163e4d30918 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -26,8 +26,8 @@
#include <vector>
#include <optional>
-#include <RobotAPI/libraries/armem/core/Entity.h>
-#include <RobotAPI/libraries/armem/core/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
namespace armarx::armem
@@ -41,7 +41,7 @@ namespace armarx::armem
* @return std::optional<AronClass>
*/
template <typename AronClass>
- std::optional<AronClass> tryCast(const EntityInstance& item)
+ std::optional<AronClass> tryCast(const wm::EntityInstance& item)
{
static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
AronClass>::value);
@@ -67,7 +67,7 @@ namespace armarx::armem
*/
template <typename AronClass>
std::vector<AronClass>
- allOfType(const std::map<std::string, Entity>& entities)
+ allOfType(const std::map<std::string, wm::Entity>& entities)
{
static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
AronClass>::value);
@@ -77,9 +77,9 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- for (const auto &[ss, entitySnapshot] : entity.history)
+ for (const auto &[ss, entitySnapshot] : entity.history())
{
- for (const auto& entityInstance : entitySnapshot.instances)
+ for (const auto& entityInstance : entitySnapshot.instances())
{
const auto o = tryCast<AronClass>(entityInstance);
@@ -113,7 +113,7 @@ namespace armarx::armem
* @return vector of "pred"-transformed elements that can be cast to AronClass
*/
template <typename AronClass>
- auto transformAllOfType(const std::map<std::string, Entity>& entities,
+ auto transformAllOfType(const std::map<std::string, wm::Entity>& entities,
auto pred) -> std::vector<decltype(pred(AronClass()))>
{
static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
@@ -129,16 +129,16 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- if (entity.history.empty())
+ if (entity.history().empty())
{
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.history.size();
+ ARMARX_INFO << "History size: " << entity.history().size();
- for (const auto &[ss, entitySnapshot] : entity.history)
+ for (const auto &[ss, entitySnapshot] : entity.history())
{
- for (const auto& entityInstance : entitySnapshot.instances)
+ for (const auto& entityInstance : entitySnapshot.instances())
{
const auto o = tryCast<AronClass>(entityInstance);
@@ -153,4 +153,4 @@ namespace armarx::armem
return outV;
}
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 4b49856774ca0fd7236ad259b2b2346094c0501b..d90a90aa04a3fcc2c3d891bfab8727a991522f44 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -10,22 +10,33 @@ set(LIBRARIES
# ArmarXGui
SimpleConfigDialog
# RobotAPI
- armem
+ aroneigenconverter armem
)
set(SOURCES
MemoryViewer.cpp
PeriodicUpdateWidget.cpp
+ LTMControlWidget.cpp
gui_utils.cpp
lifecycle.cpp
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
@@ -38,6 +49,7 @@ set(HEADERS
MemoryViewer.h
PeriodicUpdateWidget.h
+ LTMControlWidget.h
TreeWidgetBuilder.h
gui_utils.h
lifecycle.h
@@ -45,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/LTMControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/LTMControlWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..257e43d3a3f0df224b4632449c97aa4336c9f184
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/LTMControlWidget.cpp
@@ -0,0 +1,60 @@
+#include "LTMControlWidget.h"
+
+#include <QPushButton>
+#include <QLineEdit>
+#include <QTimer>
+#include <QHBoxLayout>
+
+#include <cmath>
+
+
+namespace armarx::armem::gui
+{
+
+ LTMControlWidget::LTMControlWidget()
+ {
+ setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
+
+ QLayout* layout = new QHBoxLayout();
+ this->setLayout(layout);
+
+ const int margin = 0;
+ layout->setContentsMargins(margin, margin, margin, margin);
+
+ _lineEdit = new QLineEdit("/tmp/MemoryExport", this);
+ _exportHereButton = new QPushButton("Export here", this);
+
+ _storeButton = new QPushButton("Store in LTM", this);
+
+ layout->addWidget(_lineEdit);
+ layout->addWidget(_storeButton);
+ layout->addWidget(_exportHereButton);
+
+ // Private connections.
+
+ // Public connections.
+ connect(_storeButton, &QPushButton::pressed, this, &This::store);
+ connect(_exportHereButton, &QPushButton::pressed, this, &This::exportHere);
+ }
+
+ QLineEdit* LTMControlWidget::pathInputBox()
+ {
+ return _lineEdit;
+ }
+
+ QString LTMControlWidget::getEnteredPath()
+ {
+ return _lineEdit->text();
+ }
+
+ QPushButton* LTMControlWidget::storeButton()
+ {
+ return _storeButton;
+ }
+
+ QPushButton* LTMControlWidget::exportHereButton()
+ {
+ return _exportHereButton;
+ }
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/LTMControlWidget.h b/source/RobotAPI/libraries/armem_gui/LTMControlWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c827b01173aa7b724fcfe7e700691d5a093e36e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/LTMControlWidget.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <QWidget>
+
+class QPushButton;
+class QLineEdit;
+
+namespace armarx::armem::gui
+{
+
+ class LTMControlWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = LTMControlWidget;
+
+ public:
+
+ LTMControlWidget();
+
+ QLineEdit* pathInputBox();
+ QString getEnteredPath();
+
+ QPushButton* storeButton();
+ QPushButton* exportHereButton();
+
+ public slots:
+
+ signals:
+
+ void store();
+ void exportHere();
+
+ private slots:
+
+
+ signals:
+
+
+ private:
+
+ QLineEdit* _lineEdit;
+
+ QPushButton* _storeButton;
+ QPushButton* _exportHereButton;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 78097af1a7a40ca0e8c46f1e75d8ce72783e06ab..447b35332a60a3442b63e07a2c5e129b5b7b83d1 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -1,8 +1,10 @@
#include "MemoryViewer.h"
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
+#include <RobotAPI/libraries/armem/core/diskmemory/Memory.h>
+
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -19,15 +21,18 @@
#include <QLayout>
#include <QSettings>
+#include <filesystem>
+
namespace armarx::armem::gui
{
-
MemoryViewer::MemoryViewer(
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");
@@ -39,15 +44,30 @@ namespace armarx::armem::gui
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);
+ connect(ltmControlWidget, &armem::gui::LTMControlWidget::exportHere, this, &This::exportHere);
connect(this, &This::connected, this, &This::updateMemories);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemories);
@@ -107,6 +127,50 @@ namespace armarx::armem::gui
emit disconnected();
}
+ void MemoryViewer::store()
+ {
+ TIMING_START(MemoryStore);
+
+ for (auto& [name, reader] : memoryReaders)
+ {
+ data::StoreInput input;
+ input.query = memoryGroup->queryWidget()->queryInput().toIce();
+ reader.readAndStore(input);
+ }
+
+ TIMING_END_STREAM(MemoryStore, ARMARX_VERBOSE);
+ }
+
+ void MemoryViewer::exportHere()
+ {
+ TIMING_START(MemoryExport);
+ QString qs = ltmControlWidget->getEnteredPath();
+ std::string utf8_text = qs.toUtf8().constData();
+
+ ARMARX_IMPORTANT << "Exporting all memories at '" << utf8_text << "'.";
+
+ std::filesystem::path p(utf8_text);
+ if (std::filesystem::is_regular_file(p))
+ {
+ ARMARX_WARNING << "Could not export a memory at '" << utf8_text << "'. Skipping export.";
+ return;
+ }
+
+ std::filesystem::create_directories(p);
+ for (auto& [name, reader] : memoryReaders)
+ {
+ armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
+ armem::client::QueryResult result = reader.query(input);
+
+ armem::d_ltm::Memory dMem(name);
+ ARMARX_IMPORTANT << "RELOAD";
+ dMem.reload(p / name);
+ ARMARX_IMPORTANT << "APPEND";
+ dMem.append(result.memory);
+ }
+
+ TIMING_END_STREAM(MemoryExport, ARMARX_VERBOSE);
+ }
void MemoryViewer::updateMemories()
{
@@ -177,7 +241,7 @@ namespace armarx::armem::gui
return;
}
- const std::optional<armem::Memory>& data = memoryData.at(selectedID.memoryName);
+ const std::optional<armem::wm::Memory>& data = memoryData.at(selectedID.memoryName);
if (data)
{
@@ -186,10 +250,10 @@ namespace armarx::armem::gui
return;
}
armem::MemoryID id = selectedID;
- const armem::EntitySnapshot* snapshot = nullptr;
+ const armem::wm::EntitySnapshot* snapshot = nullptr;
if (!id.hasTimestamp())
{
- const armem::Entity& entity = data->getEntity(id);
+ const armem::wm::Entity& entity = data->getEntity(id);
if (entity.empty())
{
return;
@@ -227,7 +291,7 @@ namespace armarx::armem::gui
void MemoryViewer::updateMemoryTree()
{
- std::map<std::string, const armem::Memory*> convMap;
+ std::map<std::string, const armem::wm::Memory*> convMap;
for (auto& [name, data] : memoryData)
{
if (data.has_value())
@@ -251,13 +315,12 @@ namespace armarx::armem::gui
}
}
-
const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryNameSystem";
const static std::string CONFIG_KEY_DEBUG_OBSERVER = "MemoryViewer.DebugObserverName";
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)
@@ -268,12 +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 5c8f4b2f182f798a3c076a89e6a30ff24d10050c..19ab5000d9d662080b80e621144650fd887a0bcd 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -13,7 +13,7 @@
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
#include <RobotAPI/libraries/armem_gui/memory/GroupBox.h>
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
-
+#include <RobotAPI/libraries/armem_gui/LTMControlWidget.h>
class QBoxLayout;
class QDialog;
@@ -45,8 +45,9 @@ namespace armarx::armem::gui
MemoryViewer(
QBoxLayout* updateWidgetLayout,
- QGroupBox* _memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* _instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* ltmControlWidgetLayout,
+ QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
QLabel* statusLabel
);
@@ -62,6 +63,11 @@ namespace armarx::armem::gui
public slots:
+ // LTMControlWidget
+ void store();
+ void exportHere();
+
+ // Other
void updateMemories();
void updateInstanceTree(const armem::MemoryID& selectedID);
@@ -98,15 +104,17 @@ namespace armarx::armem::gui
armem::mns::MemoryNameSystemInterfacePrx mns;
std::map<std::string, armem::client::Reader> memoryReaders;
- std::map<std::string, std::optional<armem::Memory>> memoryData;
+ std::map<std::string, std::optional<armem::wm::Memory>> memoryData;
+ 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 83d528d4bee1946760a56e74688d1eca348a31ca..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
@@ -89,10 +91,10 @@ namespace armarx::armem::gui::instance
}
- void InstanceView::update(const MemoryID& id, const Memory& memory)
+ void InstanceView::update(const MemoryID& id, const wm::Memory& memory)
{
aron::typenavigator::ObjectNavigatorPtr aronType = nullptr;
- const armem::EntityInstance* instance = nullptr;
+ const armem::wm::EntityInstance* instance = nullptr;
try
{
instance = &memory.getEntityInstance(id);
@@ -113,7 +115,7 @@ namespace armarx::armem::gui::instance
}
- void InstanceView::update(const EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
+ void InstanceView::update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
{
currentInstance = instance;
currentAronType = aronType;
@@ -147,28 +149,28 @@ 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);
}
- void InstanceView::updateMetaData(const EntityInstanceMetadata& metadata)
+ void InstanceView::updateMetaData(const wm::EntityInstanceMetadata& metadata)
{
std::vector<std::string> items =
{
@@ -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/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 00027a3e0cc4c826e24c62262bf4404f645e1bdf..0b0a0ee2655f506fa09f15b99dd0edc149e88470 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -6,7 +6,7 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
class QGroupBox;
@@ -34,8 +34,8 @@ namespace armarx::armem::gui::instance
void setStatusLabel(QLabel* statusLabel);
void setUseTypeInfo(bool enable);
- void update(const MemoryID& id, const Memory& memory);
- void update(const EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ void update(const MemoryID& id, const wm::Memory& memory);
+ void update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
void update();
@@ -56,7 +56,7 @@ namespace armarx::armem::gui::instance
void updateInstanceID(const MemoryID& id);
void updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
- void updateMetaData(const EntityInstanceMetadata& metadata);
+ void updateMetaData(const wm::EntityInstanceMetadata& metadata);
void updateImageView(const aron::datanavigator::DictNavigatorPtr& data);
void showErrorMessage(const std::string& message);
@@ -71,7 +71,7 @@ namespace armarx::armem::gui::instance
TYPE = 2,
};
- std::optional<EntityInstance> currentInstance;
+ std::optional<wm::EntityInstance> currentInstance;
aron::typenavigator::ObjectNavigatorPtr currentAronType = nullptr;
bool useTypeInfo = true;
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 e622a3a44601e6abc1e2e7f428f58c2dd1346169..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>
@@ -49,12 +51,12 @@ namespace armarx::armem::gui::memory
void TreeWidget::initBuilders()
{
- memoryBuilder.setExpand(true);
- memoryBuilder.setMakeItemFn([this](const std::string & name, const Memory * memory)
+ workingmemoryBuilder.setExpand(true);
+ workingmemoryBuilder.setMakeItemFn([this](const std::string & name, const wm::Memory * memory)
{
return makeItem(name, *memory);
});
- memoryBuilder.setUpdateItemFn([this](const std::string&, const Memory * memory, QTreeWidgetItem * memoryItem)
+ workingmemoryBuilder.setUpdateItemFn([this](const std::string&, const wm::Memory * memory, QTreeWidgetItem * memoryItem)
{
updateContainerItem(*memory, memoryItem);
if (memoryItem)
@@ -65,71 +67,71 @@ namespace armarx::armem::gui::memory
});
- coreSegmentBuilder.setExpand(true);
- coreSegmentBuilder.setMakeItemFn([this](const std::string & name, const CoreSegment & coreSeg)
+ workingmemoryCoreSegmentBuilder.setExpand(true);
+ workingmemoryCoreSegmentBuilder.setMakeItemFn([this](const std::string & name, const wm::CoreSegment & coreSeg)
{
return makeItem(name, coreSeg);
});
- coreSegmentBuilder.setUpdateItemFn([this](const std::string&, const CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
+ workingmemoryCoreSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
{
- updateTypedContainerItem(coreSeg, coreSegItem);
+ updateContainerItem(coreSeg, coreSegItem);
updateChildren(coreSeg, coreSegItem);
return true;
});
- provSegmentBuilder.setExpand(true);
- provSegmentBuilder.setMakeItemFn([this](const std::string & name, const ProviderSegment & provSeg)
+ workingmemoryProvSegmentBuilder.setExpand(true);
+ workingmemoryProvSegmentBuilder.setMakeItemFn([this](const std::string & name, const wm::ProviderSegment & provSeg)
{
return makeItem(name, provSeg);
});
- provSegmentBuilder.setUpdateItemFn([this](const std::string&, const ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
+ workingmemoryProvSegmentBuilder.setUpdateItemFn([this](const std::string&, const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
{
- updateTypedContainerItem(provSeg, provSegItem);
+ updateContainerItem(provSeg, provSegItem);
updateChildren(provSeg, provSegItem);
return true;
});
// entityBuilder.setExpand(true);
- entityBuilder.setMakeItemFn([this](const std::string & name, const Entity & entity)
+ workingmemoryEntityBuilder.setMakeItemFn([this](const std::string & name, const wm::Entity & entity)
{
return makeItem(name, entity);
});
- entityBuilder.setUpdateItemFn([this](const std::string&, const Entity & entity, QTreeWidgetItem * entityItem)
+ workingmemoryEntityBuilder.setUpdateItemFn([this](const std::string&, const wm::Entity & entity, QTreeWidgetItem * entityItem)
{
updateContainerItem(entity, entityItem);
updateChildren(entity, entityItem);
return true;
});
- snapshotBuilder.setMakeItemFn([this](const armem::Time & time, const EntitySnapshot & snapshot)
+ workingmemorySnapshotBuilder.setMakeItemFn([this](const armem::Time & time, const wm::EntitySnapshot & snapshot)
{
QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(time), snapshot);
item->setData(int(Columns::KEY), Qt::ItemDataRole::UserRole, QVariant(static_cast<qlonglong>(snapshot.time().toMicroSeconds())));
return item;
});
- snapshotBuilder.setCompareFn([](const auto & pair, QTreeWidgetItem * item)
+ workingmemorySnapshotBuilder.setCompareFn([](const auto & pair, QTreeWidgetItem * item)
{
- const EntitySnapshot& snapshot = pair.second;
+ const wm::EntitySnapshot& snapshot = pair.second;
return armarx::detail::compare(static_cast<qlonglong>(snapshot.time().toMicroSeconds()),
item->data(int(Columns::KEY), Qt::ItemDataRole::UserRole).toLongLong());
});
- snapshotBuilder.setUpdateItemFn([this](const armem::Time&, const EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
+ workingmemorySnapshotBuilder.setUpdateItemFn([this](const armem::Time&, const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
{
updateContainerItem(snapshot, snapshotItem);
updateChildren(snapshot, snapshotItem);
return true;
});
- instanceBuilder.setMakeItemFn([this](const EntityInstance & instance)
+ workingmemoryInstanceBuilder.setMakeItemFn([this](const wm::EntityInstance & instance)
{
QTreeWidgetItem* item = makeItem("", instance);
return item;
});
- instanceBuilder.setCompareFn([](const EntityInstance & lhs, QTreeWidgetItem * rhsItem)
+ workingmemoryInstanceBuilder.setCompareFn([](const wm::EntityInstance & lhs, QTreeWidgetItem * rhsItem)
{
return armarx::detail::compare(lhs.index(), rhsItem->text(0).toInt());
});
- instanceBuilder.setUpdateItemFn([this](const EntityInstance & instance, QTreeWidgetItem * instanceItem)
+ workingmemoryInstanceBuilder.setUpdateItemFn([this](const wm::EntityInstance & instance, QTreeWidgetItem * instanceItem)
{
updateItemItem(instance, instanceItem);
updateChildren(instance, instanceItem);
@@ -139,7 +141,7 @@ namespace armarx::armem::gui::memory
}
- void TreeWidget::update(const armem::Memory& memory)
+ void TreeWidget::update(const armem::wm::Memory& memory)
{
// Removing elements during the update can create unwanted signals triggering selection handling.
handleSelections = false;
@@ -148,7 +150,7 @@ namespace armarx::armem::gui::memory
emit updated();
}
- void TreeWidget::update(const std::map<std::string, const armem::Memory*>& memories)
+ void TreeWidget::update(const std::map<std::string, const armem::wm::Memory*>& memories)
{
handleSelections = false;
updateChildren(memories, this);
@@ -181,27 +183,27 @@ namespace armarx::armem::gui::memory
_selectedID = id;
const std::string levelName = item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString();
- if (levelName == Memory().getLevelName())
+ if (levelName == wm::Memory().getLevelName())
{
emit memorySelected(*_selectedID);
}
- else if (levelName == CoreSegment().getLevelName())
+ else if (levelName == wm::CoreSegment().getLevelName())
{
emit coreSegmentSelected(*_selectedID);
}
- else if (levelName == ProviderSegment().getLevelName())
+ else if (levelName == wm::ProviderSegment().getLevelName())
{
emit providerSegmentSelected(*_selectedID);
}
- else if (levelName == Entity().getLevelName())
+ else if (levelName == wm::Entity().getLevelName())
{
emit entitySelected(*_selectedID);
}
- else if (levelName == EntitySnapshot().getLevelName())
+ else if (levelName == wm::EntitySnapshot().getLevelName())
{
emit snapshotSelected(*_selectedID);
}
- else if (levelName == EntityInstance().getLevelName())
+ else if (levelName == wm::EntityInstance().getLevelName())
{
emit instanceSelected(*_selectedID);
}
@@ -212,48 +214,48 @@ namespace armarx::armem::gui::memory
}
- void TreeWidget::updateChildren(const armem::Memory& memory, QTreeWidget* tree)
+ void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree)
{
- updateChildren(std::map<std::string, const armem::Memory*> {{memory.name(), &memory}}, tree);
+ updateChildren(std::map<std::string, const armem::wm::Memory*> {{memory.name(), &memory}}, tree);
}
- void TreeWidget::updateChildren(const std::map<std::string, const armem::Memory*>& memories, QTreeWidget* tree)
+ void TreeWidget::updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories, QTreeWidget* tree)
{
- memoryBuilder.updateTree(tree, memories);
+ workingmemoryBuilder.updateTree(tree, memories);
}
- void TreeWidget::updateChildren(const armem::Memory& memory, QTreeWidgetItem* memoryItem)
+ void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem)
{
- coreSegmentBuilder.updateTree(memoryItem, memory.coreSegments);
+ workingmemoryCoreSegmentBuilder.updateTree(memoryItem, memory.coreSegments());
}
- void TreeWidget::updateChildren(const armem::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
+ void TreeWidget::updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
{
- provSegmentBuilder.updateTree(coreSegItem, coreSeg.providerSegments);
+ workingmemoryProvSegmentBuilder.updateTree(coreSegItem, coreSeg.providerSegments());
}
- void TreeWidget::updateChildren(const armem::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
+ void TreeWidget::updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
{
- entityBuilder.updateTree(provSegItem, provSeg.entities);
+ workingmemoryEntityBuilder.updateTree(provSegItem, provSeg.entities());
}
- void TreeWidget::updateChildren(const armem::Entity& entity, QTreeWidgetItem* entityItem)
+ void TreeWidget::updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem)
{
- snapshotBuilder.updateTree(entityItem, entity.history);
+ workingmemorySnapshotBuilder.updateTree(entityItem, entity.history());
}
- void TreeWidget::updateChildren(const armem::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
+ void TreeWidget::updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
{
- instanceBuilder.updateTree(snapshotItem, snapshot.instances);
+ workingmemoryInstanceBuilder.updateTree(snapshotItem, snapshot.instances());
}
- void TreeWidget::updateChildren(const armem::EntityInstance& data, QTreeWidgetItem* dataItem)
+ void TreeWidget::updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* dataItem)
{
(void) data, (void) dataItem;
}
- QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const armem::detail::MemoryItem& memoryItem)
+ QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const armem::base::detail::MemoryItem& memoryItem)
{
(void) key;
QStringList columns;
@@ -270,44 +272,33 @@ namespace armarx::armem::gui::memory
return item;
}
- void TreeWidget::updateItemItem(const armem::detail::MemoryItem& level, QTreeWidgetItem* item)
+ void TreeWidget::updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item)
{
(void) level, (void) item;
}
template <class... T>
void TreeWidget::updateContainerItem(
- const armem::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::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 be73a68b6a9fb482c57f8262c3fb514638dfb3d9..2bcf0fa1b6135534407044a3ce341b86afeab442 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -4,7 +4,9 @@
#include <QTreeWidget>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
@@ -20,8 +22,8 @@ namespace armarx::armem::gui::memory
TreeWidget();
- void update(const armem::Memory& memory);
- void update(const std::map<std::string, const armem::Memory*>& memories);
+ void update(const armem::wm::Memory& memory);
+ void update(const std::map<std::string, const armem::wm::Memory*>& memories);
std::optional<MemoryID> selectedID() const;
@@ -54,34 +56,32 @@ namespace armarx::armem::gui::memory
void initWidget();
void initBuilders();
- void updateChildren(const armem::Memory& memory, QTreeWidget* tree);
- void updateChildren(const std::map<std::string, const armem::Memory*>& memories, QTreeWidget* tree);
+ void updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree);
+ void updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories, QTreeWidget* tree);
- void updateChildren(const armem::Memory& memory, QTreeWidgetItem* memoryItem);
- void updateChildren(const armem::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem);
- void updateChildren(const armem::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem);
- void updateChildren(const armem::Entity& entity, QTreeWidgetItem* entityItem);
- void updateChildren(const armem::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem);
- void updateChildren(const armem::EntityInstance& data, QTreeWidgetItem* parent);
+ void updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem);
+ void updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem);
+ void updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem);
+ void updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem);
+ void updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem);
+ void updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* parent);
- QTreeWidgetItem* makeItem(const std::string& key, const armem::detail::MemoryItem& container);
+ QTreeWidgetItem* makeItem(const std::string& key, const armem::base::detail::MemoryItem& container);
- void updateItemItem(const armem::detail::MemoryItem& level, QTreeWidgetItem* item);
- template <class... T>
- void updateContainerItem(const armem::detail::MemoryContainer<T...>& container, QTreeWidgetItem* item);
+ void updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item);
template <class... T>
- void updateTypedContainerItem(const armem::detail::TypedEntityContainer<T...>& container, QTreeWidgetItem* item);
+ void updateContainerItem(const armem::base::detail::MemoryContainerBase<T...>& container, QTreeWidgetItem* item);
private:
- MapTreeWidgetBuilder<std::string, const Memory*> memoryBuilder;
- MapTreeWidgetBuilder<std::string, CoreSegment> coreSegmentBuilder;
- MapTreeWidgetBuilder<std::string, ProviderSegment> provSegmentBuilder;
- MapTreeWidgetBuilder<std::string, Entity> entityBuilder;
- MapTreeWidgetBuilder<armem::Time, EntitySnapshot> snapshotBuilder;
- TreeWidgetBuilder<std::vector<EntityInstance>> instanceBuilder;
+ MapTreeWidgetBuilder<std::string, const wm::Memory*> workingmemoryBuilder;
+ MapTreeWidgetBuilder<std::string, wm::CoreSegment> workingmemoryCoreSegmentBuilder;
+ MapTreeWidgetBuilder<std::string, wm::ProviderSegment> workingmemoryProvSegmentBuilder;
+ MapTreeWidgetBuilder<std::string, wm::Entity> workingmemoryEntityBuilder;
+ MapTreeWidgetBuilder<armem::Time, wm::EntitySnapshot> workingmemorySnapshotBuilder;
+ TreeWidgetBuilder<std::vector<wm::EntityInstance>> workingmemoryInstanceBuilder;
std::optional<MemoryID> _selectedID;
/// While this is false, do not handle selection updates.
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_localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
index b14f1a711e490c2bcae4eca0ca81976ca888a6ea..c3b24c4895328e951bc4950339639200206a9daf 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
@@ -47,9 +47,9 @@
// this package
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -128,7 +128,7 @@ namespace armarx::armem
// }
- std::vector<std::string> TransformReader::buildTransformChain(const armem::Memory& memory,
+ std::vector<std::string> TransformReader::buildTransformChain(const armem::wm::Memory& memory,
const TransformQuery& query) const
{
ARMARX_DEBUG << "Building transform chain";
@@ -202,7 +202,7 @@ namespace armarx::armem
return chain;
}
- inline Transform convertEntityToTransform(const armem::EntityInstance& item)
+ inline Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
{
aron::Transform aronTransform;
aronTransform.fromAron(item.data());
@@ -271,14 +271,14 @@ namespace armarx::armem
return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
}
- Eigen::Affine3f TransformReader::obtainTransform(const std::string& entityName, const armem::ProviderSegment& agentProviderSegment, const int64_t timestamp) const
+ Eigen::Affine3f TransformReader::obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, const int64_t timestamp) const
{
ARMARX_DEBUG << "getEntity:" + entityName;
const auto& entity = agentProviderSegment.getEntity(entityName);
- ARMARX_DEBUG << "History (size: " << entity.history.size() << ")"
- << simox::alg::get_keys(entity.history);
+ ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
+ << simox::alg::get_keys(entity.history());
// if (entity.history.empty())
// {
@@ -289,9 +289,9 @@ namespace armarx::armem
std::vector<Transform> transforms;
- transforms.reserve(entity.history.size());
+ transforms.reserve(entity.history().size());
- const auto entitySnapshots = simox::alg::get_values(entity.history);
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
std::transform(entitySnapshots.begin(),
entitySnapshots.end(),
std::back_inserter(transforms),
@@ -329,13 +329,13 @@ namespace armarx::armem
}
std::vector<Eigen::Affine3f>
- TransformReader::obtainTransforms(const armem::Memory& memory,
+ TransformReader::obtainTransforms(const armem::wm::Memory& memory,
const std::vector<std::string>& tfChain,
const std::string& agent,
const std::int64_t& timestamp) const
{
- ARMARX_DEBUG << "Core segments" << simox::alg::get_keys(memory.coreSegments);
+ ARMARX_DEBUG << "Core segments" << simox::alg::get_keys(memory.coreSegments());
const auto& agentProviderSegment =
memory.getCoreSegment(properties.localizationMemoryName).getProviderSegment(agent);
@@ -343,9 +343,9 @@ namespace armarx::armem
ARMARX_DEBUG << "Provider segments"
<< simox::alg::get_keys(
memory.getCoreSegment(properties.localizationMemoryName)
- .providerSegments);
+ .providerSegments());
- ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities);
+ ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
try
{
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
index 061499f7b69fd1a8a1b372efe590229f5722c73e..9b04899545e78bdad1b3d0dbcca03c0ba61b4dcd 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
@@ -64,14 +64,14 @@ namespace armarx::armem
}
private:
- std::vector<std::string> buildTransformChain(const armem::Memory& memory,
+ std::vector<std::string> buildTransformChain(const armem::wm::Memory& memory,
const TransformQuery& query) const;
- std::vector<Eigen::Affine3f> obtainTransforms(const armem::Memory& memory,
+ std::vector<Eigen::Affine3f> obtainTransforms(const armem::wm::Memory& memory,
const std::vector<std::string>& tfChain,
const std::string& agent, const std::int64_t& timestamp) const;
- Eigen::Affine3f obtainTransform(const std::string& entityName, const armem::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
+ Eigen::Affine3f obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
armem::client::Reader memoryReader;
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
index 2db61f978e45fb2f8582532454d6423026b9fcfe..1ac8ea58b31d24e14d7f2ee14eb9c34d61178e44 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
@@ -37,10 +37,10 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index aae0106dab35ac2252e90f78ee5dd79a37c9188c..d035b2b7926376c0e16e38bcfd6d8182f4609d87 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -12,35 +12,32 @@
#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/core/EntityInstance.h>
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-namespace armarx::armem
-{
- class Entity;
-} // namespace armarx
namespace armarx::armem
{
@@ -107,7 +104,7 @@ namespace armarx::armem
.coreSegments().withName(properties.mappingMemoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min,query.timeRange.max);
+ .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
// clang-format on
}
@@ -115,7 +112,7 @@ namespace armarx::armem
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, Entity>& entities)
+ std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -124,7 +121,7 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
@@ -144,16 +141,16 @@ 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)
+ for (const auto& entityInstance : entitySnapshot.instances())
{
const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
@@ -194,7 +191,7 @@ namespace armarx::armem
const auto& entities =
qResult.memory.getCoreSegment(properties.mappingMemoryName)
.getProviderSegment(query.agent)
- .entities;
+ .entities();
const auto laserScans = asLaserScans(entities);
const auto sensors = simox::alg::get_keys(entities);
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
index 40471a632c66708ecb8085ea63a65acf6013d5a2..b37a88381383b41ea6c3ca9bd3075a50e8b733ef 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
@@ -24,7 +24,7 @@ namespace armarx
SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
{
-
+
return {.agent = aronSensorHeader.agent,
.frame = aronSensorHeader.frame,
.timestamp = timeFromAron(aronSensorHeader.timestamp)};
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/Converter.h b/source/RobotAPI/libraries/aron/converter/common/Converter.h
index 570344ab618f8a353327a716aaa9ba5f850eaca8..f6e9959dfba18891807fad9579580375bf53382d 100644
--- a/source/RobotAPI/libraries/aron/converter/common/Converter.h
+++ b/source/RobotAPI/libraries/aron/converter/common/Converter.h
@@ -33,12 +33,12 @@ namespace armarx::aron
* You have to provide a converter function for the element with the signature
*
* PlainCppType fromAron(const AronType&)
- *
+ *
* @tparam T the aron vector element
* @param v the vector of elements
* @return the vector of aron elements
*/
- template <typename T>
+ template <typename T>
auto fromAron(const std::vector<T>& v) -> std::vector<decltype(fromAron(T()))>
{
std::vector<decltype(fromAron(T()))> r;
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
index 17475a3b9df0f39ee10e82443f24e5251db63c58..78cce4a1a7d2d9b6d9f5fc9a1f156683c67c0d71 100644
--- a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
@@ -39,7 +39,7 @@ namespace armarx::aron::converter
{
public:
AronVectorConverter() = delete;
-
+
template<typename T>
static std::vector<T> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
{
@@ -47,16 +47,16 @@ namespace armarx::aron::converter
const auto& dims = nav->getDimensions();
- if(dims.size() != 2)
+ if (dims.size() != 2)
{
throw error::AronException("AronVectorConverter", "ConvertToVector", "The NDArray must have two dimensions.", nav->getPath());
}
-
- if(dims.at(1) != sizeof(T))
+
+ if (dims.at(1) != sizeof(T))
{
throw error::AronException("AronVectorConverter", "ConvertToVector", "Dimension 1 of the array has to match the element size.", nav->getPath());
}
-
+
const int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
std::vector<T> v(dims.at(0));
@@ -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/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 673ea307e6b52105e4152e54c9c7b2f41f8ca48a..c2afd4f51bc1c18dd1c509a7328f31720eb519bf 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -18,7 +18,9 @@ set(LIBS
ArmarXCore
RobotAPIInterfaces
cppgen
+
Simox::SimoxUtility
+
# System libraries
Eigen3::Eigen
# PCLInterface
@@ -222,8 +224,8 @@ armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(PCL_FOUND)
-target_include_directories("${LIB_NAME}"
- SYSTEM PUBLIC
+target_include_directories("${LIB_NAME}"
+ SYSTEM PUBLIC
"${PCL_INCLUDE_DIRS}"
)
endif()
diff --git a/source/RobotAPI/libraries/aron/core/Debug.h b/source/RobotAPI/libraries/aron/core/Debug.h
index 6687e8909ffb6d33918cde779f3e9d7f8a342007..28b3958646b984914a6092d272d5b39d3a7dd2ba 100644
--- a/source/RobotAPI/libraries/aron/core/Debug.h
+++ b/source/RobotAPI/libraries/aron/core/Debug.h
@@ -47,7 +47,7 @@ namespace armarx::aron
static std::string AronDataPtrToString(const data::AronDataPtr& data)
{
dataIO::writer::NlohmannJSONWriter w;
- dataIO::Visitor::SetupWriterFromAronDataPtr(w, data);
+ dataIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
}
@@ -58,14 +58,14 @@ namespace armarx::aron
return "";
}
dataIO::writer::NlohmannJSONWriter w;
- dataIO::Visitor::SetupWriterFromAronDataPtr(w, data);
+ dataIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
}
static std::string AronTypePtrToString(const type::AronTypePtr& data)
{
typeIO::writer::NlohmannJSONWriter w;
- typeIO::Visitor::SetupWriterFromAronTypePtr(w, data);
+ typeIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
}
@@ -76,7 +76,7 @@ namespace armarx::aron
return "";
}
typeIO::writer::NlohmannJSONWriter w;
- typeIO::Visitor::SetupWriterFromAronTypePtr(w, data);
+ typeIO::Visitor::VisitAndSetup(w, data);
return w.getResult().dump(2);
}
};
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.h b/source/RobotAPI/libraries/aron/core/Descriptor.h
index 8beadd6fe75e32997382623f96e68f7a5ee70f47..26e9e24ba442a6d04f7590181e8736ace884ab29 100644
--- a/source/RobotAPI/libraries/aron/core/Descriptor.h
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.h
@@ -35,7 +35,7 @@
namespace armarx::aron::type
{
- enum Descriptor
+ enum class Descriptor
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
e##upperType,
@@ -48,11 +48,11 @@ namespace armarx::aron::type
const std::map<type::Descriptor, std::string> _descriptorstring =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { e##upperType, "armarx::aron::type::Descriptor::e" + std::string(#upperType) },
+ { Descriptor::e##upperType, "armarx::aron::type::Descriptor::e" + std::string(#upperType) },
HANDLE_ALL_ARON_TYPES
#undef RUN_ARON_MACRO
- {eUnknown, "armarx::aron::type::Descriptor::eUnknown"}
+ {Descriptor::eUnknown, "armarx::aron::type::Descriptor::eUnknown"}
};
inline std::string DESCRIPTOR_TO_STRING(const type::Descriptor d)
@@ -63,7 +63,7 @@ namespace armarx::aron::type
namespace armarx::aron::data
{
- enum Descriptor
+ enum class Descriptor
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
e##upperType,
@@ -76,11 +76,11 @@ namespace armarx::aron::data
const std::map<data::Descriptor, std::string> _descriptorstring =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { e##upperType, "armarx::aron::data::Descriptor::e" + std::string(#upperType) },
+ { Descriptor::e##upperType, "armarx::aron::data::Descriptor::e" + std::string(#upperType) },
HANDLE_ALL_ARON_DATA
#undef RUN_ARON_MACRO
- {eUnknown, "armarx::aron::data::Descriptor::eUnknown"}
+ {Descriptor::eUnknown, "armarx::aron::data::Descriptor::eUnknown"}
};
inline std::string DESCRIPTOR_TO_STRING(const data::Descriptor d)
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index aeefb6913607dde05b926bfc237262a490fa93a4..315dbed6511f2933fda4dff223a87c6f473b0592 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -44,13 +44,13 @@ namespace armarx::aron
typenavigator::NavigatorPtr generateRandomType(bool mustBeObject = false) const
{
- type::Descriptor nextType = type::eObject;
+ type::Descriptor nextType = type::Descriptor::eObject;
if (!mustBeObject)
{
std::vector<type::Descriptor> descriptors =
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- type::e##upperType,
+ type::Descriptor::e##upperType,
HANDLE_ALL_ARON_TYPES
#undef RUN_ARON_MACRO
@@ -62,7 +62,7 @@ namespace armarx::aron
switch (nextType)
{
- case type::eObject:
+ case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr t = typenavigator::ObjectNavigatorPtr(new typenavigator::ObjectNavigator(Path()));
std::string objectName = generateRandomWord();
@@ -81,7 +81,7 @@ namespace armarx::aron
}
return t;
}
- case type::eDict:
+ case type::Descriptor::eDict:
{
typenavigator::DictNavigatorPtr t = typenavigator::DictNavigatorPtr(new typenavigator::DictNavigator());
typenavigator::NavigatorPtr a = generateRandomType(false);
@@ -89,7 +89,7 @@ namespace armarx::aron
t->setAcceptedType(a);
return t;
}
- case type::eTuple:
+ case type::Descriptor::eTuple:
{
typenavigator::TupleNavigatorPtr t = typenavigator::TupleNavigatorPtr(new typenavigator::TupleNavigator());
@@ -101,7 +101,7 @@ namespace armarx::aron
}
return t;
}
- case type::eList:
+ case type::Descriptor::eList:
{
typenavigator::ListNavigatorPtr t = typenavigator::ListNavigatorPtr(new typenavigator::ListNavigator());
typenavigator::NavigatorPtr a = generateRandomType(false);
@@ -109,7 +109,7 @@ namespace armarx::aron
t->setAcceptedType(a);
return t;
}
- case type::ePair:
+ case type::Descriptor::ePair:
{
typenavigator::PairNavigatorPtr t = typenavigator::PairNavigatorPtr(new typenavigator::PairNavigator());
typenavigator::NavigatorPtr a = generateRandomType(false);
@@ -119,46 +119,46 @@ namespace armarx::aron
t->setSecondAcceptedType(b);
return t;
}
- case type::eEigenMatrix:
- case type::eEigenQuaternion:
- case type::eIVTCByteImage:
- case type::eOpenCVMat:
- case type::ePCLPointCloud:
- case type::ePosition:
- case type::eOrientation:
- case type::ePose:
-
- case type::eInt:
+ case type::Descriptor::eEigenMatrix:
+ case type::Descriptor::eEigenQuaternion:
+ case type::Descriptor::eIVTCByteImage:
+ case type::Descriptor::eOpenCVMat:
+ case type::Descriptor::ePCLPointCloud:
+ case type::Descriptor::ePosition:
+ case type::Descriptor::eOrientation:
+ case type::Descriptor::ePose:
+
+ case type::Descriptor::eInt:
{
auto t = std::make_shared<typenavigator::IntNavigator>();
return t;
}
- case type::eLong:
+ case type::Descriptor::eLong:
{
auto t = std::make_shared<typenavigator::LongNavigator>();
return t;
}
- case type::eFloat:
+ case type::Descriptor::eFloat:
{
auto t = std::make_shared<typenavigator::FloatNavigator>();
return t;
}
- case type::eDouble:
+ case type::Descriptor::eDouble:
{
auto t = std::make_shared<typenavigator::DoubleNavigator>();
return t;
}
- case type::eString:
+ case type::Descriptor::eString:
{
auto t = std::make_shared<typenavigator::StringNavigator>();
return t;
}
- case type::eBool:
+ case type::Descriptor::eBool:
{
auto t = std::make_shared<typenavigator::BoolNavigator>();
return t;
}
- case type::eTime:
+ case type::Descriptor::eTime:
{
auto t = std::make_shared<typenavigator::TimeNavigator>();
return t;
@@ -176,7 +176,7 @@ namespace armarx::aron
switch (desc)
{
// In an object, we do not want to edit the keys.
- case type::eObject:
+ case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr t = typenavigator::ObjectNavigator::DynamicCastAndCheck(type);
datanavigator::DictNavigatorPtr d = datanavigator::DictNavigatorPtr(new datanavigator::DictNavigator());
@@ -188,14 +188,14 @@ namespace armarx::aron
}
// here all totally random
- case type::eDict:
+ case type::Descriptor::eDict:
{
typenavigator::DictNavigatorPtr t = typenavigator::DictNavigator::DynamicCastAndCheck(type);
return datanavigator::NavigatorPtr(new datanavigator::DictNavigator());
}
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::e##upperType: \
+case type::Descriptor::e##upperType: \
{ \
typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
return datanavigator::NavigatorPtr(new datanavigator::ListNavigator()); \
@@ -205,7 +205,7 @@ case type::e##upperType: \
#undef RUN_ARON_MACRO
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::e##upperType: \
+case type::Descriptor::e##upperType: \
{ \
typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
datanavigator::NDArrayNavigatorPtr ndarray = datanavigator::NDArrayNavigatorPtr(new datanavigator::NDArrayNavigator()); \
@@ -216,7 +216,7 @@ case type::e##upperType: \
#undef RUN_ARON_MACRO
#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
-case type::e##upperType: \
+case type::Descriptor::e##upperType: \
{ \
typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
return datanavigator::NavigatorPtr(new datanavigator::upperData##Navigator()); \
@@ -239,7 +239,7 @@ case type::e##upperType: \
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
-case type::e##upperType: \
+case type::Descriptor::e##upperType: \
{ \
typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
datanavigator::upperData##NavigatorPtr d = datanavigator::upperData##Navigator::DynamicCastAndCheck(data); \
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
index 7b777b8201d2ddfa302135a1b7795afd134a8c9d..15d698675592fa368cca5b212300d839c2142b8f 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
@@ -36,27 +36,27 @@ namespace armarx::aron::cppcodegenerator
{
static const std::map<type::Descriptor, SerializerFactoryPtr> Factories =
{
- {type::eObject, SerializerFactoryPtr(new ObjectSerializerFactory())},
- {type::eDict, SerializerFactoryPtr(new DictSerializerFactory())},
- {type::eList, SerializerFactoryPtr(new ListSerializerFactory())},
- {type::eTuple, SerializerFactoryPtr(new TupleSerializerFactory())},
- {type::ePair, SerializerFactoryPtr(new PairSerializerFactory())},
- {type::eEigenMatrix, SerializerFactoryPtr(new EigenMatrixSerializerFactory())},
- {type::eEigenQuaternion, SerializerFactoryPtr(new EigenQuaternionSerializerFactory())},
- {type::eIVTCByteImage, SerializerFactoryPtr(new IVTCByteImageSerializerFactory())},
- {type::eOpenCVMat, SerializerFactoryPtr(new OpenCVMatSerializerFactory())},
- {type::ePCLPointCloud, SerializerFactoryPtr(new PCLPointCloudSerializerFactory())},
- {type::ePosition, SerializerFactoryPtr(new PositionSerializerFactory())},
- {type::eOrientation, SerializerFactoryPtr(new OrientationSerializerFactory())},
- {type::ePose, SerializerFactoryPtr(new PoseSerializerFactory())},
- {type::eIntEnum, SerializerFactoryPtr(new IntEnumSerializerFactory())},
- {type::eInt, SerializerFactoryPtr(new IntSerializerFactory())},
- {type::eLong, SerializerFactoryPtr(new LongSerializerFactory())},
- {type::eFloat, SerializerFactoryPtr(new FloatSerializerFactory())},
- {type::eDouble, SerializerFactoryPtr(new DoubleSerializerFactory())},
- {type::eString, SerializerFactoryPtr(new StringSerializerFactory())},
- {type::eBool, SerializerFactoryPtr(new BoolSerializerFactory())},
- {type::eTime, SerializerFactoryPtr(new TimeSerializerFactory())}
+ {type::Descriptor::eObject, SerializerFactoryPtr(new ObjectSerializerFactory())},
+ {type::Descriptor::eDict, SerializerFactoryPtr(new DictSerializerFactory())},
+ {type::Descriptor::eList, SerializerFactoryPtr(new ListSerializerFactory())},
+ {type::Descriptor::eTuple, SerializerFactoryPtr(new TupleSerializerFactory())},
+ {type::Descriptor::ePair, SerializerFactoryPtr(new PairSerializerFactory())},
+ {type::Descriptor::eEigenMatrix, SerializerFactoryPtr(new EigenMatrixSerializerFactory())},
+ {type::Descriptor::eEigenQuaternion, SerializerFactoryPtr(new EigenQuaternionSerializerFactory())},
+ {type::Descriptor::eIVTCByteImage, SerializerFactoryPtr(new IVTCByteImageSerializerFactory())},
+ {type::Descriptor::eOpenCVMat, SerializerFactoryPtr(new OpenCVMatSerializerFactory())},
+ {type::Descriptor::ePCLPointCloud, SerializerFactoryPtr(new PCLPointCloudSerializerFactory())},
+ {type::Descriptor::ePosition, SerializerFactoryPtr(new PositionSerializerFactory())},
+ {type::Descriptor::eOrientation, SerializerFactoryPtr(new OrientationSerializerFactory())},
+ {type::Descriptor::ePose, SerializerFactoryPtr(new PoseSerializerFactory())},
+ {type::Descriptor::eIntEnum, SerializerFactoryPtr(new IntEnumSerializerFactory())},
+ {type::Descriptor::eInt, SerializerFactoryPtr(new IntSerializerFactory())},
+ {type::Descriptor::eLong, SerializerFactoryPtr(new LongSerializerFactory())},
+ {type::Descriptor::eFloat, SerializerFactoryPtr(new FloatSerializerFactory())},
+ {type::Descriptor::eDouble, SerializerFactoryPtr(new DoubleSerializerFactory())},
+ {type::Descriptor::eString, SerializerFactoryPtr(new StringSerializerFactory())},
+ {type::Descriptor::eBool, SerializerFactoryPtr(new BoolSerializerFactory())},
+ {type::Descriptor::eTime, SerializerFactoryPtr(new TimeSerializerFactory())}
};
//CheckIfPtrIsNull("NavigatorFactory", "create", path, n);
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
index a431295f0583429740b3c43f0b64dfccf24b5499..42fe5d66dc70e3c5e769aaf65abc58e7ac7728c9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
index b7039894e985935fb8fb38e0d97fc10af7f7bd85..98a1dbae29a52a5392bfbbd537be7deea856a94d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
index 4bb664d2ece7171047ca837e1642a45250de19cf..a0f7a0d6d553d203e71dfec31d43c996c226b634 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
return b;
}
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 d9d22766cf2f51e163758c27c962661a3cb6758f..b038334fbb5dcc6ac39206c33174320f6b8e44d0 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
@@ -182,7 +182,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);
@@ -195,7 +195,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/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
index 5e7d3875fe29a4f10e15fb7123fecf33276e19d1..cf5b46c25b102ce0d2e49d4e971a6c5935f50000 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
@@ -61,6 +61,7 @@ namespace armarx::aron::cppcodegenerator::serializer
virtual CppBlockPtr getReadBlock(const std::string&) const override;
virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+ // TODO: Move some of those methods to upper class for enums (if we want to support multiple enums)
CppCtorPtr toCopyCtor(const std::string&) const;
CppCtorPtr toInnerEnumCtor(const std::string&) const;
CppEnumPtr toInnerEnumDefinition() const;
diff --git a/source/RobotAPI/libraries/aron/core/io/Data.h b/source/RobotAPI/libraries/aron/core/io/Data.h
index 6d29388942067854205998b2c0aaef4e4b74a402..73c693b53311f47232c1b581698dc7765495c8c4 100644
--- a/source/RobotAPI/libraries/aron/core/io/Data.h
+++ b/source/RobotAPI/libraries/aron/core/io/Data.h
@@ -40,18 +40,18 @@ namespace armarx::aron::io
public:
// TODO: Remove copy from ReaderWriter
- static constexpr const char* READER_WRITER_NAME_SLUG = "ARON_NAME";
- static constexpr const char* READER_WRITER_DICT_ACCEPTED_TYPE_SLUG = "ARON_DICT_ACCEPTED_TYPE";
- static constexpr const char* READER_WRITER_LIST_ACCEPTED_TYPE_SLUG = "ARON_LIST_ACCEPTED_TYPE";
-
- static constexpr const char* READER_WRITER_NDARRAY_NAME_SLUG = "ARON_NDARRAY_NAME";
- static constexpr const char* READER_WRITER_NDARRAY_DIMENSIONS_SLUG = "ARON_NDARRAY_DIMESIONS";
- static constexpr const char* READER_WRITER_NDARRAY_TYPE_SLUG = "ARON_NDARRAY_TYPE";
- static constexpr const char* READER_WRITER_NDARRAY_DATA_SLUG = "ARON_NDARRAY_DATA";
-
- static constexpr const char* READER_WRITER_INT_ENUM_NAME_SLUG = "ARON_INT_ENUM_NAME";
- static constexpr const char* READER_WRITER_INT_ENUM_VALUE_SLUG = "ARON_INT_ENUM_VALUE";
- static constexpr const char* READER_WRITER_ENUM_KEY_SLUG = "ARON_ENUM_KEY";
+ static constexpr const char* READER_WRITER_NAME_SLUG = "__ARON_NAME";
+ static constexpr const char* READER_WRITER_DICT_ACCEPTED_TYPE_SLUG = "__ARON_DICT_ACCEPTED_TYPE";
+ static constexpr const char* READER_WRITER_LIST_ACCEPTED_TYPE_SLUG = "__ARON_LIST_ACCEPTED_TYPE";
+
+ static constexpr const char* READER_WRITER_NDARRAY_NAME_SLUG = "__ARON_NDARRAY_NAME";
+ static constexpr const char* READER_WRITER_NDARRAY_DIMENSIONS_SLUG = "__ARON_NDARRAY_DIMESIONS";
+ static constexpr const char* READER_WRITER_NDARRAY_TYPE_SLUG = "__ARON_NDARRAY_TYPE";
+ static constexpr const char* READER_WRITER_NDARRAY_DATA_SLUG = "__ARON_NDARRAY_DATA";
+
+ static constexpr const char* READER_WRITER_INT_ENUM_NAME_SLUG = "__ARON_INT_ENUM_NAME";
+ static constexpr const char* READER_WRITER_INT_ENUM_VALUE_SLUG = "__ARON_INT_ENUM_VALUE";
+ static constexpr const char* READER_WRITER_ENUM_KEY_SLUG = "__ARON_ENUM_KEY";
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
static constexpr const char* READER_WRITER_##capsType##_TYPENAME_SLUG = #capsType;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
index 7df44a015c8f93f531e1e54c8dd2524ae98308d6..574b9167d3f6b77e4b2a8434a5dfb90911cabd2c 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
@@ -64,6 +64,6 @@ namespace armarx::aron::dataIO
virtual void loadMember(const std::string&) = 0;
// Helper functions
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::eUnknown) const = 0;
+ virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const = 0;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
index 13b6703cd69bffe9e6c8f89cf4164ada98649bc0..a0feee781f33f1623d24948600a26370368db685 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
@@ -31,21 +31,22 @@
namespace armarx::aron::dataIO
{
- void Converter::ConvertFromReader(ReaderInterface& reader, WriterInterface& writer, const aron::typenavigator::NavigatorPtr& expectedStructure)
+ void Converter::ReadAndConvert(ReaderInterface& reader, WriterInterface& writer, const aron::typenavigator::NavigatorPtr& expectedStructure)
{
- type::Descriptor t_desc = expectedStructure != nullptr ? expectedStructure->getDescriptor() : type::eUnknown;
+ type::Descriptor t_desc = expectedStructure != nullptr ? expectedStructure->getDescriptor() : type::Descriptor::eUnknown;
+ //std::cout << "Expected Structure: " << type::DESCRIPTOR_TO_STRING(t_desc) << std::endl;
data::Descriptor desc = reader.getTypeOfNext(t_desc);
switch (desc)
{
- case data::eDict:
+ case data::Descriptor::eDict:
{
int elements = reader.readStartDict();
writer.writeStartDict();
typenavigator::NavigatorPtr childType = nullptr;
- if (t_desc == type::eDict)
+ if (t_desc == type::Descriptor::eDict)
{
- auto t = typenavigator::DictNavigator::DynamicCast(expectedStructure);
+ auto t = typenavigator::DictNavigator::DynamicCastAndCheck(expectedStructure);
childType = t->getAcceptedType();
}
@@ -54,13 +55,13 @@ namespace armarx::aron::dataIO
std::string key = reader.readKey();
writer.writeKey(key);
- if (t_desc == type::eObject)
+ if (t_desc == type::Descriptor::eObject)
{
- auto t = typenavigator::ObjectNavigator::DynamicCast(expectedStructure);
+ auto t = typenavigator::ObjectNavigator::DynamicCastAndCheck(expectedStructure);
childType = t->getMemberType(key);
}
- Converter::ConvertFromReader(reader, writer, childType);
+ Converter::ReadAndConvert(reader, writer, childType);
}
writer.writeEndDict();
@@ -68,62 +69,85 @@ namespace armarx::aron::dataIO
break;
}
- case data::eList:
+ case data::Descriptor::eList:
{
int elements = reader.readStartList();
writer.writeStartList();
typenavigator::NavigatorPtr childType = nullptr;
- if (t_desc == type::eList)
+ if (t_desc == type::Descriptor::eList)
{
- auto t = typenavigator::ListNavigator::DynamicCast(expectedStructure);
+ auto t = typenavigator::ListNavigator::DynamicCastAndCheck(expectedStructure);
childType = t->getAcceptedType();
}
for (int i = 0; i < elements; ++i)
{
- if (t_desc == type::eObject)
+ if (t_desc == type::Descriptor::eObject)
{
- auto t = typenavigator::TupleNavigator::DynamicCast(expectedStructure);
+ auto t = typenavigator::TupleNavigator::DynamicCastAndCheck(expectedStructure);
childType = t->getAcceptedType(i);
}
- Converter::ConvertFromReader(reader, writer, childType);
+ Converter::ReadAndConvert(reader, writer, childType);
}
writer.writeEndList();
reader.readEndList();
break;
}
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case data::e##upperType: \
-{ \
- auto [dims, type] = reader.readStart##upperType(); \
- int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()); \
- std::vector<unsigned char> data(elements); \
- reader.readEnd##upperType(data.data()); \
- \
- writer.write##upperType(dims, type, data.data()); \
- break; \
-}
- HANDLE_COMPLEX_DATA
-#undef RUN_ARON_MACRO
+ case data::Descriptor::eNDArray:
+ {
+ auto [dims, type] = reader.readStartNDArray();
+ int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ std::vector<unsigned char> data(elements);
+ reader.readEndNDArray(data.data());
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case data::e##upperType: \
-{ \
- lowerType val = reader.readPrimitive<lowerType>(); \
- writer.writePrimitive(val); \
- break; \
-}
+ writer.writeNDArray(dims, type, data.data());
+ break;
+ }
- HANDLE_PRIMITIVE_DATA
-#undef RUN_ARON_MACRO
+ case data::Descriptor::eInt:
+ {
+ int val = reader.readPrimitive<int>();
+ writer.writePrimitive(val);
+ break;
+ }
+ case data::Descriptor::eLong:
+ {
+ long val = reader.readPrimitive<long>();
+ writer.writePrimitive(val);
+ break;
+ }
+ case data::Descriptor::eFloat:
+ {
+ float val = reader.readPrimitive<float>();
+ writer.writePrimitive(val);
+ break;
+ }
+ case data::Descriptor::eDouble:
+ {
+ double val = reader.readPrimitive<double>();
+ writer.writePrimitive(val);
+ break;
+ }
+ case data::Descriptor::eString:
+ {
+ std::string val = reader.readPrimitive<std::string>();
+ writer.writePrimitive(val);
+ break;
+ }
+ case data::Descriptor::eBool:
+ {
+ bool val = reader.readPrimitive<bool>();
+ writer.writePrimitive(val);
+ break;
+ }
default:
{
- throw error::DescriptorNotValidException("LegacyAronDataReader", "SetupReaderAndGetResult", "Data-Type could not be resolved", desc);
+ throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "Data-Type could not be resolved", desc);
}
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h
index d51d81761b764d5616bb70e5cefd67dd105987bd..1ea6c6f5c3bf984d2fad6b8ffd55f7cc7d50f76c 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h
@@ -43,6 +43,6 @@ namespace armarx::aron::dataIO
Converter() = delete;
public:
- static void ConvertFromReader(ReaderInterface&, WriterInterface&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
+ static void ReadAndConvert(ReaderInterface&, WriterInterface&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
};
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
index a757d2778ca04857fb436977bcec6e4eb9de3710..b1c8e774e9b252bb582ba7a7dbf2c38c8f158122 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
@@ -45,7 +45,7 @@ namespace armarx::aron::dataIO
// Interface
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::eUnknown) const = 0;
+ virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const = 0;
virtual ElementTypename getNextElement() const = 0;
// General implementation
@@ -115,7 +115,7 @@ namespace armarx::aron::dataIO
protected:
// members
- data::Descriptor descriptor = data::eUnknown;
+ data::Descriptor descriptor = data::Descriptor::eUnknown;
ElementTypename element;
unsigned int childrenSize = 0;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index aa5d3d14864aff4fd5d86b8cfc47b06a81cc0200..2dc7c3db842d4e7deb5ee1c5d94b4be5618f2e64 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
@@ -72,7 +72,7 @@ namespace armarx::aron::dataIO::reader
auto current_nav = getNextAndIncrease();
auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(current_nav);
int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(data::eDict, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eDict, current_nav_casted);
stack.push(newToken);
return c;
}
@@ -95,7 +95,7 @@ namespace armarx::aron::dataIO::reader
auto current_nav = getNextAndIncrease();
auto current_nav_casted = datanavigator::ListNavigator::DynamicCastAndCheck(current_nav);
int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(data::eList, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eList, current_nav_casted);
stack.push(newToken);
return c;
}
@@ -192,6 +192,17 @@ namespace armarx::aron::dataIO::reader
// Helper functions
data::Descriptor NavigatorReader::getTypeOfNext(const type::Descriptor hint) const
{
+ if (stack.empty())
+ {
+ if (hint == type::Descriptor::eUnknown)
+ {
+ return data::Descriptor::eDict;
+ }
+ else
+ {
+ return Resolver::GetCorresponding(hint);
+ }
+ }
auto token = stack.top();
return token->getTypeOfNext(hint);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
index 0bcc56b7bdc6e22775fc2d4345e1f9715e942d1f..db52ad1d85c2f10807d10f63bc44c7de717e6b97 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
@@ -66,7 +66,7 @@ namespace armarx::aron::dataIO::reader
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::eUnknown) const override;
+ virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
private:
datanavigator::NavigatorPtr getNext();
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
index 2efb3352f203d93fa0b647ea318affea7d41b4fd..4b85fe7725b3840c2e9c2b87a5d6cc9c48f9e63f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
@@ -69,7 +69,7 @@ namespace armarx::aron::dataIO::reader
}
}
- data::Descriptor getTypeOfNext(const type::Descriptor hint = type::eUnknown) const override
+ data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override
{
const datanavigator::NavigatorPtr next = getNextElement();
return next->getDescriptor();
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 9b15c88280210b4e9b2e583ba7439d87e68909d9..c243c4e4beaf851090607423dd91e1d5ade4118a 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -70,7 +70,7 @@ namespace armarx::aron::dataIO::reader
{
nlohmann::json current_json = getNextAndIncrease();
int c = current_json.size();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::eDict, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eDict, current_json);
stack.push(newToken);
return c;
}
@@ -78,7 +78,7 @@ namespace armarx::aron::dataIO::reader
bool NlohmannJSONReader::readEndDict()
{
auto token = stack.top();
- token->assertType(data::eDict);
+ token->assertType(data::Descriptor::eDict);
if (token->finishedElement())
{
@@ -92,7 +92,7 @@ namespace armarx::aron::dataIO::reader
{
nlohmann::json current_json = getNextAndIncrease();
int c = current_json.size();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::eList, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eList, current_json);
stack.push(newToken);
return c;
}
@@ -100,7 +100,7 @@ namespace armarx::aron::dataIO::reader
bool NlohmannJSONReader::readEndList()
{
auto token = stack.top();
- token->assertType(data::eList);
+ token->assertType(data::Descriptor::eList);
if (token->finishedElement())
{
@@ -184,6 +184,17 @@ namespace armarx::aron::dataIO::reader
// Helper functions
data::Descriptor NlohmannJSONReader::getTypeOfNext(const type::Descriptor hint) const
{
+ if (stack.empty())
+ {
+ if (hint == type::Descriptor::eUnknown)
+ {
+ return data::Descriptor::eDict;
+ }
+ else
+ {
+ return Resolver::GetCorresponding(hint);
+ }
+ }
auto token = stack.top();
return token->getTypeOfNext(hint);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
index 1ca1810a0c3085e4d53b7a79c934dc1eaa7e1e25..7749530308b806da5898881879cef6df26143358 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -64,7 +64,7 @@ namespace armarx::aron::dataIO::reader
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
- virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::eUnknown) const override;
+ virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
private:
nlohmann::json getNext();
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index d155a2d155027ad03f16a359a903da88dae6776c..04acd920b51789d2ce925f8aab48c268baf3fed7 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -57,7 +57,7 @@ namespace armarx::aron::dataIO::reader
switch (descriptor)
{
- case data::eDict:
+ case data::Descriptor::eDict:
{
for (auto it = data.begin(); it != data.end(); ++it)
{
@@ -66,7 +66,7 @@ namespace armarx::aron::dataIO::reader
childrenSize = data.size();
break;
}
- case data::eList:
+ case data::Descriptor::eList:
{
childrenSize = data.size();
break;
@@ -84,36 +84,36 @@ namespace armarx::aron::dataIO::reader
// Check if specific NDArray key exists
if (next.find(io::Data::READER_WRITER_NDARRAY_DATA_SLUG) != next.end())
{
- return data::eNDArray;
+ return data::Descriptor::eNDArray;
}
- return data::eDict;
+ return data::Descriptor::eDict;
}
if (next.is_array())
{
- return data::eList;
+ return data::Descriptor::eList;
}
if (next.is_number_integer())
{
data::Descriptor d = Resolver::GetFirstIfRelated(
Resolver::GetCorresponding(hint),
- data::eLong);
+ data::Descriptor::eLong);
return d;
}
if (next.is_number_float())
{
data::Descriptor d = Resolver::GetFirstIfRelated(
Resolver::GetCorresponding(hint),
- data::eDouble);
+ data::Descriptor::eDouble);
return d;
}
if (next.is_boolean())
{
- return data::eBool;
+ return data::Descriptor::eBool;
}
if (next.is_string())
{
- return data::eString;
+ return data::Descriptor::eString;
}
throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Could not convert to data::Descriptor enum.");
}
@@ -122,12 +122,12 @@ namespace armarx::aron::dataIO::reader
{
switch (descriptor)
{
- case data::eDict:
+ case data::Descriptor::eDict:
{
nlohmann::json ret = element[getCurrentKey()];
return ret;
}
- case data::eList:
+ case data::Descriptor::eList:
{
nlohmann::json ret = element[currentIndex];
return ret;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
index 9f2dd3cd6e470c72698aae1c5c0bdcd948194e53..b5be280a8cfe72963afa63b88a900b6885250c33 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
@@ -33,76 +33,76 @@
namespace armarx::aron::dataIO
{
- void Visitor::SetupWriterFromAronDataPtr(WriterInterface& writer, const datanavigator::NavigatorPtr& aron)
+ void Visitor::VisitAndSetup(WriterInterface& writer, const datanavigator::NavigatorPtr& aron)
{
- SetupWriterFromAronDataPtr(writer, aron->getResult());
+ VisitAndSetup(writer, aron->getResult());
}
- void Visitor::SetupWriterFromAronDataPtr(WriterInterface& writer, const data::AronDataPtr& aron)
+ void Visitor::VisitAndSetup(WriterInterface& writer, const data::AronDataPtr& aron)
{
data::Descriptor desc = Resolver::GetDescriptor(aron);
switch (desc)
{
- case data::eDict:
+ case data::Descriptor::eDict:
{
data::AronDictPtr casted = data::AronDictPtr::dynamicCast(aron);
writer.writeStartDict();
for (const auto& [key, value] : casted->elements)
{
writer.writeKey(key);
- Visitor::SetupWriterFromAronDataPtr(writer, value);
+ Visitor::VisitAndSetup(writer, value);
}
writer.writeEndDict();
break;
}
- case data::eList:
+ case data::Descriptor::eList:
{
data::AronListPtr casted = data::AronListPtr::dynamicCast(aron);
writer.writeStartList();
for (const auto& value : casted->elements)
{
- Visitor::SetupWriterFromAronDataPtr(writer, value);
+ Visitor::VisitAndSetup(writer, value);
}
writer.writeEndList();
break;
}
- case data::eNDArray:
+ case data::Descriptor::eNDArray:
{
data::AronNDArrayPtr casted = data::AronNDArrayPtr::dynamicCast(aron);
writer.writeNDArray(casted->dimensions, casted->type, casted->data.data());
break;
}
- case data::eInt:
+ case data::Descriptor::eInt:
{
data::AronIntPtr casted = data::AronIntPtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
break;
}
- case data::eLong:
+ case data::Descriptor::eLong:
{
data::AronLongPtr casted = data::AronLongPtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
break;
}
- case data::eFloat:
+ case data::Descriptor::eFloat:
{
data::AronFloatPtr casted = data::AronFloatPtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
break;
}
- case data::eDouble:
+ case data::Descriptor::eDouble:
{
data::AronDoublePtr casted = data::AronDoublePtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
break;
}
- case data::eString:
+ case data::Descriptor::eString:
{
data::AronStringPtr casted = data::AronStringPtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
break;
}
- case data::eBool:
+ case data::Descriptor::eBool:
{
data::AronBoolPtr casted = data::AronBoolPtr::dynamicCast(aron);
writer.writePrimitive(casted->value);
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h
index b45ef50c60207e97e6e29e170df39cf0be237a90..b6b00cbbc7d55ff95c3a1674af3dd7d6b9b28e29 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h
@@ -41,8 +41,8 @@ namespace armarx::aron::dataIO
Visitor() = delete;
public:
- static void SetupWriterFromAronDataPtr(WriterInterface&, const datanavigator::NavigatorPtr&);
- static void SetupWriterFromAronDataPtr(WriterInterface&, const data::AronDataPtr&);
+ static void VisitAndSetup(WriterInterface&, const datanavigator::NavigatorPtr&);
+ static void VisitAndSetup(WriterInterface&, const data::AronDataPtr&);
public:
};
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h
index 950baf3f13359a2ff38db78a16af0c73e24d303e..f91f6c0404fa5f1efaca3301b754165e98abebc8 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h
@@ -71,9 +71,9 @@ namespace armarx::aron::dataIO
data::Descriptor desc = this->getDescriptor();
switch (desc)
{
- case data::eDict:
+ case data::Descriptor::eDict:
return currentKey;
- case data::eList:
+ case data::Descriptor::eList:
return std::to_string(currentIndex);
default:
throw error::DescriptorNotValidException("NavigatorWriterToken", "toElementAccessor", "Could not resove a type of a navigator. Allowed are only containers.", desc);
@@ -82,7 +82,7 @@ namespace armarx::aron::dataIO
protected:
// members
- data::Descriptor descriptor = data::eUnknown;
+ data::Descriptor descriptor = data::Descriptor::eUnknown;
ElementTypename element;
// current index
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
index adebb69a3a86875ac18e9854c96256c01458a9a3..15f57ecf56e00bf09720c4250e8972b9f79f546d 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
@@ -53,7 +53,7 @@ namespace armarx::aron::dataIO::writer
{
Path path = generatePath();
auto data = std::make_shared<datanavigator::DictNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data::eDict, data);
+ auto new_token = std::make_shared<NavigatorWriterToken>(data::Descriptor::eDict, data);
stack.push(new_token);
}
@@ -73,7 +73,7 @@ namespace armarx::aron::dataIO::writer
{
Path path = generatePath();
auto data = std::make_shared<datanavigator::ListNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data::eList, data);
+ auto new_token = std::make_shared<NavigatorWriterToken>(data::Descriptor::eList, data);
stack.push(new_token);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index e57ee04fd96febf4aeeb128385ac14dce87d9217..b768c5da072915d37406b2c0d7f3fb9bb5839dd9 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -32,7 +32,7 @@ namespace armarx::aron::dataIO::writer
void NlohmannJSONWriter::writeStartDict()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::eDict, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::Descriptor::eDict, data);
stack.push(new_token);
}
@@ -51,7 +51,7 @@ namespace armarx::aron::dataIO::writer
void NlohmannJSONWriter::writeStartList()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::eList, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(data::Descriptor::eList, data);
stack.push(new_token);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
index b9aca9ffceea3bbae3fbd76794112bdca58379d8..48decff3a1763338b8d2de06c217bfa38b60cf86 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
@@ -27,12 +27,12 @@
namespace armarx::aron::typeIO
{
- void Converter::ConvertFromReader(ReaderInterface& reader, WriterInterface& writer)
+ void Converter::ReadAndConvert(ReaderInterface& reader, WriterInterface& writer)
{
type::Descriptor desc = reader.getTypeOfNext();
switch (desc)
{
- case type::eObject:
+ case type::Descriptor::eObject:
{
const auto [name, elements] = reader.readStartObject();
writer.writeStartObject(name);
@@ -41,134 +41,134 @@ namespace armarx::aron::typeIO
{
std::string key = reader.readKey();
writer.writeKey(key);
- Converter::ConvertFromReader(reader, writer);
+ Converter::ReadAndConvert(reader, writer);
}
writer.writeEndObject();
reader.readEndObject();
break;
}
- case type::eDict:
+ case type::Descriptor::eDict:
{
reader.readStartDict();
writer.writeStartDict();
- Converter::ConvertFromReader(reader, writer);
+ Converter::ReadAndConvert(reader, writer);
writer.writeEndDict();
reader.readEndDict();
break;
}
- case type::eTuple:
+ case type::Descriptor::eTuple:
{
int elements = reader.readStartTuple();
writer.writeStartTuple();
for (int i = 0; i < elements; ++i)
{
- Converter::ConvertFromReader(reader, writer);
+ Converter::ReadAndConvert(reader, writer);
}
writer.writeEndTuple();
reader.readEndTuple();
break;
}
- case type::eList:
+ case type::Descriptor::eList:
{
reader.readStartList();
writer.writeStartList();
- Converter::ConvertFromReader(reader, writer);
+ Converter::ReadAndConvert(reader, writer);
writer.writeEndList();
reader.readEndList();
break;
}
- case type::eEigenMatrix:
+ case type::Descriptor::eEigenMatrix:
{
auto x = reader.readEigenMatrix();
writer.writeEigenMatrix(std::get<0>(x), std::get<1>(x));
break;
}
- case type::eEigenQuaternion:
+ case type::Descriptor::eEigenQuaternion:
{
auto x = reader.readEigenQuaternion();
writer.writeEigenQuaternion(x);
break;
}
- case type::eIVTCByteImage:
+ case type::Descriptor::eIVTCByteImage:
{
auto x = reader.readIVTCByteImage();
writer.writeIVTCByteImage(std::get<0>(x), std::get<1>(x), std::get<2>(x));
break;
}
- case type::eOpenCVMat:
+ case type::Descriptor::eOpenCVMat:
{
auto x = reader.readOpenCVMat();
writer.writeOpenCVMat(std::get<0>(x), std::get<1>(x));
break;
}
- case type::ePCLPointCloud:
+ case type::Descriptor::ePCLPointCloud:
{
auto x = reader.readPCLPointCloud();
writer.writePCLPointCloud(std::get<0>(x), std::get<1>(x), std::get<2>(x));
break;
}
- case type::ePosition:
+ case type::Descriptor::ePosition:
{
reader.readPosition();
writer.writePosition();
break;
}
- case type::eOrientation:
+ case type::Descriptor::eOrientation:
{
reader.readOrientation();
writer.writeOrientation();
break;
}
- case type::ePose:
+ case type::Descriptor::ePose:
{
reader.readPose();
writer.writePose();
break;
}
- case type::eInt:
+ case type::Descriptor::eInt:
{
reader.readInt();
writer.writeInt();
break;
}
- case type::eLong:
+ case type::Descriptor::eLong:
{
reader.readLong();
writer.writeLong();
break;
}
- case type::eFloat:
+ case type::Descriptor::eFloat:
{
reader.readFloat();
writer.writeFloat();
break;
}
- case type::eDouble:
+ case type::Descriptor::eDouble:
{
reader.readDouble();
writer.writeDouble();
break;
}
- case type::eString:
+ case type::Descriptor::eString:
{
reader.readString();
writer.writeString();
break;
}
- case type::eBool:
+ case type::Descriptor::eBool:
{
reader.readBool();
writer.writeBool();
break;
}
- case type::eTime:
+ case type::Descriptor::eTime:
{
reader.readTime();
writer.writeTime();
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h
index f1c59c61894ff07dfdea24d1f5682620392ebf35..8ef1007efb1d06ac2b0360ccc8008f7fdfa634d2 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h
@@ -45,7 +45,7 @@ namespace armarx::aron::typeIO
Converter() = delete;
public:
- static void ConvertFromReader(ReaderInterface&, WriterInterface&);
+ static void ReadAndConvert(ReaderInterface&, WriterInterface&);
public:
};
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h
index c4487c6baee1fd033932c276dc114891f274d575..17b364ff4874a397820cf3143fb180cf21cd8275 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h
@@ -54,8 +54,8 @@ namespace armarx::aron::typeIO
{
switch (descriptor)
{
- case type::eObject:
- case type::eIntEnum:
+ case type::Descriptor::eObject:
+ case type::Descriptor::eIntEnum:
{
return elementName;
}
@@ -125,7 +125,7 @@ namespace armarx::aron::typeIO
protected:
// members
- type::Descriptor descriptor = type::eUnknown;
+ type::Descriptor descriptor = type::Descriptor::eUnknown;
ElementTypename element;
std::string elementName = "";
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
index 19a19936f8cc8e01f494c00105735a6556c1ef14..0effe2a60983b988abd7c3e8ae8191f69e30ee0d 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
@@ -59,7 +59,7 @@ namespace armarx::aron::typeIO::reader
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::eObject, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eObject, current_nav_casted);
stack.push(newToken);
return {newToken->getElementName(), newToken->getElementChildrenSize()};
}
@@ -67,7 +67,7 @@ namespace armarx::aron::typeIO::reader
bool NavigatorReader::readEndObject()
{
auto token = stack.top();
- token->assertType(type::eObject);
+ token->assertType(type::Descriptor::eObject);
if (token->finishedElement())
{
@@ -81,7 +81,7 @@ namespace armarx::aron::typeIO::reader
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::DictNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::eDict, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eDict, current_nav_casted);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -89,7 +89,7 @@ namespace armarx::aron::typeIO::reader
bool NavigatorReader::readEndDict()
{
auto token = stack.top();
- token->assertType(type::eDict);
+ token->assertType(type::Descriptor::eDict);
if (token->finishedElement())
{
@@ -103,7 +103,7 @@ namespace armarx::aron::typeIO::reader
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::ListNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::eList, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eList, current_nav_casted);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -111,7 +111,7 @@ namespace armarx::aron::typeIO::reader
bool NavigatorReader::readEndList()
{
auto token = stack.top();
- token->assertType(type::eList);
+ token->assertType(type::Descriptor::eList);
if (token->finishedElement())
{
@@ -125,7 +125,7 @@ namespace armarx::aron::typeIO::reader
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::TupleNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::eTuple, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eTuple, current_nav_casted);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -133,7 +133,7 @@ namespace armarx::aron::typeIO::reader
bool NavigatorReader::readEndTuple()
{
auto token = stack.top();
- token->assertType(type::eTuple);
+ token->assertType(type::Descriptor::eTuple);
if (token->finishedElement())
{
@@ -147,7 +147,7 @@ namespace armarx::aron::typeIO::reader
{
auto current_nav = getNextAndIncrease();
auto current_nav_casted = typenavigator::PairNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::ePair, current_nav_casted);
+ auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::ePair, current_nav_casted);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -155,7 +155,7 @@ namespace armarx::aron::typeIO::reader
bool NavigatorReader::readEndPair()
{
auto token = stack.top();
- token->assertType(type::ePair);
+ token->assertType(type::Descriptor::ePair);
if (token->finishedElement())
{
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index eea8c5dc279d527d9aa6024b1c3bfed2f2e9da8a..98d6953ade3f36ca2408dc379a3b325e8fd007de 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -40,7 +40,7 @@ namespace armarx::aron::typeIO::reader
}
NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
- input(n)
+ input(nlohmann::json::parse(n))
{
}
@@ -58,7 +58,7 @@ namespace armarx::aron::typeIO::reader
std::tuple<std::string, int> NlohmannJSONReader::readStartObject()
{
nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::eObject, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eObject, current_json);
stack.push(newToken);
return {newToken->getElementName(), newToken->getElementChildrenSize()};
}
@@ -66,7 +66,7 @@ namespace armarx::aron::typeIO::reader
bool NlohmannJSONReader::readEndObject()
{
NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::eObject);
+ token->assertType(type::Descriptor::eObject);
if (token->finishedElement())
{
@@ -79,7 +79,7 @@ namespace armarx::aron::typeIO::reader
int NlohmannJSONReader::readStartDict()
{
nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::eDict, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eDict, current_json);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -87,7 +87,7 @@ namespace armarx::aron::typeIO::reader
bool NlohmannJSONReader::readEndDict()
{
NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::eDict);
+ token->assertType(type::Descriptor::eDict);
if (token->finishedElement())
{
@@ -100,7 +100,7 @@ namespace armarx::aron::typeIO::reader
int NlohmannJSONReader::readStartList()
{
nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::eList, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eList, current_json);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -108,7 +108,7 @@ namespace armarx::aron::typeIO::reader
bool NlohmannJSONReader::readEndList()
{
NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::eList);
+ token->assertType(type::Descriptor::eList);
if (token->finishedElement())
{
@@ -121,7 +121,7 @@ namespace armarx::aron::typeIO::reader
int NlohmannJSONReader::readStartTuple()
{
nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::eTuple, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eTuple, current_json);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -129,7 +129,7 @@ namespace armarx::aron::typeIO::reader
bool NlohmannJSONReader::readEndTuple()
{
NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::eTuple);
+ token->assertType(type::Descriptor::eTuple);
if (token->finishedElement())
{
@@ -142,7 +142,7 @@ namespace armarx::aron::typeIO::reader
int NlohmannJSONReader::readStartPair()
{
nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::ePair, current_json);
+ auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::ePair, current_json);
stack.push(newToken);
return newToken->getElementChildrenSize();
}
@@ -150,7 +150,7 @@ namespace armarx::aron::typeIO::reader
bool NlohmannJSONReader::readEndPair()
{
NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::ePair);
+ token->assertType(type::Descriptor::ePair);
if (token->finishedElement())
{
@@ -268,6 +268,11 @@ namespace armarx::aron::typeIO::reader
// Helper functions
type::Descriptor NlohmannJSONReader::getTypeOfNext() const
{
+ if (stack.empty())
+ {
+ return type::Descriptor::eObject;
+ }
+
auto token = stack.top();
return token->getTypeOfNext();
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index 78b3c04ab5fe09cef34cee4c96c5749a9eff908c..3d3503af2567199be5ebf627f45813f2b220c606 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -57,14 +57,14 @@ namespace armarx::aron::typeIO::reader
{
case type::Descriptor::eObject:
{
- for (auto it = type.begin(); it != type.end(); ++it)
+ for (auto& [key, val] : type.get<nlohmann::json::object_t>())
{
- if (it.key() == io::Data::READER_WRITER_NAME_SLUG)
+ if (key == io::Data::READER_WRITER_NAME_SLUG)
{
continue;
}
- allMemberNames.push_back(it.key());
+ allMemberNames.push_back(key);
}
childrenSize = allMemberNames.size();
elementName = element[io::Data::READER_WRITER_NAME_SLUG];
@@ -85,7 +85,7 @@ namespace armarx::aron::typeIO::reader
childrenSize = 1;
break;
}
- case type::ePair:
+ case type::Descriptor::ePair:
{
childrenSize = 2;
break;
@@ -103,87 +103,87 @@ namespace armarx::aron::typeIO::reader
{
if (next.find(io::Data::READER_WRITER_DICT_ACCEPTED_TYPE_SLUG) != next.end())
{
- return type::eDict;
+ return type::Descriptor::eDict;
}
if (next.find(io::Data::READER_WRITER_LIST_ACCEPTED_TYPE_SLUG) != next.end())
{
- return type::eList;
+ return type::Descriptor::eList;
}
if (next.find(io::Data::READER_WRITER_INT_ENUM_NAME_SLUG) != next.end())
{
- return type::eIntEnum;
+ return type::Descriptor::eIntEnum;
}
if (next.find(io::Data::READER_WRITER_NDARRAY_NAME_SLUG) != next.end())
{
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "EigenMatrix")
{
- return type::eEigenMatrix;
+ return type::Descriptor::eEigenMatrix;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "EigenQuaternion")
{
- return type::eEigenQuaternion;
+ return type::Descriptor::eEigenQuaternion;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "IVTCByteImage")
{
- return type::eIVTCByteImage;
+ return type::Descriptor::eIVTCByteImage;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "OpenCVMat")
{
- return type::eOpenCVMat;
+ return type::Descriptor::eOpenCVMat;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "PCLPointCloud")
{
- return type::ePCLPointCloud;
+ return type::Descriptor::ePCLPointCloud;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Position")
{
- return type::ePosition;
+ return type::Descriptor::ePosition;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Orientation")
{
- return type::eOrientation;
+ return type::Descriptor::eOrientation;
}
if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Pose")
{
- return type::ePose;
+ return type::Descriptor::ePose;
}
throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Found indicator of NDArray but could not resolve real type. Found JSON: " + next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG].dump(2));
}
- return type::eObject;
+ return type::Descriptor::eObject;
}
if (next.is_array())
{
- return type::eTuple;
+ return type::Descriptor::eTuple;
}
if (next.is_string())
{
if (next == io::Data::READER_WRITER_INT_TYPENAME_SLUG)
{
- return type::eInt;
+ return type::Descriptor::eInt;
}
if (next == io::Data::READER_WRITER_LONG_TYPENAME_SLUG)
{
- return type::eLong;
+ return type::Descriptor::eLong;
}
if (next == io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG)
{
- return type::eFloat;
+ return type::Descriptor::eFloat;
}
if (next == io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG)
{
- return type::eDouble;
+ return type::Descriptor::eDouble;
}
if (next == io::Data::READER_WRITER_STRING_TYPENAME_SLUG)
{
- return type::eString;
+ return type::Descriptor::eString;
}
if (next == io::Data::READER_WRITER_BOOL_TYPENAME_SLUG)
{
- return type::eBool;
+ return type::Descriptor::eBool;
}
if (next == io::Data::READER_WRITER_TIME_TYPENAME_SLUG)
{
- return type::eTime;
+ return type::Descriptor::eTime;
}
}
throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Could not convert to type::Descriptor enum. Found JSON: " + next.dump(2));
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
index 99005c64e9a148fad4cbf4b54932d10f68332a5a..1b0bb02c4ea191472b8a5502ade389fefcc30028 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
@@ -33,137 +33,137 @@
namespace armarx::aron::typeIO
{
- void Visitor::SetupWriterFromAronTypePtr(WriterInterface& writer, const typenavigator::NavigatorPtr& aron)
+ void Visitor::VisitAndSetup(WriterInterface& writer, const typenavigator::NavigatorPtr& aron)
{
- SetupWriterFromAronTypePtr(writer, aron->getResult());
+ VisitAndSetup(writer, aron->getResult());
}
- void Visitor::SetupWriterFromAronTypePtr(WriterInterface& writer, const type::AronTypePtr& aron)
+ void Visitor::VisitAndSetup(WriterInterface& writer, const type::AronTypePtr& aron)
{
type::Descriptor desc = Resolver::GetDescriptor(aron);
switch (desc)
{
- case type::eObject:
+ case type::Descriptor::eObject:
{
type::AronObjectPtr casted = type::AronObjectPtr::dynamicCast(aron);
writer.writeStartObject(casted->objectName);
for (const auto& [key, value] : casted->elementTypes)
{
writer.writeKey(key);
- Visitor::SetupWriterFromAronTypePtr(writer, value);
+ Visitor::VisitAndSetup(writer, value);
}
writer.writeEndObject();
break;
}
- case type::eDict:
+ case type::Descriptor::eDict:
{
type::AronDictPtr casted = type::AronDictPtr::dynamicCast(aron);
writer.writeStartDict();
- Visitor::SetupWriterFromAronTypePtr(writer, casted->acceptedType);
+ Visitor::VisitAndSetup(writer, casted->acceptedType);
writer.writeEndDict();
break;
}
- case type::eTuple:
+ case type::Descriptor::eTuple:
{
type::AronTuplePtr casted = type::AronTuplePtr::dynamicCast(aron);
writer.writeStartTuple();
for (const auto& value : casted->elementTypes)
{
- Visitor::SetupWriterFromAronTypePtr(writer, value);
+ Visitor::VisitAndSetup(writer, value);
}
writer.writeEndTuple();
break;
}
- case type::eList:
+ case type::Descriptor::eList:
{
type::AronListPtr casted = type::AronListPtr::dynamicCast(aron);
writer.writeStartList();
- Visitor::SetupWriterFromAronTypePtr(writer, casted->acceptedType);
+ Visitor::VisitAndSetup(writer, casted->acceptedType);
writer.writeEndList();
break;
}
- case type::eEigenMatrix:
+ case type::Descriptor::eEigenMatrix:
{
type::AronEigenMatrixPtr casted = type::AronEigenMatrixPtr::dynamicCast(aron);
writer.writeEigenMatrix(casted->dimensions, casted->typeName);
break;
}
- case type::eEigenQuaternion:
+ case type::Descriptor::eEigenQuaternion:
{
type::AronEigenQuaternionPtr casted = type::AronEigenQuaternionPtr::dynamicCast(aron);
writer.writeEigenQuaternion(casted->typeName);
break;
}
- case type::eIVTCByteImage:
+ case type::Descriptor::eIVTCByteImage:
{
type::AronIVTCByteImagePtr casted = type::AronIVTCByteImagePtr::dynamicCast(aron);
writer.writeIVTCByteImage(casted->width, casted->height, casted->typeName);
break;
}
- case type::eOpenCVMat:
+ case type::Descriptor::eOpenCVMat:
{
type::AronOpenCVMatPtr casted = type::AronOpenCVMatPtr::dynamicCast(aron);
writer.writeOpenCVMat(casted->dimensions, casted->typeName);
break;
}
- case type::ePCLPointCloud:
+ case type::Descriptor::ePCLPointCloud:
{
type::AronPCLPointCloudPtr casted = type::AronPCLPointCloudPtr::dynamicCast(aron);
writer.writePCLPointCloud(casted->width, casted->height, casted->typeName);
break;
}
- case type::ePosition:
+ case type::Descriptor::ePosition:
{
writer.writePosition();
break;
}
- case type::eOrientation:
+ case type::Descriptor::eOrientation:
{
writer.writeOrientation();
break;
}
- case type::ePose:
+ case type::Descriptor::ePose:
{
writer.writePose();
break;
}
- case type::eInt:
+ case type::Descriptor::eInt:
{
type::AronIntPtr casted = type::AronIntPtr::dynamicCast(aron);
writer.writeInt();
break;
}
- case type::eLong:
+ case type::Descriptor::eLong:
{
type::AronLongPtr casted = type::AronLongPtr::dynamicCast(aron);
writer.writeLong();
break;
}
- case type::eFloat:
+ case type::Descriptor::eFloat:
{
type::AronFloatPtr casted = type::AronFloatPtr::dynamicCast(aron);
writer.writeFloat();
break;
}
- case type::eDouble:
+ case type::Descriptor::eDouble:
{
type::AronDoublePtr casted = type::AronDoublePtr::dynamicCast(aron);
writer.writeDouble();
break;
}
- case type::eString:
+ case type::Descriptor::eString:
{
type::AronStringPtr casted = type::AronStringPtr::dynamicCast(aron);
writer.writeString();
break;
}
- case type::eBool:
+ case type::Descriptor::eBool:
{
type::AronBoolPtr casted = type::AronBoolPtr::dynamicCast(aron);
writer.writeBool();
break;
}
- case type::eTime:
+ case type::Descriptor::eTime:
{
type::AronTimePtr casted = type::AronTimePtr::dynamicCast(aron);
writer.writeTime();
@@ -171,7 +171,7 @@ namespace armarx::aron::typeIO
}
default:
{
- throw error::DescriptorNotValidException("LegacyAronTypeWriter", "SetupWriterFromAronTypePtr", "Type-Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
+ throw error::DescriptorNotValidException("Visitor", "SetupWriterFromAronTypePtr", "Type-Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
}
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h
index 6f781b3dae61236e202ffffd0fd243f89732f5d7..fc9ea5b879011b109b5831418feab0e373311aef 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h
@@ -41,8 +41,8 @@ namespace armarx::aron::typeIO
Visitor() = delete;
public:
- static void SetupWriterFromAronTypePtr(WriterInterface&, const typenavigator::NavigatorPtr&);
- static void SetupWriterFromAronTypePtr(WriterInterface&, const type::AronTypePtr&);
+ static void VisitAndSetup(WriterInterface&, const typenavigator::NavigatorPtr&);
+ static void VisitAndSetup(WriterInterface&, const type::AronTypePtr&);
public:
};
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h
index 2b858da0c52b65d3e13d72b4ec3d11edc7ef7e4d..37be1d9cae29ffd46a84ab818fd3ac9f6967b705 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h
@@ -86,7 +86,7 @@ namespace armarx::aron::typeIO
protected:
// members
- type::Descriptor descriptor = type::eUnknown;
+ type::Descriptor descriptor = type::Descriptor::eUnknown;
ElementTypename element;
// current index
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h
index 20b337298c61c58e0665fb1bcf21bb54eb1d845d..d94350f9df9a19ef04898b70e34449c883062a95 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h
@@ -56,32 +56,32 @@ namespace armarx::aron::typeIO::writer
{
switch (descriptor)
{
- case type::eDict:
+ case type::Descriptor::eDict:
{
typenavigator::DictNavigatorPtr casted = typenavigator::DictNavigator::DynamicCastAndCheck(element);
casted->setAcceptedType(n);
break;
}
- case type::eList:
+ case type::Descriptor::eList:
{
typenavigator::ListNavigatorPtr casted = typenavigator::ListNavigator::DynamicCastAndCheck(element);
casted->setAcceptedType(n);
break;
}
- case type::eObject:
+ case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(element);
casted->addMemberType(currentKey, n);
break;
}
- case type::eTuple:
+ case type::Descriptor::eTuple:
{
typenavigator::TupleNavigatorPtr casted = typenavigator::TupleNavigator::DynamicCastAndCheck(element);
casted->addAcceptedType(n);
currentIndex++;
break;
}
- case type::ePair:
+ case type::Descriptor::ePair:
{
typenavigator::PairNavigatorPtr casted = typenavigator::PairNavigator::DynamicCastAndCheck(element);
casted->addAcceptedType(n);
@@ -98,13 +98,13 @@ namespace armarx::aron::typeIO::writer
{
switch (descriptor)
{
- case type::eObject:
+ case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr casted = typenavigator::ObjectNavigator::DynamicCast(element);
casted->setObjectName(n);
break;
}
- case type::eIntEnum:
+ case type::Descriptor::eIntEnum:
{
typenavigator::IntEnumNavigatorPtr casted = typenavigator::IntEnumNavigator::DynamicCast(element);
casted->setEnumName(n);
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index 187c648096a0fa0e3c9e3b406b1dbc4a7f0b1dcf..0f3e96192baf5c2c5d088e0c3a5a2fb4e354d68d 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -30,7 +30,7 @@ namespace armarx::aron::typeIO::writer
{
nlohmann::json data;
data[io::Data::READER_WRITER_NAME_SLUG] = n;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eObject, data);
stack.push(new_token);
}
@@ -68,7 +68,7 @@ namespace armarx::aron::typeIO::writer
void NlohmannJSONWriter::writeStartDict()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eDict, data);
stack.push(new_token);
}
@@ -87,7 +87,7 @@ namespace armarx::aron::typeIO::writer
void NlohmannJSONWriter::writeStartTuple()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eTuple, data);
stack.push(new_token);
}
@@ -106,7 +106,7 @@ namespace armarx::aron::typeIO::writer
void NlohmannJSONWriter::writeStartPair()
{
nlohmann::json data;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
+ auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::ePair, data);
stack.push(new_token);
}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
index 826524dddcd7c6a7d2e8bf7919240bb7d1b109b0..ea4410885b98ece7ee08e8c0ffb1c7443253c5f6 100644
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
@@ -94,16 +94,5 @@ namespace armarx::aron::typeIO::writer
}
element[io::Data::READER_WRITER_NAME_SLUG] = n;
}
-
- private:
- // members
- type::Descriptor descriptor;
- nlohmann::json element;
-
- // current index
- unsigned int currentIndex;
-
- // current key
- std::string currentKey;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
index ee89003517219102eb5be1a276876434fb63630a..1adcdfff3dd6786b6042bec2b20591ad56ff3281 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
@@ -36,8 +36,7 @@ namespace armarx::aron::datanavigator
NavigatorPtr NavigatorFactory::create(const data::AronDataPtr& aron, const Path& path) const
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {data::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
-
+ {data::Descriptor::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
static const std::map<data::Descriptor, NavigatorFactoryPtr> Factories =
{
HANDLE_ALL_ARON_DATA
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
index 97942372a9c6be2667ff6639c13779fc734e933a..6806d9dd41abe290d246d7a66a036f39a499461a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
@@ -94,6 +94,10 @@ namespace armarx::aron::datanavigator
bool NDArrayNavigator::equalsDeep(const NDArrayNavigatorPtr& other) const
{
+ if (other == nullptr)
+ {
+ return false;
+ }
return *this == *other;
}
@@ -205,7 +209,7 @@ namespace armarx::aron::datanavigator
switch (type->getDescriptor())
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::e##upperType: \
+case type::Descriptor::e##upperType: \
{ \
typenavigator::upperType##NavigatorPtr casted = typenavigator::upperType##Navigator::DynamicCast(type); \
if (std::vector<int>(aron->dimensions.begin(), std::prev(aron->dimensions.end())) != casted->getDimensions() || aron->type != casted->getTypename()) \
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/data/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
index 90653ddcd7e080510528673ceb24c4c14f70af09..67a2a6d816823611376158d2862c0b5918d6d693 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
@@ -96,6 +96,10 @@ namespace armarx::aron::datanavigator
bool DictNavigator::equalsDeep(const DictNavigatorPtr& other) const
{
+ if (other == nullptr)
+ {
+ return false;
+ }
return *this == *other;
}
@@ -221,7 +225,7 @@ namespace armarx::aron::datanavigator
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
{
- case type::eObject:
+ case type::Descriptor::eObject:
{
typenavigator::ObjectNavigatorPtr objectTypeNav = typenavigator::ObjectNavigator::DynamicCast(type);
for (const auto& [key, nav] : childrenNavigators)
@@ -233,7 +237,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- case type::eDict:
+ case type::Descriptor::eDict:
{
typenavigator::DictNavigatorPtr dictTypeNav = typenavigator::DictNavigator::DynamicCast(type);
for (const auto& [_, nav] : childrenNavigators)
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
index 3cce35ea38e9f72e4be92bd185893d7d400909a2..d2d5bee4af83060dad86c93498be761f42a84f80 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
@@ -100,6 +100,10 @@ namespace armarx::aron::datanavigator
bool ListNavigator::equalsDeep(const ListNavigatorPtr& other) const
{
+ if (other == nullptr)
+ {
+ return false;
+ }
return *this == *other;
}
@@ -212,7 +216,7 @@ namespace armarx::aron::datanavigator
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
{
- case type::eList:
+ case type::Descriptor::eList:
{
typenavigator::ListNavigatorPtr listTypeNav = typenavigator::ListNavigator::DynamicCast(type);
for (const auto& nav : childrenNavigators)
@@ -224,7 +228,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- case type::eTuple:
+ case type::Descriptor::eTuple:
{
typenavigator::TupleNavigatorPtr tupleTypeNav = typenavigator::TupleNavigator::DynamicCast(type);
unsigned int i = 0;
@@ -237,7 +241,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- case type::ePair:
+ case type::Descriptor::ePair:
{
typenavigator::PairNavigatorPtr pairTypeNav = typenavigator::PairNavigator::DynamicCast(type);
if (childrenSize() != 2)
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp
index 860b760ce31c864efcdd498c5e2dfa3c2f571c90..23e117fc3f23ed1639faa0b310ba9ddd0f997e24 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.cpp
@@ -50,6 +50,11 @@ namespace armarx::aron::datanavigator
} \
\
/* operators */ \
+ upperType##Navigator::operator lowerType() const \
+ { \
+ return aron->value; \
+ } \
+ \
bool upperType##Navigator::operator==(const upperType##Navigator& other) const \
{ \
const auto& otherAron = other.toAron##upperType##Ptr(); \
@@ -71,6 +76,10 @@ namespace armarx::aron::datanavigator
bool upperType##Navigator::equalsDeep(const upperType##NavigatorPtr& other) const \
{ \
/* only for consistency. There is no "deep" comparison. */ \
+ if (other == nullptr) \
+ { \
+ return false; \
+ } \
return *this == *other; \
} \
\
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h
index 195ba609dff8b0b4b352a5c0a68ee8db0167e69b..2891a277fd1e2d13f116bce302957bcc6042f746 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h
@@ -54,6 +54,7 @@ namespace armarx::aron::datanavigator
upperType##Navigator(const lowerType&, const Path& = Path()); \
\
/* operators */ \
+ operator lowerType() const; \
bool operator==(const upperType##Navigator&) const; \
virtual bool equalsDataNavigator(const NavigatorPtr&) const override; \
bool equalsDeep(const upperType##NavigatorPtr&) const; \
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
index 48d135200d0590e175128d771706b5d19d3bfa3a..f0375363b95993f9e216d1e140fa459ea99a6913 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
@@ -35,8 +35,7 @@ namespace armarx::aron::typenavigator
NavigatorPtr NavigatorFactory::create(const type::AronTypePtr& aron, const Path& path) const
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {type::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
-
+ {type::Descriptor::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
static const std::map<type::Descriptor, NavigatorFactoryPtr> Factories =
{
HANDLE_ALL_ARON_TYPES
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
index 8f82d1d0776d9b4629f6a75379b56f70661e9531..bc030f335ae8d67ecd9160e949a8943d99196e16 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
@@ -24,6 +24,9 @@
// Header
#include "Object.h"
+// ArmarX
+#include "SimoxUtility/algorithm/get_map_keys_values.h"
+
namespace armarx::aron::typenavigator
{
@@ -88,6 +91,10 @@ namespace armarx::aron::typenavigator
NavigatorPtr ObjectNavigator::getMemberType(const std::string& s) const
{
+ if (memberTypes.find(s) == memberTypes.end())
+ {
+ throw error::StringNotValidException("ObjectNavigator", "getMemberType", "Member not set. The list of all members is: " + simox::alg::to_string(simox::alg::get_keys(memberTypes)), s);
+ }
return memberTypes.at(s);
}
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/aron/core/test/aronTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
index 1210ed0a5d0058825dede94196653f0af3fc201f..e8fd551be59727494f1c92040d81a8bddf1fe8bd 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
@@ -154,7 +154,7 @@ void runTestWithInstances(T& k1, T& k2)
std::string k1_aron_json_str = k1_aron_json.dump(4);
armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_k1;
- armarx::aron::dataIO::Visitor::SetupWriterFromAronDataPtr(direct_json_writer_for_k1, k1_aron);
+ armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_k1, k1_aron);
nlohmann::json direct_k1_aron_json = direct_json_writer_for_k1.getResult();
std::string direct_k1_aron_json_str = direct_k1_aron_json.dump(4);
BOOST_CHECK_EQUAL((k1_aron_json_str == direct_k1_aron_json_str), true);
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)