diff --git a/.gitignore b/.gitignore
index fe771d93cccfb4e0a5c37769f7d8f55de79ba029..aba6e15d3795100181de6448968fc7d75a77e367 100644
--- a/.gitignore
+++ b/.gitignore
@@ -75,3 +75,4 @@ etc/python/dist/*
# ArViz Recordings
data/RobotAPI/ArVizStorage/
+.vscode/
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 2388c61e39a8e0582f63b11dec0ca45ec51c8d24..fac11b677edbf7fc26cd2538e453981601e94f13 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -328,5 +328,13 @@
propertyName="ObjectPoseStorageName"
propertyIsOptional="true"
propertyDefaultValue="ObjectMemory" />
+ <Proxy include="RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h"
+ humanName="Memory Name System"
+ typeName="armem::mns::MemoryNameSystemInterfacePrx"
+ memberName="MemoryNameSystem"
+ getterName="getMemoryNameSystem"
+ propertyName="MemoryNameSystemName"
+ propertyIsOptional="true"
+ propertyDefaultValue="MemoryNameSystem" />
</Lib>
</VariantInfo>
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 95ec0bddab3697b5270df9c454318d38b63e6467..e6f93e12cda04bf7a80e6a41daada5189f2ef787 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -9,5 +9,6 @@
<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"/>
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5082b0ffc78b15d395b64deb6f0ba3c14ac38ea9
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
@@ -0,0 +1,251 @@
+# ==================================================================
+# ArticulatedObjectExampleMemoryWriterClient 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.ArticulatedObjectExampleMemoryWriterClient.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.ArticulatedObjectExampleMemoryWriterClient.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.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.ArticulatedObjectExampleMemoryWriterClient.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ArMemMemoryNameSystem: Ice object name of the `MemoryNameSystem` component.
+# Attributes:
+# - Default: ArMemMemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ArMemMemoryNameSystem = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem: No Description
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName: No Description
+# Attributes:
+# - Default: Object
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName: No Description
+# Attributes:
+# - Default: ExampleProvider
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver = DebugObserver
+
+
+# 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.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/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..82769e0f22bfa6de0ca678be094d396df07c89e0
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -0,0 +1,254 @@
+# ==================================================================
+# ArticulatedObjectLocalizerExample 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.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
+# Attributes:
+# - Default: ArmarXObjects
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
+# 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.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/ArMemObjectMemory/config/ArticulatedObjectStateProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectStateProviderExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..392cba9cc038fcc870930199b577e5699b2d9b48
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectStateProviderExample.cfg
@@ -0,0 +1,204 @@
+# ==================================================================
+# ArticulatedObjectStateProviderExample 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.ArticulatedObjectStateProviderExample.ArticulatedObjectTopicName: Name of the articulated object topic.
+# Attributes:
+# - Default: ArticulatedObjectTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectStateProviderExample.ArticulatedObjectTopicName = ArticulatedObjectTopic
+
+
+# ArmarX.ArticulatedObjectStateProviderExample.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.ArticulatedObjectStateProviderExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectStateProviderExample.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.ArticulatedObjectStateProviderExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectStateProviderExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectStateProviderExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectStateProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectStateProviderExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# 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.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/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 22981bc341684bcac781c01b26249ba6376c1ad2..df483eb31778702f71c8a1ff6c13bf260f86c605 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -7,7 +7,7 @@
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
+ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ApplicationName: Application name
@@ -92,6 +92,14 @@
# ArmarX.LoggingGroup = ""
+# ArmarX.ObjectMemory.:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory. = ""
+
+
# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
@@ -158,6 +166,71 @@
# ArmarX.ObjectMemory.mem.MemoryName = Object
+# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
+
+
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage = true
+
+
+# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage: Name of the objects package to load from.
+# Attributes:
+# - Default: ArmarXObjects
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
+
+
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: ArticulatedObjectInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
+
+
+# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Attachments
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName = Attachments
+
+
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize = -1
+
+
# ArmarX.ObjectMemory.mem.cls.CoreSegmentName: Name of the object clazz core segment.
# Attributes:
# - Default: Class
@@ -166,6 +239,41 @@
# ArmarX.ObjectMemory.mem.cls.CoreSegmentName = Class
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Environment/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Environment/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = true
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
# - Default: true
@@ -183,12 +291,12 @@
# ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage: Name of the objects package to load from.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
@@ -211,10 +319,10 @@
# ArmarX.ObjectMemory.mem.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
# Attributes:
-# - Default: -1
+# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.inst.MaxHistorySize = -1
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize = 25
# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
diff --git a/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
new file mode 100644
index 0000000000000000000000000000000000000000..9622c34e544899a5f2e81e01f5f6f7ad6744b013
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="RobotSkillsMemory" creation="2021-05-07.11:08:24" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg b/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/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/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp 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.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.RemoteGuiProvider.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.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.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.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# 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/RobotSkillsMemory/config/SkillsMemory.cfg b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..57a92b84c82797807a38c8635dbe1fb7d3d3d43d
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
@@ -0,0 +1,262 @@
+# ==================================================================
+# SkillsMemory 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.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.SkillsMemory.:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory. = ""
+
+
+# ArmarX.SkillsMemory.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.SkillsMemory.EnableProfiling = false
+
+
+# ArmarX.SkillsMemory.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.SkillsMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SkillsMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.ObjectName = ""
+
+
+# ArmarX.SkillsMemory.StatechartCoreSegmentName: Name of the core segment for statecharts.
+# Attributes:
+# - Default: Statechart
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.StatechartCoreSegmentName = Statechart
+
+
+# ArmarX.SkillsMemory.TransitionsProviderSegmentName: Name of the provider segment for statechart transitions.
+# Attributes:
+# - Default: Transitions
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.TransitionsProviderSegmentName = Transitions
+
+
+# ArmarX.SkillsMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Skills
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.mem.MemoryName = Skills
+
+
+# ArmarX.SkillsMemory.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.SkillsMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.SkillsMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.SkillsMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.SkillsMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.SkillsMemory.tpc.sub.ProfilerListener: Name of the ProfilerListenerInterface topics to subscribe.
+# Attributes:
+# - Default: StateReportingTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillsMemory.tpc.sub.ProfilerListener = StateReportingTopic
+
+
+# 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/RobotSkillsMemory/config/global.cfg b/scenarios/RobotSkillsMemory/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f9f5f6c52197a68f353b2a5a3f1f7cc7d4d6272b
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario RobotSkillsMemory
+# ==================================================================
+
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 71e4197cc71b0ec094ecec84b47a4e35f0a3854a..f946e02d4298910c72bb4d8a59bf9d64d42ac5ab 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -15,12 +15,14 @@ namespace armarx::viz
{
public:
Client() = default;
+ virtual ~Client() = default;
Client(armarx::Component& component, std::string topicNameProperty = "ArVizTopicName")
{
componentName = component.getName();
component.getTopicFromProperty(_topic, topicNameProperty);
}
+
Client(ManagedIceObject& obj, const std::string& topicName = "ArVizTopic")
{
componentName = obj.getName();
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp b/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp
index 2e968ca7dd19298ac9eff6080370edb73a5138ca..ed2f2221ef763270c461ab3c1c1bc436ac26a2a9 100644
--- a/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.cpp
@@ -24,6 +24,7 @@
namespace armarx::viz
{
+ ScopedClient::ScopedClient(const Client& client): Client(client) {}
Layer ScopedClient::layer(std::string const& name) const
{
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.h b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
index 23271b5ba6de545b183728ba04fdaa86f7938e0a..78adbcab32013de4b82f9a35671063e66a4aaa11 100644
--- a/source/RobotAPI/components/ArViz/Client/ScopedClient.h
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
@@ -28,7 +28,7 @@ namespace armarx::viz
/**
* @brief `viz::Client` that will delete (clear) committed layers when destroyed.
*
- * Note that, as a consequence, a network call be performed in the destructor.
+ * Note that, as a consequence, a network call will be performed in the destructor.
*
* This might be useful if you have a class `MyTask` perform visualizing while performing some task,
* but whose visualization should be removed when the task finished.
@@ -41,6 +41,7 @@ namespace armarx::viz
{
public:
using Client::Client;
+ ScopedClient(const Client& client);
Layer layer(std::string const& name) const override;
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1e48cdea6aa941033225c291e50a3da312f9475
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -0,0 +1,172 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ArticulatedObjectLocalizerExample.h"
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx::articulated_object
+{
+ ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
+
+ armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
+
+ articulatedObjectWriter->registerPropertyDefinitions(defs);
+ articulatedObjectReader->registerPropertyDefinitions(defs);
+
+ return defs;
+ }
+
+ std::string ArticulatedObjectLocalizerExample::getDefaultName() const
+ {
+ return "ArticulatedObjectLocalizerExample";
+ }
+
+ void ArticulatedObjectLocalizerExample::onInitComponent() {}
+
+ void ArticulatedObjectLocalizerExample::onConnectComponent()
+ {
+ articulatedObjectWriter->connect();
+ articulatedObjectReader->connect();
+
+ ARMARX_IMPORTANT << "Running example.";
+ start = armem::Time::now();
+
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency);
+ task->start();
+ }
+
+ void ArticulatedObjectLocalizerExample::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void ArticulatedObjectLocalizerExample::onExitComponent() {}
+
+ VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
+ {
+ const std::string dishwasherName = "CupboardWithDishwasher";
+
+ const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+
+ const auto it = std::find_if(descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+ {
+ return desc.name == dishwasherName;
+ });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << dishwasherName << " not (yet) available";
+ return nullptr;
+ }
+
+ return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
+ }
+
+ armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+ return
+ armem::articulated_object::ArticulatedObject
+ {
+ .description = {
+ .name = obj.getName(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
+ },
+ .instance = "", // TODO(fabian.reister):
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()
+ },
+ .timestamp = timestamp
+ };
+ }
+
+ void ArticulatedObjectLocalizerExample::run()
+ {
+ if (dishwasher == nullptr)
+ {
+ dishwasher = createDishwasher();
+
+ if (dishwasher == nullptr) // still
+ {
+ return;
+ }
+ }
+
+ ARMARX_DEBUG << "Reporting articulated objects";
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const float t = float((now - start).toSecondsDouble());
+
+ // move joints at certain frequency
+ const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
+
+ const std::map<std::string, float> jointValues
+ {
+ {"dishwasher_door_joint", M_PIf32 / 2 * k},
+ {"drawer_joint", 350 * k}
+ };
+
+ dishwasher->setJointValues(jointValues);
+
+ armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
+ articulatedObjectWriter->store(armemDishwasher);
+ }
+
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..75c0764cfb9c6103a82c68c0faba2b984ffabb86
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -0,0 +1,81 @@
+
+#pragma once
+
+
+// ArmarX
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// RobotAPI
+#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/workingmemory/Memory.h>
+
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+namespace armarx::articulated_object
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class ArticulatedObjectLocalizerExample :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
+ {
+ public:
+ ArticulatedObjectLocalizerExample();
+
+ /// @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;
+
+ void run();
+
+
+ private:
+
+ VirtualRobot::RobotPtr createDishwasher();
+ std::shared_ptr<VirtualRobot::Robot> dishwasher;
+
+ /// Reference timestamp for object movement
+ armem::Time start;
+
+ armarx::PeriodicTask<ArticulatedObjectLocalizerExample>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+
+ struct Properties
+ {
+ float updateFrequency{25.F};
+ } p;
+
+ };
+
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e92a5b368905d9046bb439cf435c9ae4276aed3e
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME ArticulatedObjectLocalizerExample)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+# Add the component
+armarx_add_component(
+ COMPONENT_LIBS
+ ArmarXCore
+ armem_objects
+ SOURCES
+ ArticulatedObjectLocalizerExample.cpp
+
+ HEADERS
+ ArticulatedObjectLocalizerExample.h
+)
+
+# Add unit tests
+add_subdirectory(test)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+ # If your component is not defined in ::armarx, specify its namespace here:
+ COMPONENT_NAMESPACE "armarx::articulated_object"
+)
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..551ba1265648103ef7d94282bc320ea486e3e3a6
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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::ArticulatedObjectLocalizerExample
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ArticulatedObjectLocalizerExample
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArticulatedObjectLocalizerExample.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::articulated_object::ArticulatedObjectLocalizerExample instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e67df2dc9f93e582d59ce368103bcc022cf7843b
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ArticulatedObjectLocalizerExample)
+
+armarx_add_test(ArticulatedObjectLocalizerExampleTest ArticulatedObjectLocalizerExampleTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index d19343cee661810483206528bfccaa62c614bff4..97d8648b4fde12137ab393ed95bcae6ae2a8648c 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -30,3 +30,5 @@ add_subdirectory(ViewSelection)
add_subdirectory(SkillObserver)
+
+add_subdirectory(ArticulatedObjectLocalizerExample)
\ No newline at end of file
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
index 1395fc02f564792a7ffe7e0963264f5c0ac6fb9e..b6bd42b716b7fc14a7e3eb3b0550b9f81411b804 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
@@ -3,7 +3,8 @@ armarx_component_set_name("ObjectPoseProviderExample")
set(COMPONENT_LIBS
# ArmarXCore
- ArmarXCore ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreInterfaces
# RobotAPI
RobotAPI::ArmarXObjects
)
@@ -21,3 +22,4 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
#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 f759697348f61927af7df5c9297e2081b0c1db91..c0ef77b2f4b728d245c446b9782336843157d9e6 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
namespace armarx
{
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 4eda2528386b1edd333bab29f06480255434b69d..de02c9f4e7ef45534c2675db6173205436b4475c 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1 +1,2 @@
add_subdirectory(ExampleMemoryClient)
+add_subdirectory(VirtualRobotReaderExampleClient)
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cd132bb87336bf793e86e79b7e048ea044921491
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("VirtualRobotReaderExampleClient")
+
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore
+ RobotAPIInterfaces
+ armem_robot_state
+)
+
+set(SOURCES
+ VirtualRobotReaderExampleClient.cpp
+)
+
+set(HEADERS
+ VirtualRobotReaderExampleClient.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::robot_state")
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85ca85f48ca62c7bc686bd9286d52fae5480ebc7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -0,0 +1,105 @@
+
+
+#include "VirtualRobotReaderExampleClient.h"
+
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+
+namespace armarx::robot_state
+{
+ VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
+ virtualRobotReader(*this) {}
+
+ armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(p.robotName, "robotName");
+ defs->optional(p.updateFrequency, "updateFrequency");
+
+ virtualRobotReader.registerPropertyDefinitions(defs);
+
+ return defs;
+ }
+
+ std::string VirtualRobotReaderExampleClient::getDefaultName() const
+ {
+ return "VirtualRobotReaderExampleClient";
+ }
+
+ void VirtualRobotReaderExampleClient::onInitComponent() {}
+
+ void VirtualRobotReaderExampleClient::onConnectComponent()
+ {
+ virtualRobotReader.connect();
+
+ ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
+
+ task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task->start();
+ }
+
+ void VirtualRobotReaderExampleClient::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void VirtualRobotReaderExampleClient::onExitComponent() {}
+
+ void VirtualRobotReaderExampleClient::run()
+ {
+
+ // initialize if needed
+ if (virtualRobot == nullptr)
+ {
+ TIMING_START(getRobot);
+
+ virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
+
+ if (virtualRobot == nullptr)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
+ return;
+ }
+ // only print timing once the robot is loadable & loaded
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+
+ TIMING_START(synchronizeRobot);
+ virtualRobotReader.synchronizeRobot(*virtualRobot, now);
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+
+ }
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e1d01f5564e96c7e46ad2db33081835873dd7a6
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -0,0 +1,96 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// ArmarX
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// RobotAPI
+#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/workingmemory/Memory.h>
+
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+namespace armarx::robot_state
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class VirtualRobotReaderExampleClient :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
+ {
+ public:
+ VirtualRobotReaderExampleClient();
+
+ /// @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;
+
+ void run();
+
+
+ private:
+
+ struct Properties
+ {
+ std::string robotName{"Armar6"};
+ float updateFrequency{10.F};
+ } p;
+
+ armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+
+ armem::robot_state::VirtualRobotReader virtualRobotReader;
+
+ std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+
+ };
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 18ccb6875d1611e05cf20a8fa714dd293df995e6..b6d997f6882daa1b9ee95d97fb4502ae181aa9ab 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -22,6 +22,8 @@
#include "ObjectMemory.h"
+#include <mutex>
+
namespace armarx::armem::server::obj
{
@@ -52,6 +54,11 @@ namespace armarx::armem::server::obj
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+ articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
+ articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
+
+ attachmentSegment.defineProperties(defs, prefix + "attachments.");
+
return defs;
}
@@ -60,7 +67,13 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
classSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ server::ComponentPluginUser::workingMemoryMutex),
+ articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ attachmentSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
{
}
@@ -75,22 +88,47 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::init();
- try
+ const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
+ {
+ try
+ {
+ fn();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment for unknown reason.";
+ }
+ };
+
+ initSegmentWithCatch("class", [&]()
{
classSegment.init();
- }
- catch (const LocalException& e)
+ });
+
+ initSegmentWithCatch("articulated object class", [&]()
{
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (const std::exception& e)
+ articulatedObjectClassSegment.init();
+ });
+
+ initSegmentWithCatch("articulated object instance", [&]()
{
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (...)
+ articulatedObjectInstanceSegment.setArticulatedObjectClassSegment(articulatedObjectClassSegment);
+ articulatedObjectInstanceSegment.init();
+ });
+
+ initSegmentWithCatch("attachment", [&]()
{
- ARMARX_ERROR << "Failed to init class segment for unknown reason.";
- }
+ attachmentSegment.init();
+ });
+
}
void ObjectMemory::onConnectComponent()
@@ -107,17 +145,31 @@ namespace armarx::armem::server::obj
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
instance::SegmentAdapter::connect(
- robotStateComponent, robot,
+ robotStateComponent,
+ robot,
kinematicUnitObserver,
- ArVizComponentPluginUser::arviz,
+ ArVizComponentPluginUser::getArvizClient(),
debugObserver
);
classSegment.connect(
- ArVizComponentPluginUser::arviz
+ ArVizComponentPluginUser::getArvizClient()
+ );
+
+ articulatedObjectClassSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
+ );
+
+ articulatedObjectInstanceSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
+ );
+
+ attachmentSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
);
createRemoteGuiTab();
RemoteGui_startRunningTask();
+
}
void ObjectMemory::onDisconnectComponent()
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 65bd9c472828faa2b8b7f58679c4922c1aef389d..2dfd668d1e43df914ff7b2f02205e699ad2ea5e9 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -38,6 +38,9 @@
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -108,6 +111,11 @@ namespace armarx::armem::server::obj
clazz::Segment classSegment;
+ articulated_object_class::Segment articulatedObjectClassSegment;
+ articulated_object_instance::Segment articulatedObjectInstanceSegment;
+ attachments::Segment attachmentSegment;
+
+ // associations::Segment associationsSegment;
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
{
@@ -118,6 +126,6 @@ namespace armarx::armem::server::obj
};
-}
+} // namespace armarx::armem::server::obj
#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 4667365b1a6187694ca7bdbd06003275828ead55..137453970c1a97f0f3bd9653c067e959db21b4a5 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -4,8 +4,11 @@ armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
)
set(SOURCES
@@ -18,7 +21,7 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
#generate the application
-armarx_generate_and_add_component_executable()
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server::robot_state)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 1624f899ca6dbae34d3ef210d84559c3a1390b2b..2a3d01143be737e9afb7ce5c27253e2cb6d4891e 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -20,39 +20,69 @@
* GNU General Public License
*/
+#include "RobotStateMemory.h"
+
// STD
#include <algorithm>
#include <iostream>
#include <fstream>
+#include <memory>
-// Header
-#include "RobotStateMemory.h"
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
// Simox
#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx::armem::server::robot_state
{
RobotStateMemory::RobotStateMemory()
+ : descriptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ proprioceptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ localizationSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
+
}
+ RobotStateMemory::~RobotStateMemory() = default;
+
+
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ const std::string robotUnitPrefix{sensorValuePrefix};
+
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- defs->optional(robotUnitSensorPrefix, "SensorValuePrefix", "Prefix of all sensor values");
- defs->optional(robotUnitMemoryBatchSize, "RobotUnitMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
- defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
+ defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
+ defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
+ defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
+ defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
+
+ descriptionSegment.defineProperties(defs);
+ proprioceptionSegment.defineProperties(defs);
+ localizationSegment.defineProperties(defs);
+ commonVisu.defineProperties(defs);
return defs;
}
@@ -66,10 +96,15 @@ namespace armarx
void RobotStateMemory::onInitComponent()
{
+
+ descriptionSegment.init();
+ proprioceptionSegment.init();
+ localizationSegment.init();
+ commonVisu.init();
+
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
-
Ice::StringSeq includePaths;
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
@@ -95,9 +130,16 @@ namespace armarx
void RobotStateMemory::onConnectComponent()
{
- setupRobotUnitSegment();
+ descriptionSegment.connect(getArvizClient(), getRobotUnit());
- setupLocalizationCoreSegment();
+ proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
+ toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
+
+ localizationSegment.connect(getArvizClient());
+
+ commonVisu.connect(
+ getArvizClient()
+ );
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -124,45 +166,6 @@ namespace armarx
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotStateMemory::setupLocalizationCoreSegment()
- {
- ARMARX_INFO << "Adding core segment " << localizationCoreSegmentName;
- workingMemory.addCoreSegments({localizationCoreSegmentName});
- }
-
- /*************************************************************/
- // RobotUnit Streaming functions
- /*************************************************************/
- void RobotStateMemory::setupRobotUnitSegment()
- {
- ARMARX_INFO << "Adding core segment " << proprioceptionCoreSegmentName;
- workingMemory.addCoreSegments({proprioceptionCoreSegmentName});
-
- ARMARX_INFO << "Adding provider segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = proprioceptionCoreSegmentName;
- input.providerSegmentName = robotUnitProviderSegmentName;
-
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
-
- auto result = addSegments({input})[0];
-
- if (!result.success)
- {
- ARMARX_ERROR << "Could not add segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName << ". The error message is: " << result.errorMessage;
- }
-
- robotUnitProviderID.memoryName = workingMemoryName;
- robotUnitProviderID.coreSegmentName = proprioceptionCoreSegmentName;
- robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
- }
-
void RobotStateMemory::convertRobotUnitStreamingDataToAron()
{
auto& data = handler->getDataBuffer();
@@ -176,8 +179,11 @@ namespace armarx
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData convertedAndGroupedData;
- for (const auto& [name, dataEntry] : keys.entries)
+ for (const auto& [nameEntry, dataEntry] : keys.entries)
{
+ std::string name = nameEntry;
+ std::string jointOrWhateverName;
+
std::string groupName = "";
if (configSensorMapping.find(name) != configSensorMapping.end())
{
@@ -185,13 +191,18 @@ namespace armarx
}
else
{
- size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
- if (dot_pos == std::string::npos)
+ const size_t first_dot_pos = name.find(".");
+ const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
+ if (second_dot_pos == std::string::npos)
{
ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
continue;
}
- groupName = name.substr(0, dot_pos);
+ groupName = name.substr(0, second_dot_pos);
+
+ jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
+
+ name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
}
RobotUnitData::RobotUnitDataGroup e;
@@ -205,6 +216,9 @@ namespace armarx
auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
dict->addElement("IterationId", iterId);
+ const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
+ dict->addElement("name", nameId);
+
e.timestamp = currentTimestep.timestampUSec;
e.name = groupName;
e.data = dict;
@@ -294,7 +308,9 @@ namespace armarx
auto start = std::chrono::high_resolution_clock::now();
// send batch to memory
- armem::data::Commit c;
+ armem::data::Commit proprioceptionCommit;
+ armem::data::Commit odometryCommit;
+
for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
{
std::lock_guard g{robotUnitDataMutex};
@@ -309,15 +325,55 @@ namespace armarx
const auto& timeUSec = encTimestep.timestamp;
const auto& aron = encTimestep.data;
- armem::data::EntityUpdate& update = c.updates.emplace_back();
+ armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
update.entityID = entityID;
update.timeCreatedMicroSeconds = timeUSec;
update.instancesData = {aron->toAronDictPtr()};
}
+
+ // odometry pose -> localization segment
+ auto it = convertedAndGroupedData.groups.find("sens.Platform");
+ if (it != convertedAndGroupedData.groups.end())
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+
+ const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
+ const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
+ const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
+ const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+
+ // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
+ const auto timestamp = armem::Time::now();
+
+ const ::armarx::armem::robot_state::Transform transform
+ {
+ .header =
+ {
+ .parentFrame = OdometryFrame,
+ .frame = "root", // TODO: robot root node
+ .agent = robotUnitProviderID.providerSegmentName,
+ .timestamp = timestamp
+ },
+ .transform = odometryPose
+ };
+
+ localizationSegment.storeTransform(transform);
+
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
+ }
+
+
robotUnitDataQueue.pop();
}
- auto results = commit(c);
+ auto results = commit(proprioceptionCommit);
auto stop = std::chrono::high_resolution_clock::now();
ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
@@ -442,4 +498,5 @@ namespace armarx
robotUnitConversionTask->start();
robotUnitStoringTask->start();
}
+
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 490b04109a1608b6c9de61a56e44f6c92b2b9d38..e8174416e9b547f70af5e5c6b578242a0234743d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -30,18 +30,25 @@
// Simox
#include <SimoxUtility/meta/enum/adapt_enum.h>
-// BaseClass
+// ArmarX
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+// BaseClass
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-// ArmarX
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-namespace armarx
+namespace armarx::armem::server::robot_state
{
+
/**
* @defgroup Component-RobotSensorMemory RobotSensorMemory
* @ingroup RobotAPI-Components
@@ -56,12 +63,14 @@ namespace armarx
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser
-
+ virtual public RobotUnitComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser
{
public:
RobotStateMemory();
+ virtual ~RobotStateMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -83,32 +92,31 @@ namespace armarx
private:
// RobotUnit Streaming
- void setupRobotUnitSegment();
-
void convertRobotUnitStreamingDataToAron();
void storeConvertedRobotUnitDataInMemory();
void startRobotUnitStream();
void stopRobotUnitStream();
- // localization segment
- void setupLocalizationCoreSegment();
-
private:
std::string workingMemoryName = "RobotStateMemory";
bool addCoreSegmentOnUsage = false;
mutable std::recursive_mutex startStopMutex;
- // Memory IDs
+ // Core segments
+ // - description
+ description::Segment descriptionSegment;
// - proprioception
- std::string proprioceptionCoreSegmentName = "Proprioception";
- std::string robotUnitProviderSegmentName = "RobotUnit"; // get robot name?
+ proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
// - localization
- std::string localizationCoreSegmentName = "Localization";
+ localization::Segment localizationSegment;
+
+ // Joint visu for all segments => robot pose and configuration
+ Visu commonVisu;
// RobotUnit stuff
RobotUnitDataStreaming::DataStreamingDescription keys;
@@ -131,6 +139,8 @@ namespace armarx
// params
static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr const char* sensorValuePrefix = "RobotUnit.";
+
int robotUnitPollFrequency = 50;
std::string robotUnitSensorPrefix = "sens.*";
unsigned int robotUnitMemoryBatchSize = 50;
@@ -144,4 +154,5 @@ namespace armarx
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
};
-}
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
index c00bd89e9c47178acbe3ea49ac4b3d3bc3cc5436..b9d1d4fd44ed2bf58f8eccd64faa9ab0eafc83ef 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/CMakeLists.txt
@@ -2,14 +2,15 @@ armarx_component_set_name("SkillsMemory")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXCore ArmarXCoreInterfaces ArmarXCoreObservers # for DebugObserverInterface
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem
+ RobotAPICore RobotAPIInterfaces armem armem_skills
# RobotAPIComponentPlugins # for ArViz and other plugins
${IVT_LIBRARIES}
)
+
set(SOURCES
SkillsMemory.cpp
)
@@ -17,8 +18,10 @@ set(HEADERS
SkillsMemory.h
)
+
armarx_add_component("${SOURCES}" "${HEADERS}")
+
#generate the application
armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 295e2c6aee2010162c6919af97141f28989b6040..8a03cc15970b9da91dd3699ca883c94db13db522 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -22,22 +22,39 @@
#include "SkillsMemory.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
+
+
namespace armarx
{
SkillsMemory::SkillsMemory()
- {
- }
+ = default;
armarx::PropertyDefinitionsPtr SkillsMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish
+ defs->topic(debugObserver);
+
+ // Statechart Logging
+ defs->optional(p.statechartCoreSegmentName, "StatechartCoreSegmentName", "Name of the core segment for statecharts.");
+ defs->optional(p.statechartTransitionsProviderSegmentName, "TransitionsProviderSegmentName", "Name of the provider segment for statechart transitions.");
+ defs->optional(p.statechartTransitionsTopicName, "tpc.sub.ProfilerListener", "Name of the ProfilerListenerInterface topics to subscribe.");
+
+ const std::string prefix = "mem.";
+ defs->optional(p.memoryName, prefix + "MemoryName", "Name of this memory server.");
+
return defs;
}
@@ -50,17 +67,27 @@ namespace armarx
void SkillsMemory::onInitComponent()
{
- workingMemory.name() = memoryName;
+ workingMemory.name() = p.memoryName;
+
+ {
+ armarx::armem::wm::CoreSegment& c = workingMemory.addCoreSegment(p.statechartCoreSegmentName, armarx::armem::arondto::Statechart::Transition::toInitialAronType());
+ c.addProviderSegment("Transitions", armarx::armem::arondto::Statechart::Transition::toInitialAronType());
+ }
}
void SkillsMemory::onConnectComponent()
{
+ statechartListener = createStatechartListener(p.statechartTransitionsTopicName);
}
void SkillsMemory::onDisconnectComponent()
{
+ if (statechartListener)
+ {
+ getArmarXManager()->removeObjectBlocking(statechartListener->getName());
+ }
}
@@ -73,7 +100,7 @@ namespace armarx
// WRITING
armem::data::AddSegmentsResult SkillsMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
+ armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments({input}, p.core.addOnUsage);
return result;
}
@@ -88,4 +115,90 @@ namespace armarx
// READING
// Inherited from Plugin
+
+ IceInternal::Handle<StatechartListener>
+ SkillsMemory::createStatechartListener(const std::string& topicName, const std::string& name)
+ {
+ const std::string name_ = name.empty() ? topicName + ".Listener" : name;
+ ARMARX_DEBUG << "Registering StatechartListener '" << name_ << "' listening to topic '" << topicName << "'.";
+
+ IceInternal::Handle<armarx::StatechartListener> listener = Component::create<armarx::StatechartListener>();
+ listener->setName(name_);
+ listener->setTopicName(topicName);
+
+ // Callback for the transition listener
+ listener->registerCallback(
+ [this](const std::vector<StatechartListener::Transition>& transitions,
+ armarx::StatechartListener & source)
+ {
+ this->reportTransitions(transitions);
+ });
+
+ try
+ {
+ getArmarXManager()->addObject(listener);
+ }
+ catch (const Ice::AlreadyRegisteredException& e)
+ {
+ ARMARX_ERROR << "The name '" << name_ << "' is already used. Please choose another one.\n"
+ << "Reason: " << e.what();
+ getArmarXManager()->removeObjectBlocking(name_);
+ }
+ listener->getObjectScheduler()->waitForObjectState(armarx::ManagedIceObjectState::eManagedIceObjectStarted);
+
+ return listener;
+ }
+
+ void
+ SkillsMemory::reportTransitions(const std::vector<StatechartListener::Transition>& transitions)
+ {
+ for (const StatechartListener::Transition& t : transitions)
+ {
+ const std::string& entityName = getStatechartName(t.targetStateIdentifier);
+ IceUtil::Time transitionTime = IceUtil::Time::microSeconds(t.timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = armem::MemoryID()
+ .withMemoryName(p.memoryName)
+ .withCoreSegmentName(p.statechartCoreSegmentName)
+ .withProviderSegmentName(p.statechartTransitionsProviderSegmentName)
+ .withEntityName(entityName);
+
+ update.timeCreated = transitionTime;
+ armem::arondto::Statechart::Transition data;
+ armem::toAron(data, t);
+ update.instancesData.push_back(data.toAron());
+
+ try
+ {
+ workingMemory.update(update);
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING << e.what();
+ }
+ }
+ }
+
+ std::string SkillsMemory::getStatechartName(std::string stateName)
+ {
+ const std::string delimiter = "->";
+ const int maxLevels = 2;
+
+ size_t pos;
+ int levels = 0;
+ std::string statechartName;
+ while ((pos = stateName.find(delimiter)) != std::string::npos && levels < maxLevels)
+ {
+ if (levels != 0)
+ {
+ statechartName += delimiter;
+ }
+ statechartName += stateName.substr(0, pos);
+ stateName.erase(0, pos + delimiter.length());
+ levels++;
+ }
+
+ return statechartName;
+ }
}
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index 6201ffb07640bf4c75473d0a0e29876f4fdbf265..ebf0b02c48f90e31b5f93321343b12a66a2723c9 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -24,12 +24,14 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/armem_skills/StatechartListener.h>
namespace armarx
@@ -80,7 +82,33 @@ namespace armarx
private:
- std::string memoryName = "SkillsMemory";
- bool addCoreSegmentOnUsage = true;
+ DebugObserverInterfacePrx debugObserver;
+
+ struct Properties
+ {
+ std::string memoryName = "Skills";
+
+ // Statechart transition logging
+ std::string statechartCoreSegmentName = "Statechart";
+ std::string statechartTransitionsProviderSegmentName = "Transitions";
+ std::string statechartTransitionsTopicName = "StateReportingTopic";
+
+ struct CoreSegments
+ {
+ bool addOnUsage = false;
+ };
+ CoreSegments core;
+ };
+ Properties p;
+
+
+
+ IceInternal::Handle<StatechartListener> createStatechartListener(const std::string& topicName,
+ const std::string& name = "");
+ IceInternal::Handle<armarx::StatechartListener> statechartListener;
+ void reportTransitions(const std::vector<StatechartListener::Transition>& transitions);
+
+ // Gets the statechart name from a state name (takes first two levels of the hierarchy)
+ static std::string getStatechartName(std::string stateName);
};
}
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 3833b4444736bc1f78fdf9c34d1c5d6ea4acfc8c..bfe0764272dbee12270de46cfe7cf0b90f851bb0 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -49,6 +49,57 @@ module armarx
long first = 0; ///< First index to get.
long last = -1; ///< Last index to get (inclusive).
};
+
+
+ /**
+ * @brief Get snapshot(s) around timestamp.
+ *
+ * If there is a snapshot which exactly matches the query timestamp,
+ * the behavior is as for the @see Single query.
+ *
+ * However, if there is no matching timestamp, the snapshots before
+ * and after the query timestamp will be returned.
+ *
+ * If #eps is positive, the snapshots must be within the time
+ * range [timestamp - eps, timestamp + eps].
+ * Consequently, the query result can be empty.
+ *
+ * Hint:
+ * This query can be quite useful when interpolating snapshots
+ * is possible.
+ */
+ class TimeApprox extends EntityQuery
+ {
+ long timestamp = -1;
+ long eps = -1;
+ };
+
+ /**
+ * @brief Get the last snapshot before or at the timestamp.
+ *
+ * This query is kind of similar to latest() but with a reference timestamp.
+ *
+ */
+ class BeforeOrAtTime extends EntityQuery
+ {
+ long timestamp;
+ }
+
+ /**
+ * @brief Get the last snapshot before the timestamp or a sequence of
+ * n last elements before the timestamp depending on #maxEntries
+ *
+ * Depending on #maxEntries, the behavior is as follows:
+ * - #maxEntries == 1 => last element before timestamp
+ * - #maxEntries > 1 => n last elements before timestamp
+ * - #maxEntries < 0 => all elements before timestamp
+ */
+ class BeforeTime extends EntityQuery
+ {
+ long timestamp;
+ long maxEntries = 1;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index ebcf8c9766a5ca1d8204a1aac7c0f317c2173371..8e86d96dc398d707ab4e8629e4b971519db8dc65 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,8 +1,12 @@
#include <VirtualRobot/XML/ObjectIO.h>
+#include <boost/algorithm/string.hpp>
+
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -43,6 +47,9 @@ namespace armarx
else
{
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
+
+ // make sure this data path is available => e.g. for findArticulatedObjects
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
}
}
}
@@ -149,6 +156,28 @@ namespace armarx
return objects;
}
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjects(bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+
+ const bool local = true;
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
+ {
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
+ {
+ const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
+ objects.insert(objects.end(), dataset.begin(), dataset.end());
+ }
+ }
+ return objects;
+ }
+
std::map<std::string, std::vector<ObjectInfo>>
ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
{
@@ -192,6 +221,95 @@ namespace armarx
return objects;
}
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+
+ const bool local = true;
+
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
+ {
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
+ {
+ const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
+ datasets[datasetDir] = dataset;
+ }
+ }
+ return datasets;
+ }
+
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+ path datasetDir = _rootDirAbs() / dataset;
+ if (!fs::is_directory(datasetDir))
+ {
+ ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
+ << datasetDir;
+ return {};
+ }
+
+ const std::vector<std::string> validExtensions{".urdf", ".xml"};
+
+ const auto hasValidExtension = [&](const std::string & file) -> bool
+ {
+ return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
+ };
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
+ const bool local = true;
+ for (const path& dir : simox::fs::list_directory(datasetDir, local))
+ {
+ if (not fs::is_directory(datasetDir / dir))
+ {
+ continue;
+ }
+
+ for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ {
+ const std::string xml = std::filesystem::path(file).string();
+
+ if (hasValidExtension(xml))
+ {
+ try
+ {
+ const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ if (robot != nullptr && robot->isPassive())
+ {
+ const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
+
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
+ {
+ .name = robot->getName(),
+ .xml = {packageName, relativeXMLPath}
+ // .dataset = dataset
+ });
+ }
+ }
+ catch (const armarx::LocalException& ex)
+ {
+ ARMARX_WARNING << ex.what();
+ }
+ catch (...)
+ {
+
+ }
+ }
+ }
+ }
+ return objects;
+ }
+
VirtualRobot::ManipulationObjectPtr
ObjectFinder::loadManipulationObject(const std::optional<ObjectInfo>& ts)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 6e50a9d9cbeb1c82b7b928b2289f4b74799d70c7..2b342f3ac1151d0589bbc4b0212c3aed2fde798d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -1,6 +1,7 @@
#pragma once
#include <filesystem>
+#include <unordered_map>
#include <VirtualRobot/VirtualRobot.h>
@@ -9,6 +10,8 @@
#include "ObjectInfo.h"
#include "ObjectPose.h"
+#include <RobotAPI/libraries/armem_objects/types.h>
+
namespace armarx
{
/**
@@ -46,6 +49,10 @@ namespace armarx
std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjects(bool checkPaths) const;
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const;
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> findAllArticulatedObjectsByDataset(bool checkPaths = true) const;
+
VirtualRobot::ManipulationObjectPtr
static loadManipulationObject(const std::optional<ObjectInfo>& ts);
VirtualRobot::ManipulationObjectPtr
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 53da6a10a507a5a63c5205f22d8a6643bfb867c0..171da9c1a660b8257b2abc3f1b1f05ff6c0deb98 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -1,5 +1,7 @@
#include "ObjectPose.h"
+#include <SimoxUtility/math/pose/invert.h>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
@@ -100,6 +102,26 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+ void ObjectPose::setObjectPoseRobot(
+ const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
+ {
+ this->objectPoseRobot = objectPoseRobot;
+ if (updateObjectPoseGlobal)
+ {
+ this->objectPoseGlobal = robotPose * objectPoseRobot;
+ }
+ }
+
+ void ObjectPose::setObjectPoseGlobal(
+ const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot)
+ {
+ this->objectPoseGlobal = objectPoseGlobal;
+ if (updateObjectPoseRobot)
+ {
+ this->objectPoseRobot = simox::math::inverted_pose(robotPose) * objectPoseGlobal;
+ }
+ }
+
std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 556ac04be8c43a686e9d8f0e839fdb5bb13f7daa..023d6351f3bcf2533188e0df197b7f8e61806388 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -39,6 +39,9 @@ namespace armarx::objpose
void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
+ void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
+ void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
+
/// Name of the providing component.
std::string providerName;
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 1778ba42d2c67c1d6967485ee5fbcaa05241488a..5df6b028f0272c51a0538392375c27421d19dc06 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -20,8 +20,10 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
-add_subdirectory(armem_robot_localization)
+add_subdirectory(armem_robot)
+add_subdirectory(armem_robot_state)
add_subdirectory(armem_robot_mapping)
+add_subdirectory(armem_skills)
add_subdirectory(aron)
add_subdirectory(NJointControllerGuiPluginUtility)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 6753e0a6cbf4e95f5111bc22d7dce0faec279a31..d256ee202c02d8ebf204455afa241731557815c8 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -23,24 +23,23 @@
#include "RobotNameHelper.h"
-#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
-
#include <Eigen/Dense>
+#include <algorithm>
namespace armarx
{
- void RobotNameHelper::writeRobotInfoNode(
- const RobotInfoNodePtr& n,
- std::ostream& str,
- std::size_t indent,
- char indentChar)
+ void RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
+ std::ostream& str,
+ std::size_t indent,
+ char indentChar)
{
const std::string ind(4 * indent, indentChar);
if (!n)
@@ -59,11 +58,12 @@ namespace armarx
return robotInfo;
}
- const std::string RobotNameHelper::LocationLeft = "Left";
+ const std::string RobotNameHelper::LocationLeft = "Left";
const std::string RobotNameHelper::LocationRight = "Right";
- RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
- : root(Node(robotInfo)), robotInfo(robotInfo)
+ RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile) :
+ root(Node(robotInfo)), robotInfo(robotInfo)
{
ARMARX_TRACE;
StatechartProfilePtr p = profile;
@@ -73,7 +73,6 @@ namespace armarx
p = p->getParent();
}
profiles.emplace_back(""); // for matching the default/root
-
}
std::string RobotNameHelper::select(const std::string& path) const
@@ -94,17 +93,50 @@ namespace armarx
return node.value();
}
- RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+ RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile)
{
return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
}
+ RobotNameHelperPtr RobotNameHelper::Create(const std::string& robotXmlFilename,
+ const StatechartProfilePtr& profile)
+ {
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
+ RapidXmlReaderNode robotNode = reader->getRoot("Robot");
+
+ return Create(readRobotInfo(robotNode.first_node("RobotInfo")), profile);
+ }
+
+ RobotInfoNodePtr RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ {
+ const std::string name = infoNode.name();
+ const std::string profile = infoNode.attribute_value_or_default("profile", "");
+ const std::string value = infoNode.attribute_value_or_default("value", "");
+
+ const auto nodes = infoNode.nodes();
+
+ std::vector<RobotInfoNodePtr> children;
+ children.reserve(nodes.size());
+
+ std::transform(nodes.begin(),
+ nodes.end(),
+ std::back_inserter(children),
+ [](const auto & childNode)
+ {
+ return readRobotInfo(childNode);
+ });
+
+ return new RobotInfoNode(name, profile, value, children);
+ }
+
RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side) const
{
return Arm(shared_from_this(), side);
}
- RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
{
return RobotArm(Arm(shared_from_this(), side), robot);
}
@@ -114,11 +146,7 @@ namespace armarx
return rnh;
}
- RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
- : robotInfo(robotInfo)
- {
-
- }
+ RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo) {}
std::string RobotNameHelper::Node::value()
{
@@ -126,7 +154,8 @@ namespace armarx
return robotInfo->value;
}
- RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
+ RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name,
+ const std::vector<std::string>& profiles)
{
ARMARX_TRACE;
if (!isValid())
@@ -168,7 +197,6 @@ namespace armarx
}
}
-
std::string RobotNameHelper::Arm::getSide() const
{
return side;
@@ -232,8 +260,7 @@ namespace armarx
{
ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
auto path = getHandModelPath();
- return ArmarXDataPath::getAbsolutePath(path, path) ?
- path : "";
+ return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
}
std::string RobotNameHelper::Arm::getHandModelPackage() const
@@ -254,16 +281,17 @@ namespace armarx
return select("FullHandCollisionModel");
}
- RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
ARMARX_TRACE;
return RobotArm(*this, robot);
}
- RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side)
- : rnh(rnh), side(side)
+ RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
+ const std::string& side) :
+ rnh(rnh), side(side)
{
-
}
std::string RobotNameHelper::Arm::select(const std::string& path) const
@@ -310,8 +338,9 @@ namespace armarx
{
ARMARX_TRACE;
std::string abs;
- ARMARX_CHECK_EXPRESSION(ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
- return VirtualRobot::TriMeshModel::FromFile(abs);
+ ARMARX_CHECK_EXPRESSION(
+ ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
+ return VirtualRobot::TriMeshModel::FromFile(abs);
}
VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
@@ -341,8 +370,8 @@ namespace armarx
return arm;
}
- RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
- : arm(arm), robot(robot)
+ RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
+ arm(arm), robot(robot)
{
ARMARX_CHECK_NOT_NULL(robot);
}
@@ -351,4 +380,4 @@ namespace armarx
{
return profiles;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index d37ecea9db567faab03640bc9d2ce12815f6f65b..65fbc12c2b037ab7b1f7e9101d651e2592e168aa 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
namespace armarx
@@ -33,6 +34,8 @@ namespace armarx
using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
using StatechartProfilePtr = std::shared_ptr<class StatechartProfile>;
+ class RapidXmlReaderNode;
+
class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
{
public:
@@ -128,8 +131,10 @@ namespace armarx
std::string select(const std::string& path) const;
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+ static RobotNameHelperPtr Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+
RobotNameHelper(RobotNameHelper&&) = default;
RobotNameHelper(const RobotNameHelper&) = default;
RobotNameHelper& operator=(RobotNameHelper&&) = default;
@@ -140,6 +145,9 @@ namespace armarx
const std::vector<std::string>& getProfiles() const;
const RobotInfoNodePtr& getRobotInfoNodePtr() const;
private:
+
+ static RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+
RobotNameHelper& self()
{
return *this;
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index be30284443c02aa0c4a90225855df5b2d753f6aa..eb83670b3cf4633d3fe2ce6bf52e61256a8d7931 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -215,7 +215,7 @@ armarx_enable_aron_file_generation_for_target(
)
-add_library(RobotAPI::libraries::armem ALIAS "${LIB_NAME}")
+add_library(RobotAPI::armem ALIAS "${LIB_NAME}")
# add unit tests
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index e35f9e08d2f6f13eaab6fbbeee010179e02dbf45..7fc432536b62411a912d7829bc40fd82efa5a855 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -27,9 +27,6 @@ namespace armarx::armem::client
ComponentPluginUser();
~ComponentPluginUser() override;
- protected:
-
- void setMemory(server::MemoryInterfacePrx memory);
/**
* @brief Resolve the given memory name, add it as dependency and update the readers and writers.
@@ -40,6 +37,10 @@ namespace armarx::armem::client
virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
using mns::plugins::ClientPluginUserBase::useMemory;
+ protected:
+
+ void setMemory(server::MemoryInterfacePrx memory);
+
};
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index d9cf1e4f39d48bbc6f4aeb1dd39bc448c3cd6cbf..011948cc29860ed26b63dfdf93e59358c8180e08 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -86,7 +86,7 @@ namespace armarx::armem::client
void
Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
{
- for (const auto& [subscription, callback] : callbacks)
+ for (const auto& [subscription, callbacks] : this->callbacks)
{
std::vector<MemoryID> matchingSnapshotIDs;
@@ -100,7 +100,10 @@ namespace armarx::armem::client
if (not matchingSnapshotIDs.empty())
{
- callback(subscription, matchingSnapshotIDs);
+ for (auto& callback : callbacks)
+ {
+ callback(subscription, matchingSnapshotIDs);
+ }
}
}
}
@@ -124,7 +127,15 @@ namespace armarx::armem::client
void
Reader::subscribe(const MemoryID& id, callback callback)
{
- callbacks[id] = callback;
+ callbacks[id].push_back(callback);
+ }
+
+ void Reader::subscribe(const MemoryID& subscriptionID, callback_updated_only callback)
+ {
+ subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ callback(updatedSnapshotIDs);
+ });
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index cf0adbb45b087faf16188cb89a45bd22839dda97..5cbeff84a3b6f979cd39d87b8271ff7e81d0c01b 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -27,8 +27,13 @@ namespace armarx::armem::client
{
using callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using callback_updated_only = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+
template <class CalleeT>
using member_callback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ template <class CalleeT>
+ using member_callback_updated_only = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+
public:
@@ -54,6 +59,7 @@ namespace armarx::armem::client
data::StoreResult readAndStore(const data::StoreInput& input) const;
void subscribe(const MemoryID& subscriptionID, callback callback);
+ void subscribe(const MemoryID& subscriptionID, callback_updated_only callback);
/**
* Subscribe with a class member function:
* @code
@@ -69,6 +75,15 @@ namespace armarx::armem::client
};
subscribe(subscriptionID, cb);
}
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback_updated_only<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
/// Function handling updates from the MemoryListener ice topic.
void updated(const std::vector<MemoryID>& updatedIDs) const;
@@ -84,7 +99,7 @@ namespace armarx::armem::client
private:
- std::unordered_map<MemoryID, callback> callbacks;
+ std::unordered_map<MemoryID, std::vector<callback>> callbacks;
};
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 44f276ac95cc79d867287be68ebdb1b734056849..8c7858bfc6e5c14ca5f756512d000928f14b8502 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -50,16 +50,15 @@ namespace armarx::armem::client
ReaderComponentPluginUser();
~ReaderComponentPluginUser() override;
+ virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
+ using mns::plugins::ClientPluginUserBase::useMemory;
virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
-
protected:
void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
protected:
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 6df32feea8e85f4b2edf2eb43c3ba131a3238f78..0f5f21f808fb646fe8af27192d04c82a5739426e 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -40,16 +40,13 @@ namespace armarx::armem::client
WriterComponentPluginUser();
~WriterComponentPluginUser() override;
- protected:
-
- void setWritingMemory(server::WritingMemoryInterfacePrx memory);
-
virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
using mns::plugins::ClientPluginUserBase::useMemory;
-
protected:
+ void setWritingMemory(server::WritingMemoryInterfacePrx memory);
+
Writer memoryWriter;
private:
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index 42232a707d2e3e1a111662f97db641ecfca614d0..e7ce598a68d393d6da88a9e4ec52a801d10999c5 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -44,4 +44,60 @@ namespace armarx::armem::client::query
return _addChild(selector);
}
+ void Builder::allInCoreSegment(const MemoryID& coreSegmentID)
+ {
+ coreSegments().withName(coreSegmentID.coreSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().all();
+ }
+
+ void Builder::allLatestInCoreSegment(const MemoryID& coreSegmentID)
+ {
+ coreSegments().withName(coreSegmentID.coreSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ void Builder::allInProviderSegment(const MemoryID& providerSegmentID)
+ {
+ coreSegments().withName(providerSegmentID.coreSegmentName)
+ .providerSegments().withName(providerSegmentID.providerSegmentName)
+ .entities().all()
+ .snapshots().all();
+ }
+
+ void Builder::allLatestInProviderSegment(const MemoryID& providerSegmentID)
+ {
+ coreSegments().withName(providerSegmentID.coreSegmentName)
+ .providerSegments().withName(providerSegmentID.providerSegmentName)
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ void Builder::allEntitySnapshots(const MemoryID& entityID)
+ {
+ coreSegments().withName(entityID.coreSegmentName)
+ .providerSegments().withName(entityID.providerSegmentName)
+ .entities().withName(entityID.entityName)
+ .snapshots().all();
+ }
+
+ void Builder::latestEntitySnapshot(const MemoryID& entityID)
+ {
+ coreSegments().withName(entityID.coreSegmentName)
+ .providerSegments().withName(entityID.providerSegmentName)
+ .entities().withName(entityID.entityName)
+ .snapshots().latest();
+ }
+
+ void Builder::singleEntitySnapshot(const MemoryID& snapshotID)
+ {
+ coreSegments().withName(snapshotID.coreSegmentName)
+ .providerSegments().withName(snapshotID.providerSegmentName)
+ .entities().withName(snapshotID.entityName)
+ .snapshots().atTime(snapshotID.timestamp);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 1741fbdb60d46aecacd217f07da414e2f3b7538d..7f4c2b7c63440f8182d2e489e74ea33ab98e8317 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -41,6 +41,24 @@ namespace armarx::armem::client::query
}
+ // Short hands for common queries
+
+ /// Get all snapshots from all entities in all provider segments in a core segment.
+ void allInCoreSegment(const MemoryID& coreSegmentID);
+ /// Get latest snapshots from all entities in all provider segments in a core segment.
+ void allLatestInCoreSegment(const MemoryID& coreSegmentID);
+
+ /// Get all snapshots from all entities in a provider segment.
+ void allInProviderSegment(const MemoryID& providerSegmentID);
+ /// Get latest snapshots from all entities in a provider segment.
+ void allLatestInProviderSegment(const MemoryID& providerSegmentID);
+
+ void allEntitySnapshots(const MemoryID& entityID);
+ void latestEntitySnapshot(const MemoryID& entityID);
+
+ void singleEntitySnapshot(const MemoryID& snapshotID);
+
+
QueryInput buildQueryInput() const;
armem::query::data::Input buildQueryInputIce() const;
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 8dcb88991450b95583c7228b2349fdfdf05cba95..35203a014fa423cec4f8ef0f4d09759bc54385a9 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/source/RobotAPI/libraries/armem/client/query/query_fns.h
@@ -134,4 +134,34 @@ namespace armarx::armem::client::query_fns
};
}
-}
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ atTimeApprox(Time time, Duration eps)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.atTimeApprox(time, eps);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeOrAtTime(Time time)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeOrAtTime(time);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeTime(Time time, long nElements = 1)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeTime(time, nElements);
+ };
+ }
+
+} // namespace armarx::armem::client::query_fns
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index f2bae712818b566ef51444aafb84af6031d338d0..cc3a73829ab7c79eabc96c30b55c4ca350ac33ad 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,4 +1,5 @@
#include "selectors.h"
+#include "RobotAPI/libraries/armem/core/ice_conversions.h"
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
@@ -42,6 +43,15 @@ namespace armarx::armem::client::query
return *this;
}
+ SnapshotSelector& SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
+ {
+ auto& q = _addQuery<dq::entity::TimeApprox>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.eps, eps);
+ return *this;
+ }
+
+
SnapshotSelector& SnapshotSelector::indexRange(long first, long last)
{
auto& q = _addQuery<dq::entity::IndexRange>();
@@ -51,6 +61,22 @@ namespace armarx::armem::client::query
}
+ SnapshotSelector& SnapshotSelector::beforeOrAtTime(Time timestamp)
+ {
+ auto& q = _addQuery<dq::entity::BeforeOrAtTime>();
+ toIce(q.timestamp, timestamp);
+ return *this;
+ }
+
+
+ SnapshotSelector& SnapshotSelector::beforeTime(Time timestamp, long maxEntries)
+ {
+ auto& q = _addQuery<dq::entity::BeforeTime>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.maxEntries, maxEntries);
+ return *this;
+ }
+
SnapshotSelector& EntitySelector::snapshots()
{
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index 8bc2850b390b394a5d2b5fc07753bb846f598416..df642ded9a70ea2688eaca5865f730e8e99ee6de 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.h
@@ -26,6 +26,10 @@ namespace armarx::armem::client::query
SnapshotSelector& latest();
SnapshotSelector& atTime(Time timestamp);
+ SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
+
+ SnapshotSelector& beforeTime(Time timestamp, long maxEntries = 1);
+ SnapshotSelector& beforeOrAtTime(Time timestamp);
SnapshotSelector& timeRange(Time min, Time max);
SnapshotSelector& indexRange(long first, long last);
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index ea04bb93945341c9dd8b433a7004a478f50581be..5c1946ed0f55591457c7ce7fcf7a8a5b9a325ee4 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -75,4 +75,15 @@ namespace armarx::armem
}, results);
}
+
+ EntityUpdate& Commit::add()
+ {
+ return updates.emplace_back();
+ }
+
+ EntityUpdate& Commit::add(const EntityUpdate& update)
+ {
+ return updates.emplace_back(update);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index ff3215ab7ea6d01de892eec9f99ec911e6e667b8..48707600ab242ad3a624c6e26965acdef18839fa 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -85,6 +85,9 @@ namespace armarx::armem
*/
std::vector<EntityUpdate> updates;
+ EntityUpdate& add();
+ EntityUpdate& add(const EntityUpdate& update);
+
friend std::ostream& operator<<(std::ostream& os, const Commit& rhs);
};
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 09415ec4abd73185f77050c89683b326f3c3e9d0..7ddf2130801767eabb60c01caba1d2dc427caba3 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -90,19 +90,19 @@ namespace armarx::armem
bool hasMemoryName() const
{
- return memoryName.size() > 0;
+ return !memoryName.empty();
}
bool hasCoreSegmentName() const
{
- return coreSegmentName.size() > 0;
+ return !coreSegmentName.empty();
}
bool hasProviderSegmentName() const
{
- return providerSegmentName.size() > 0;
+ return !providerSegmentName.empty();
}
bool hasEntityName() const
{
- return entityName.size() > 0;
+ return !entityName.empty();
}
bool hasTimestamp() const
{
@@ -254,7 +254,7 @@ namespace armarx::armem
*/
bool contains(const MemoryID& general, const MemoryID& specific);
-}
+} // namespace armarx::armem
namespace std
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index 328d1f59d793cbcc9a7d0febc0951775d200013a..f76f7657dda6e14a9c35bb4ad5edf58471680144 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -7,8 +7,8 @@
namespace armarx::armem
{
-
using Time = IceUtil::Time;
+ using Duration = IceUtil::Time;
/**
* @brief Returns `time` as e.g. "123456789.012 ms".
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index f5e6d3e0d10cb7b3f1ed51bcc8f665cb48ec3cbf..ed2197e66fc41d424cc793980a771684e12d3de1 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -20,3 +20,10 @@ void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
dto.timestamp = bo.timestamp.toMicroSeconds();
dto.instanceIndex = bo.instanceIndex;
}
+
+std::ostream& armarx::armem::arondto::operator<<(std::ostream& os, const MemoryID& rhs)
+{
+ armem::MemoryID bo;
+ fromAron(rhs, bo);
+ return os << bo;
+}
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
index 5d105fd9761f3ff1d8eaa58fabf4aa257719478b..8688c3b3fdf5dcb2ce9684960ab3a98449c8c13a 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -9,3 +9,7 @@ namespace armarx::armem
void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
void toAron(arondto::MemoryID& dto, const MemoryID& bo);
}
+namespace armarx::armem::arondto
+{
+ std::ostream& operator<<(std::ostream& os, const MemoryID& rhs);
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 6ea66c74b5216c9e4a92c4e719e01193e39b5ae1..ead811e5a744b9208a30eb7761c381b040cd7e90 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -140,7 +140,16 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Add the missing provider segment (with this core segment's type).
+ ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
+ return provSeg.update(update);
+ }
+ else
+ {
+ throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ }
}
}
@@ -230,13 +239,20 @@ namespace armarx::armem::base
{
Base::_copySelf(other);
other.aronType() = _aronType;
+ other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
}
virtual void _copySelfEmpty(DerivedT& other) const override
{
Base::_copySelfEmpty(other);
other.aronType() = _aronType;
+ other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
}
+
+ private:
+
+ bool _addMissingProviderSegmentDuringUpdate = true;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 53b398a284bc8ebb1151d5266f81ec7a43010cea..713ba580089443b9fc2ca6f982e72970aa28dde7 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -108,7 +108,7 @@ namespace armarx::armem::base
this->_checkContainerName(id.memoryName, this->name());
if (id.hasCoreSegmentName())
{
- return getCoreSegment(id.providerSegmentName).findEntity(id);
+ return getCoreSegment(id.coreSegmentName).findEntity(id);
}
else
{
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
index 986612d1e788bee74e572f20b89dbc78fb99ea58..ede63a494a7aff26c54ba1d7149200c91be7d987 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
@@ -19,62 +19,82 @@ namespace armarx::armem::wm
bool Visitor::applyTo(Memory& memory)
{
+ bool cont = true;
+ visitEnter(memory);
for (auto& [_, coreSeg] : memory)
{
if (!applyTo(coreSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(memory);
+ return cont;
}
bool Visitor::applyTo(CoreSegment& coreSegment)
{
+ bool cont = true;
+ visitEnter(coreSegment);
for (auto& [_, provSeg] : coreSegment)
{
if (!applyTo(provSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(coreSegment);
+ return cont;
}
bool Visitor::applyTo(ProviderSegment& providerSegment)
{
+ bool cont = true;
+ visitEnter(providerSegment);
for (auto& [_, entity] : providerSegment)
{
if (!applyTo(entity))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(providerSegment);
+ return cont;
}
bool Visitor::applyTo(Entity& entity)
{
+ bool cont = true;
+ visitEnter(entity);
for (auto& [_, snapshot] : entity)
{
if (!applyTo(snapshot))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(entity);
+ return cont;
}
bool Visitor::applyTo(EntitySnapshot& snapshot)
{
+ bool cont = true;
+ visitEnter(snapshot);
for (auto& instance : snapshot)
{
if (!applyTo(instance))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(snapshot);
+ return cont;
}
bool Visitor::applyTo(EntityInstance& instance)
@@ -85,62 +105,82 @@ namespace armarx::armem::wm
bool Visitor::applyTo(const Memory& memory)
{
+ bool cont = true;
+ visitEnter(memory);
for (const auto& [_, coreSeg] : memory)
{
if (!applyTo(coreSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(memory);
+ return cont;
}
bool Visitor::applyTo(const CoreSegment& coreSegment)
{
+ bool cont = true;
+ visitEnter(coreSegment);
for (const auto& [_, provSeg] : coreSegment)
{
if (!applyTo(provSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(coreSegment);
+ return cont;
}
bool Visitor::applyTo(const ProviderSegment& providerSegment)
{
+ bool cont = true;
+ visitEnter(providerSegment);
for (const auto& [_, entity] : providerSegment)
{
if (!applyTo(entity))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(providerSegment);
+ return cont;
}
bool Visitor::applyTo(const Entity& entity)
{
+ bool cont = true;
+ visitEnter(entity);
for (const auto& [_, snapshot] : entity)
{
if (!applyTo(snapshot))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(entity);
+ return cont;
}
bool Visitor::applyTo(const EntitySnapshot& snapshot)
{
+ bool cont = true;
+ visitEnter(snapshot);
for (const auto& instance : snapshot)
{
if (!applyTo(instance))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(snapshot);
+ return cont;
}
bool Visitor::applyTo(const EntityInstance& instance)
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
index 7dded894331809986a991351f0eb7c7675b11812..53378a6976889f4169be8ea35d4f8560df9bf96f 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ice_conversions.cpp
@@ -74,6 +74,7 @@ namespace armarx::armem
{
ice.aronType = providerSegment.aronType()->getResult();
}
+ ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
toIce(ice.entities, providerSegment.entities());
}
void fromIce(const data::ProviderSegment& ice, wm::ProviderSegment& providerSegment)
@@ -84,6 +85,7 @@ namespace armarx::armem
{
providerSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
}
+ ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
fromIce(ice.entities, providerSegment.entities());
}
@@ -95,6 +97,7 @@ namespace armarx::armem
{
ice.aronType = coreSegment.aronType()->getResult();
}
+ ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
toIce(ice.providerSegments, coreSegment.providerSegments());
}
void fromIce(const data::CoreSegment& ice, wm::CoreSegment& coreSegment)
@@ -105,6 +108,7 @@ namespace armarx::armem
{
coreSegment.aronType() = aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice.aronType));
}
+ ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
fromIce(ice.providerSegments, coreSegment.providerSegments());
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 6895cf4e5b27ecec5872a38890d8111b41405fa7..c1ae33fbe624fe2558c8f2dfb11d2ae380fb192a 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -99,9 +99,30 @@ namespace armarx::armem::server
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
ARMARX_CHECK_NOT_NULL(workingMemory);
+ auto handleException = [](const std::string & what)
+ {
+ data::CommitResult result;
+ data::EntityUpdateResult& r = result.results.emplace_back();
+ r.success = false;
+ r.errorMessage = what;
+ return result;
+ };
armem::Commit commit;
- armem::fromIce(commitIce, commit, timeArrived);
+ try
+ {
+ armem::fromIce(commitIce, commit, timeArrived);
+ }
+ catch (const aron::error::AronNotValidException& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
+ catch (const Ice::Exception& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
armem::CommitResult result = this->commit(commit);
data::CommitResult resultIce;
@@ -147,6 +168,16 @@ namespace armarx::armem::server
result.success = false;
result.errorMessage = e.what();
}
+ catch (const aron::error::AronException& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ catch (const Ice::Exception& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
}
if (publishUpdates)
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 447bd259a2c24a3ca80c91c59a518daed56c18f1..2a4308ebdc837255a9f6a816dc62880dc09e8c21 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,15 +1,20 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
+#include <cstdint>
+#include <iterator>
-#include "BaseQueryProcessorBase.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <ArmarXCore/core/exceptions/LocalException.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>
+#include "BaseQueryProcessorBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+
namespace armarx::armem::base::query_proc
{
@@ -48,6 +53,18 @@ namespace armarx::armem::base::query_proc
{
process(result, *q, entity);
}
+ else if (auto q = dynamic_cast<const armem::query::data::entity::TimeApprox*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
else
{
throw armem::error::UnknownQueryType("entity snapshot", query);
@@ -153,6 +170,197 @@ namespace armarx::armem::base::query_proc
}
+ inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // now either refItFwd is a perfect match ...
+ if (refItFwd->first == timestamp)
+ {
+ return refItFwd;
+ }
+
+ // ... or we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+ inline auto lastElementBefore(const auto& history, const auto& timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeOrAtTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+
+ if (beforeOrAt != entity.history().end())
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+ }
+
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
+
+ const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
+ if (itBefore == entity.history().end())
+ {
+ ARMARX_WARNING << "No suitable element found";
+ return;
+ }
+
+ const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
+
+ const int nEntries = [&]()
+ {
+ // see query.ice
+ if (maxEntries > 0)
+ {
+ return std::min(nEntriesBefore, maxEntries);
+ }
+
+ return nEntriesBefore; // all elements before timestamp
+ }
+ ();
+
+ auto it = itBefore;
+ for (std::int64_t i = 0; i < nEntries; i++, --it)
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::TimeApprox& query,
+ const _EntityT& entity) const
+ {
+
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+ const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+
+ // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
+ const auto isInRange = [&](const Time & t) -> bool
+ {
+ if (epsDuration <= 0)
+ {
+ return true;
+ }
+
+ return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
+ };
+
+ // first element before or at timestamp
+ const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+ const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
+ const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
+
+ // first element greater
+ const auto after = entity.history().upper_bound(referenceTimestamp);
+ const bool isAfterValid = after != entity.history().end();
+
+ // if both are invalid, there is nothing to be gained here.
+ if ((not isBeforeOrAtValid) and (not isAfterValid))
+ {
+ const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
+
+ ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
+ return;
+ }
+ // -> now one or both are valid ...
+
+ // is 'before' a perfect match?
+ if(isPerfectMatch)
+ {
+ addResultSnapshot(result, beforeOrAt);
+ return;
+ }
+
+ // only 'before' valid?
+ if (not isAfterValid)
+ {
+ if (isInRange(beforeOrAt->first))
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+ return;
+ }
+
+ // only 'after' valid?
+ if (not isBeforeOrAtValid)
+ {
+ if (isInRange(after->first))
+ {
+ addResultSnapshot(result, after);
+ }
+ return;
+ }
+ // -> now both are valid
+
+ // return both (if in range) => user can interpolate
+ if (isInRange(beforeOrAt->first))
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+
+ if (isInRange(after->first))
+ {
+ addResultSnapshot(result, after);
+ }
+
+ }
+
+
static size_t negativeIndexSemantics(long index, size_t size)
{
const size_t max = size > 0 ? size - 1 : 0;
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 42d357b005ae9c4c4663bc72fa91ed4fc459b78a..789600c2fe82857b865c845d6892fc2da44d3d6a 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -20,6 +20,9 @@
* GNU General Public License
*/
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <boost/test/tools/old/interface.hpp>
#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
#define ARMARX_BOOST_TEST
@@ -294,6 +297,308 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
}
+/* BeforeTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 3500, 1 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 3500, 2});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
+
+/* BeforeOrAtTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 3500 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 3000 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 1 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/* TimeApprox */
+
+/**
+ * @brief Lookup between elements. No range specified.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is OK.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 600});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is too small.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 100});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 0);
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Only next element is in range.
+ *
+ * Desired behavior: Return only element after query timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3700, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Lookup between elements. Only previous element is in range.
+ *
+ * Desired behavior: Return only element before query timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3300, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Lookup with perfect match.
+ *
+ * Desired behavior: Return only element matching timestamp exactly.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3000, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Invalid lookup into the past.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 1, 1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/**
+ * @brief Invalid lookup into the future.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 10'000, 1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/**
+ * @brief Lookup into the future, but still considered valid as time range is not set.
+ *
+ * Desired behavior: Return most recent element in history.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 10'000, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(5'000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
+{
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+}
+
+
+
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 0), 0);
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index f7335ec9498212fdf10fce0f4106cb12aced2c61..16da9f1709528dc7b9c7d484be494a2f1e6da851 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -13,7 +13,6 @@
* 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 RobotComponents::ArmarXObjects::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -135,7 +134,7 @@ namespace armarx::armem
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.history().size();
+ ARMARX_DEBUG << "History size: " << entity.history().size();
for (const auto &[ss, entitySnapshot] : entity.history())
{
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index a410033a4dcdda6b0e17f0de7ab35e4f9a044f12..4dc890e600b5419db408d3110455a19fd96f2e68 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -34,7 +34,7 @@ namespace armarx::armem::gui::memory
header()->setMinimumSectionSize(25);
header()->resizeSection(int(Columns::KEY), 250);
- header()->resizeSection(int(Columns::SIZE), 25);
+ header()->resizeSection(int(Columns::SIZE), 30);
header()->setTextElideMode(Qt::TextElideMode::ElideRight);
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index a26840ac78305a9afcf6ab55498d5bdf45d9c017..8c0f625f022e3efeb7cf7763841a25e6469b6ac8 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -14,7 +14,8 @@ armarx_add_library(
RobotAPI::ArViz
RobotAPI::ComponentPlugins
RobotAPI::Core
- RobotAPI::libraries::armem
+ RobotAPI::armem
+ RobotAPI::armem_robot
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -27,6 +28,21 @@ armarx_add_library(
server/instance/Decay.h
server/instance/RobotHeadMovement.h
server/instance/Visu.h
+
+ server/articulated_object_class/Segment.h
+ server/articulated_object_instance/Segment.h
+ # server/articulated_object/SegmentAdapter.h
+ server/articulated_object_instance/Visu.h
+
+ server/attachments/Segment.h
+
+ client/articulated_object/Reader.h
+ client/articulated_object/Writer.h
+ client/articulated_object/interfaces.h
+
+ client/attachment/Reader.h
+ client/attachment/Writer.h
+
SOURCES
aron_conversions.cpp
@@ -38,14 +54,34 @@ armarx_add_library(
server/instance/Decay.cpp
server/instance/RobotHeadMovement.cpp
server/instance/Visu.cpp
+
+ server/articulated_object_class/Segment.cpp
+
+ server/articulated_object_instance/Segment.cpp
+ # server/articulated_object/SegmentAdapter.cpp
+ server/articulated_object_instance/Visu.cpp
+
+ server/attachments/Segment.cpp
+
+ client/articulated_object/Reader.cpp
+ client/articulated_object/Writer.cpp
+
+ client/attachment/Reader.cpp
+ client/attachment/Writer.cpp
+
)
+
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
aron/ObjectClass.xml
aron/ObjectInstance.xml
+
+ aron/Attachment.xml
+ # aron/Constraint.xml
)
add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml b/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml
new file mode 100644
index 0000000000000000000000000000000000000000..76cb21c07a16e8f37c0fa00f6a4ab2b90cfd2983
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml
@@ -0,0 +1,74 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::attachment::AgentDescription">
+
+ <!-- Memory link pointing to arondto::Robot or arondto::ArticulatedObject -->
+ <ObjectChild key="id">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <!-- one of the robot's frames -->
+ <ObjectChild key="frame">
+ <string/>
+ </ObjectChild>
+ </Object>
+
+ <!-- Fixed transformation between agent and object -->
+ <Object name="armarx::armem::arondto::attachment::ObjectAttachment">
+ <ObjectChild key="agent">
+ <armarx::armem::arondto::attachment::AgentDescription />
+ </ObjectChild>
+
+ <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild>
+
+ <ObjectChild key="object">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key="active">
+ <Bool/>
+ </ObjectChild>
+
+ <ObjectChild key="timestamp">
+ <Time/>
+ </ObjectChild>
+ </Object>
+
+ <!-- Fixed transformation between agent and articulated object -->
+ <Object name="armarx::armem::arondto::attachment::ArticulatedObjectAttachment">
+
+ <ObjectChild key="agent">
+ <armarx::armem::arondto::attachment::AgentDescription />
+ </ObjectChild>
+
+ <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild>
+
+ <ObjectChild key="object">
+ <armarx::armem::arondto::attachment::AgentDescription />
+ </ObjectChild>
+
+ <ObjectChild key="active">
+ <Bool/>
+ </ObjectChild>
+
+ <ObjectChild key="timestamp">
+ <Time/>
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Constraint.xml b/source/RobotAPI/libraries/armem_objects/aron/Constraint.xml
new file mode 100644
index 0000000000000000000000000000000000000000..534df98a19ce5caa5ed59c6601993c2d598017d9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/Constraint.xml
@@ -0,0 +1,93 @@
+<!-- This class defines a link between two scene entities, e.g. robots, articulated objects or objects -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </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/PackagePath.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::RobotID">
+ <ObjectChild key="name">
+ <string/>
+ </ObjectChild>
+ <ObjectChild key="instance">
+ <string/>
+ </ObjectChild>
+ </Object>
+
+ <IntEnum name="armarx::armem::arondto::SceneEntityType">
+ <EnumValue key="Robot" value="0" />
+ <EnumValue key="ArticulatedObject" value="1" />
+ <EnumValue key="Object" value="2" />
+ </IntEnum>
+
+ <Object name="armarx::armem::arondto::SceneEntity">
+ <ObjectChild key="type">
+ <armarx::armem::arondto::SceneEntityType />
+ </ObjectChild>
+
+ <!-- could be a variant -->
+ <!-- <ObjectChild key="objectId">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+ <ObjectChild key="robotId">
+ <armarx::arondto::RobotID />
+ </ObjectChild> -->
+ </Object>
+
+ <Object name="armarx::armem::arondto::BodyLinkDescription">
+ <ObjectChild key="entity">
+ <armarx::armem::arondto::SceneEntity />
+ </ObjectChild>
+
+ <ObjectChild key="node">
+ <string/>
+ </ObjectChild>
+
+ <ObjectChild key="offset">
+ <Pose/>
+ </ObjectChild>
+ </Object>
+
+ <!-- http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual -->
+ <IntEnum name="armarx::armem::arondto::ConstraintType">
+ <EnumValue key="Fixed" value="0" />
+ <!-- <EnumValue key="Prismatic" value="1" /> -->
+ <!-- <EnumValue key="Revolute" value="2" /> -->
+ <!-- <EnumValue key="PointToPoint" value="3" /> -->
+ <!-- <EnumValue key="ConeTwist" value="4" /> -->
+ <!-- <EnumValue key="Contact" value="6" /> -->
+ <!-- <EnumValue key="Gear" value="7" /> -->
+ <!-- <EnumValue key="Spring" value="7" /> -->
+ </IntEnum>
+
+ <Object name="armarx::armem::arondto::Constraint">
+
+ <ObjectChild key="type">
+ <armarx::armem::arondto::ConstraintType />
+ </ObjectChild>
+
+ <!-- <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild> -->
+
+
+ <ObjectChild key="bodyA">
+ <armarx::armem::arondto::BodyLinkDescription/>
+ </ObjectChild>
+
+ <ObjectChild key="bodyB">
+ <armarx::armem::arondto::BodyLinkDescription/>
+ </ObjectChild>
+
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 859f3d601024ba97dd0e59e938afaba2718a95a1..8602178224b696fdcf026f50184628ee7a870dae 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -2,26 +2,95 @@
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
+namespace armarx::armem
{
- bo = dto.pose;
-}
-void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
-{
- dto.pose = bo;
-}
+ void fromAron(const arondto::ObjectInstance& dto,
+ objpose::arondto::ObjectPose& bo)
+ {
+ bo = dto.pose;
+ }
+ void 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 fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+ {
+ objpose::fromAron(dto.pose, bo);
+ }
+
+ void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+ {
+ objpose::toAron(dto.pose, bo);
+ }
+
+
+ /* Attachments */
+ void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+ {
+ fromAron(dto.id, bo.id);
+ aron::fromAron(dto.frame, bo.frame);
+ }
+
+ void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
+ {
+ toAron(dto.id, bo.id);
+ aron::toAron(dto.frame, bo.frame);
+ }
-void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
-{
- objpose::toAron(dto.pose, bo);
-}
+ void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
+ {
+ fromAron(dto.agent, bo.agent);
+ aron::fromAron(dto.transformation, bo.transformation);
+ fromAron(dto.object, bo.object);
+ aron::fromAron(dto.active, bo.active);
+ // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+ }
+ void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+ {
+ toAron(dto.agent, bo.agent);
+ aron::toAron(dto.transformation, bo.transformation);
+ toAron(dto.object, bo.object);
+ aron::toAron(dto.active, bo.active);
+ // TODO aron::toAron(dto.timestamp, bo.timestamp);
+ }
+
+
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo)
+ {
+ fromAron(dto.agent, bo.agent);
+ aron::fromAron(dto.transformation, bo.transformation);
+ fromAron(dto.object, bo.object);
+ aron::fromAron(dto.active, bo.active);
+ // TODO aron::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo)
+ {
+ toAron(dto.agent, bo.agent);
+ aron::toAron(dto.transformation, bo.transformation);
+ toAron(dto.object, bo.object);
+ aron::toAron(dto.active, bo.active);
+ // TODO aron::toAron(dto.timestamp, bo.timestamp);
+ }
+
+
+
+
+} // namespace armarx::armem
+
+armarx::armem::MemoryID
+armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
+{
+ return MemoryID("Object/Instance")
+ .withProviderSegmentName(objectPose.providerName)
+ .withEntityName(objectPose.objectID.str())
+ .withTimestamp(objectPose.timestamp);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 7ea733e99c75895ff7aba463f36a0f53d0c4bd38..61e89c37ec00aaabed94e1b20a2ba1b50be976a8 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -3,7 +3,9 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
{
@@ -12,4 +14,25 @@ namespace armarx::armem
void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+
+
+ /* Attachments */
+ void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+ void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
+
+ void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+ void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
+
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+
+} // namespace armarx::armem
+
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::obj
+{
+ /// Make a Memory ID for the object instance snapshot representing this pose.
+ MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d11b7421313e4586e4347b31c9f8c79c09f169f5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -0,0 +1,324 @@
+#include "Reader.h"
+
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/PackagePath.h>
+
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::articulated_object
+{
+
+ Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreInstanceSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ def->optional(properties.coreClassSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object classes.");
+ def->optional(properties.providerName, prefix + "ProviderName");
+ }
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+
+ armem::MemoryID id = armem::MemoryID();
+ id.memoryName = properties.memoryName;
+ id.coreSegmentName = properties.coreClassSegmentName;
+ // listen to all provider segments!
+
+ memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+ }
+
+ void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+ {
+ // const std::string& nameWithDataset = snapshotId.providerSegmentName;
+
+ // arondto::RobotDescription aronArticulatedObjectDescription;
+ // aronArticulatedObjectDescription.fromAron(snapshotId.);
+
+ // TODO(fabian.reister): implement
+ }
+
+ void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ ARMARX_INFO << "New objects available!";
+
+ // // Query all entities from provider.
+ // armem::client::query::Builder qb;
+
+ // // clang-format off
+ // qb
+ // .coreSegments().withName(properties.coreClassSegmentName)
+ // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ // .entities().withName(name)
+ // .snapshots().atTime(timestamp);
+ // // clang-format on
+
+ // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
+ // {
+ // updateKnownObject(snapshotID);
+ // });
+ }
+
+ std::optional<ArticulatedObject> Reader::get(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Unknown object " << name;
+ return std::nullopt;
+ }
+
+ return get(*description, timestamp);
+ }
+
+ ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp)
+ {
+ ArticulatedObject obj{.description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
+
+ synchronize(obj, timestamp);
+
+ return obj;
+ }
+
+ void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
+ {
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return;
+ }
+
+ obj.config = std::move(*state);
+ }
+
+ std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
+ std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ .entities().all() // withName(name)
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotDescription(qResult.memory);
+ }
+
+ std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreInstanceSegmentName)
+ .providerSegments().all() // withName(properties.providerName) // agent
+ .entities().withName(description.name)
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotState(qResult.memory);
+ }
+
+ std::optional<robot::RobotState>
+ Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ {
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.coreInstanceSegmentName);
+ // clang-format on
+
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ {
+
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ continue;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotState(instance);
+ }
+
+ return std::nullopt;
+ }
+
+ std::optional<robot::RobotDescription>
+ Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ {
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.coreClassSegmentName);
+ // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ // clang-format on
+
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ {
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ // return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ continue;
+ // return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
+ }
+
+ return std::nullopt;
+ }
+
+ std::vector<robot::RobotDescription>
+ Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::vector<robot::RobotDescription> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto &[name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ descriptions.push_back(*robotDescription);
+ }
+ }
+ }
+
+ return descriptions;
+ }
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..21a4591cd2f1f5c9a125177e50285b3802f561b9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -0,0 +1,82 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+#include "interfaces.h"
+
+namespace armarx::armem::articulated_object
+{
+ class Reader:
+ virtual public ReaderInterface
+ {
+ public:
+ Reader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
+
+ std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
+ ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
+
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+
+ // TODO(fabian.reister): register property defs
+
+ protected:
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
+ private:
+ void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObject(const armem::MemoryID& snapshotId);
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
+ std::string coreClassSegmentName = "ArticulatedObjectClass";
+ std::string providerName = "ArmarXObjects";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.articulated.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& component;
+ };
+
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9062747de124ac3db5ba37e3f14f2b8cdfc7b692
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -0,0 +1,326 @@
+#include "Writer.h"
+
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+
+
+namespace armarx::armem::articulated_object
+{
+ Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+
+ void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreInstanceSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ def->optional(properties.coreClassSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object classes.");
+ def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
+ }
+
+ void Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
+
+ memoryWriter.setWritingMemory(result.proxy);
+ memoryReader.setReadingMemory(result.proxy);
+
+ const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+
+ const auto resultCoreInstanceSegmentName =
+ memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+ armem::MemoryID refId = armem::MemoryID(resultCoreClassSegment.segmentID);
+
+ armem::MemoryID id;
+ id.setCoreSegmentID(refId); // listen to all provider segments!
+
+ updateKnownObjects();
+ memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
+ }
+
+ void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+ {
+
+ arondto::RobotDescription aronArticulatedObjectDescription;
+ // aronArticulatedObjectDescription.fromAron(snapshotId.ent);
+
+ // TODO(fabian.reister): implement
+ }
+
+ void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ ARMARX_INFO << "New objects available!";
+ updateKnownObjects();
+ }
+
+ void Writer::updateKnownObjects()
+ {
+ knownObjects = queryDescriptions(IceUtil::Time::now());
+ }
+
+ std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+ {
+ const auto objectId = knownObjects.find(obj.description.name);
+
+ // check if exists
+ if (objectId != knownObjects.end())
+ {
+ return objectId->second;
+ }
+
+ // otherwise create
+ if (properties.allowClassCreation)
+ {
+ return storeClass(obj);
+ }
+
+ return std::nullopt;
+ }
+
+ std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ ARMARX_DEBUG << "Trying to create core segment + provider segment";
+
+ // TODO(fabian.reister): variable provider segment
+ const auto result = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ const auto& timestamp = obj.timestamp;
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId
+ .withEntityName(obj.description.name)
+ .withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::RobotDescription aronArticulatedObjectDescription;
+ toAron(aronArticulatedObjectDescription, obj.description);
+
+ update.instancesData = {aronArticulatedObjectDescription.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ return std::nullopt;
+ }
+
+ // update cache (TODO: likely remove this)
+ knownObjects[obj.description.name] = updateResult.snapshotID;
+
+ return updateResult.snapshotID;
+ }
+
+ bool Writer::storeInstance(const ArticulatedObject& obj)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ ARMARX_DEBUG << "Trying to create core segment + provider segment";
+
+ const auto result =
+ memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return false;
+ }
+
+ const auto& timestamp = obj.timestamp;
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId
+ .withEntityName(obj.description.name)
+ .withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::Robot aronArticulatedObject;
+ robot::toAron(aronArticulatedObject, obj);
+
+ const auto descriptionId = storeOrGetClass(obj);
+
+ if (not descriptionId)
+ {
+ ARMARX_WARNING << "Could not get class for object " << obj.description.name;
+ return false;
+ }
+
+ // install memory link
+ toAron(aronArticulatedObject.description, *descriptionId);
+
+ update.instancesData = {aronArticulatedObject.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_WARNING << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ bool Writer::store(const ArticulatedObject& obj)
+ {
+ const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
+
+ if (not classId)
+ {
+ ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
+ << "Known classes are " << simox::alg::get_keys(knownObjects);
+ return false;
+ }
+
+ // TODO(fabian.reister): integrate memory link
+ return storeInstance(obj);
+ }
+
+ // const std::string& Writer::getPropertyPrefix() const
+ // {
+ // return propertyPrefix;
+ // }
+
+
+ // TODO this is a duplicate
+ std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.coreClassSegmentName)
+ .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
+ }
+
+ std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::unordered_map<std::string, armem::MemoryID> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
+ const armem::wm::EntityInstance& instance = sn.getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
+ }
+
+ }
+
+ return descriptions;
+ }
+
+
+ std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2031dd402e546a21a80a17c28f8b2e4107f9059
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -0,0 +1,93 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+#include "RobotAPI/libraries/armem/client.h"
+
+#include "interfaces.h"
+
+
+namespace armarx::armem::articulated_object
+{
+
+ class Writer:
+ virtual public WriterInterface
+ {
+ public:
+ Writer(armem::ClientComponentPluginUser& component);
+ virtual ~Writer() = default;
+
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ bool store(const ArticulatedObject& obj) override;
+
+ bool storeInstance(const ArticulatedObject& obj);
+ std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj);
+
+ // const std::string& getPropertyPrefix() const override;
+
+ private:
+ std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
+
+ void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects();
+ void updateKnownObject(const armem::MemoryID& snapshotId);
+
+ // TODO duplicate
+ std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
+ std::string coreClassSegmentName = "ArticulatedObjectClass";
+ std::string providerName;
+
+ bool allowClassCreation = false;
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.articulated.";
+
+ armem::client::Writer memoryWriter;
+ std::mutex memoryWriterMutex;
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryReaderMutex;
+
+ // key: name of object: RobotDescription::name
+ std::unordered_map<std::string, MemoryID> knownObjects;
+
+ armem::ClientComponentPluginUser& component;
+ };
+
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..ced0a76fd07fe4575ea4762ea062a804431fff2a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -0,0 +1,29 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+namespace armarx::armem::articulated_object
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
+
+ virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) = 0;
+ virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) = 0;
+ };
+
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ virtual bool store(const ArticulatedObject& obj) = 0;
+ };
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e3ce169b9ef41292b7ad565f94b74faf141ee7ad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -0,0 +1,210 @@
+#include "Reader.h"
+
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/PackagePath.h>
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+namespace armarx::armem::attachment
+{
+
+ namespace
+ {
+
+ template<typename AronClass, typename ArmemClass>
+ auto getAttachments(const armarx::armem::wm::Memory& memory)
+ {
+ // using ArmemClass = decltype(fromAron(AronClass()));
+ using ArmemClassVector = std::vector<ArmemClass>;
+
+ ArmemClassVector attachments;
+
+ for (const auto&[_, coreSegment] : memory.coreSegments())
+ {
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ try
+ {
+ AronClass aronAttachment;
+ aronAttachment.fromAron(instance.data());
+
+ ArmemClass attachment;
+ fromAron(aronAttachment, attachment);
+
+ if (attachment.active)
+ {
+ attachments.push_back(attachment);
+ }
+
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ continue;
+ }
+ }
+ }
+ }
+
+ return attachments;
+ }
+ } // namespace
+
+ Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreAttachmentsSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object attachments.");
+ }
+
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+
+ std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ }
+
+ std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ }
+
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
+ {
+ // Query all entities from all provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ }
+
+ std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreAttachmentsSegmentName)
+ .providerSegments().withName(providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ }
+
+
+
+} // namespace armarx::armem::attachment
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..bcfb3d1a135a2446f255a0038a2aaad85c2a3412
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem::attachment
+{
+ class Reader
+ {
+ public:
+ Reader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+
+ private:
+
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.attachment.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& component;
+ };
+
+
+} // namespace armarx::armem::attachment
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4d16070b84e4d29aca1742ce0356fb0c69ea5b0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -0,0 +1,144 @@
+#include "Writer.h"
+
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+
+
+namespace armarx::armem::attachment
+{
+ Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+
+ void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreAttachmentsSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object attachments.");
+ def->optional(properties.providerName, prefix + "ProviderName");
+ }
+
+ void Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
+
+ memoryWriter.setWritingMemory(result.proxy);
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+
+ std::optional<armem::MemoryID> Writer::commit(const ObjectAttachment& attachment)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ const auto& timestamp = attachment.timestamp;
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId
+ .withEntityName(attachment.object.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::attachment::ObjectAttachment aronAttachment;
+ toAron(aronAttachment, attachment);
+
+ update.instancesData = {aronAttachment.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return updateResult.snapshotID;
+
+ }
+
+ std::optional<armem::MemoryID> Writer::commit(const ArticulatedObjectAttachment& attachment)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ const auto& timestamp = attachment.timestamp;
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId
+ .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::attachment::ArticulatedObjectAttachment aronAttachment;
+ toAron(aronAttachment, attachment);
+
+ update.instancesData = {aronAttachment.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return updateResult.snapshotID;
+ }
+
+
+
+} // namespace armarx::armem::attachment
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..f20421d4b27d9e2fc10e106e238a9684fca9d2cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -0,0 +1,73 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+#include "RobotAPI/libraries/armem/client.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
+
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem::attachment
+{
+
+ class Writer
+ {
+ public:
+ Writer(armem::ClientComponentPluginUser& component);
+ virtual ~Writer() = default;
+
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ std::optional<armem::MemoryID> commit(const ObjectAttachment& attachment);
+ std::optional<armem::MemoryID> commit(const ArticulatedObjectAttachment&);
+
+ private:
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
+ std::string providerName = "AttachmentProvider";
+
+ bool allowClassCreation = false;
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.articulated.";
+
+ armem::client::Writer memoryWriter;
+ std::mutex memoryWriterMutex;
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryReaderMutex;
+
+ armem::ClientComponentPluginUser& component;
+ };
+
+
+} // namespace armarx::armem::attachment
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..422b8e6be1fa3b50fe0ffc154ce2612528c3a578
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -0,0 +1,185 @@
+#include "Segment.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
+ defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
+ "If true, load the objects from the objects package on startup.");
+
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ if (p.loadFromObjectsPackage)
+ {
+ loadByObjectFinder(p.objectsPackage);
+ }
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ void Segment::loadByObjectFinder(const std::string& package)
+ {
+ ObjectFinder finder(package);
+
+ const auto knownArticulatedObjectDescriptions = finder.findAllArticulatedObjectsByDataset(true);
+ ARMARX_DEBUG << "Found " << knownArticulatedObjectDescriptions.size() << " articulated objects";
+
+ loadObjectsIntoMemory(knownArticulatedObjectDescriptions, package);
+ }
+
+ void Segment::loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(package);
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ // ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
+ // << objectFinder.getPackageName() << "' ...";
+ Commit commit;
+
+ for (const auto& [datasetName, descriptions] : datasets)
+ {
+
+ for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(datasetName + "/" + desc.name);
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription aronRobotDescription;
+ toAron(aronRobotDescription, desc);
+ // TODO toAron(aronRobotDescription.timestamp, now);
+
+ update.instancesData = { aronRobotDescription.toAron()};
+ }
+
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " articulated object classes from '"
+ << package << "' in dataset '" << datasetName << "'.";
+ }
+ iceMemory.commit(commit);
+ }
+
+
+
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ {
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ for (const auto& snapshot : simox::alg::get_values(entity.history()))
+ {
+ const auto& entityInstance = snapshot.getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(snapshot.id());
+
+ objects.emplace(armem::MemoryID(snapshot.id()), *description);
+ }
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known articulated object classes: " << objects.size();
+
+ return objects;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2a7bbc6fbf2c875e11fb25427f5a7a8809aee4c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
@@ -0,0 +1,112 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> getKnownObjectClasses() const;
+
+
+ private:
+ void loadByObjectFinder(const std::string& package);
+ void loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package);
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreClassSegmentName = "ArticulatedObjectClass";
+ int64_t maxHistorySize = -1;
+
+ std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
+ bool loadFromObjectsPackage = true;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7fa562a61f81e8a09889182efe136a963da1a2a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
@@ -0,0 +1,175 @@
+#include "Segment.h"
+
+#include <sstream>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
+
+#include "Visu.h"
+
+
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix)
+ {
+ defs->optional(p.coreInstanceSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ this->visu = std::make_unique<Visu>(arviz, *this);
+ visu->init();
+ }
+
+ void Segment::setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment)
+ {
+ classSegment = &segment;
+ }
+
+ ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects() const
+ {
+ ARMARX_CHECK_NOT_NULL(classSegment);
+ const auto knownObjectClasses = classSegment->getKnownObjectClasses();
+
+ ARMARX_DEBUG << "Class segment has " << knownObjectClasses.size() << " known articulated objects";
+
+ const auto escape = [](std::string & v)
+ {
+ v = simox::alg::replace_all(v, "/", "\\/");
+ };
+
+ const auto resolveDescriptionLink = [&](auto & articulatedObject, const auto & aronDescriptionLink) -> bool
+ {
+ armem::MemoryID descriptionLink;
+ fromAron(aronDescriptionLink, descriptionLink);
+
+ escape(descriptionLink.providerSegmentName);
+
+ ARMARX_DEBUG << "Lookup key is " << descriptionLink;
+
+ // const auto keys = simox::alg::get_keys(knownObjectClasses);
+ // ARMARX_DEBUG << "Known keys " << keys;
+
+ const auto it = knownObjectClasses.find(descriptionLink);
+ if (it == knownObjectClasses.end())
+ {
+ ARMARX_WARNING << "Unknown object class " << descriptionLink
+ << "Known classes are " << simox::alg::get_keys(knownObjectClasses);
+ return false;
+ }
+
+ articulatedObject.description = it->second;
+ return true;
+ };
+
+ ::armarx::armem::articulated_object::ArticulatedObjects objects;
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreInstanceSegmentName))
+ {
+ for (const auto& [_, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ arondto::Robot aronArticulatedObject;
+ aronArticulatedObject.fromAron(entityInstance.data());
+
+ armem::articulated_object::ArticulatedObject articulatedObject;
+ fromAron(aronArticulatedObject, articulatedObject);
+
+ // resolve memory link for description
+ const arondto::MemoryID& aronDescriptionLink = aronArticulatedObject.description;
+ if (not resolveDescriptionLink(articulatedObject, aronDescriptionLink))
+ {
+ continue;
+ }
+
+ objects.push_back(articulatedObject);
+ }
+ }
+
+ return objects;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d50be817effddb795e8cd6adf1bda992d177e87
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
@@ -0,0 +1,112 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <map>
+#include <string>
+#include <optional>
+#include <mutex>
+
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+ class Segment;
+}
+
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ struct CommitStats
+ {
+ int numUpdated = 0;
+ };
+
+
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix = "");
+
+ void init();
+
+ void setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment);
+
+ ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects() const;
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ articulated_object_class::Segment const* classSegment;
+
+
+ struct Properties
+ {
+ std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1ee962acf267c3263676772816ba7d79f8e46cb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
@@ -0,0 +1,160 @@
+#include "Visu.h"
+
+#include <algorithm>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <SimoxUtility/math/pose.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "Segment.h"
+
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+
+ void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.enabled, prefix + "enabled",
+ "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
+ "Frequency of visualization.");
+ }
+
+
+ viz::Layer Visu::visualizeProvider(
+ const std::string& providerName,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ {
+ viz::Layer layer = arviz.layer(providerName);
+
+ visualizeObjects(layer, objects);
+
+ return layer;
+ }
+
+
+ void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ {
+ const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ {
+
+ const auto xmlPath = obj.description.xml.serialize();
+
+ // clang-format off
+ auto robot = viz::Robot(obj.description.name)
+ // .file(xmlPath.package, xmlPath.path)
+ .file(xmlPath.path, xmlPath.path)
+ .joints(obj.config.jointMap)
+ .pose(obj.config.globalPose);
+ // clang-format on
+
+ robot.useFullModel();
+
+ layer.add(robot);
+ };
+
+ std::for_each(objects.begin(), objects.end(), visualizeObject);
+
+ }
+
+ void Visu::init()
+ {
+ updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
+ updateTask->start();
+ }
+
+
+ void Visu::visualizeRun()
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_DEBUG << "Update task";
+
+ if (not p.enabled)
+ {
+ return;
+ }
+
+ // TIMING_START(Visu);
+
+ const auto articulatedObjects = segment.getArticulatedObjects();
+ ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+
+ viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
+
+ ARMARX_DEBUG << "visualizing objects";
+ visualizeObjects(layer, articulatedObjects);
+
+ ARMARX_DEBUG << "Committing objects";
+ arviz.commit({layer});
+
+ ARMARX_DEBUG << "Done committing";
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+
+ }
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..3590270fe545c830b4907082dd127d8a44598587
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
@@ -0,0 +1,101 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+ class Segment;
+
+ /**
+ * @brief Visualizes articulated objects
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ void init();
+
+ protected:
+ viz::Layer visualizeProvider(
+ const std::string& providerName,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects
+ ) const;
+
+ void visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects
+ ) const;
+
+
+ private:
+ viz::Client arviz;
+ const Segment& segment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+
+
+ PeriodicTask<Visu>::pointer_type updateTask;
+ void visualizeRun();
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::CheckBox enabled;
+
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..338bc804b12c0569542e57ed283741e4145893a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -0,0 +1,127 @@
+#include "Segment.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+
+
+namespace armarx::armem::server::obj::attachments
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("Attachments");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::vector<armarx::armem::attachment::ObjectAttachment> Segment::getAttachments(const armem::Time& timestamp) const
+ {
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+
+ if (not aronAttachment)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ armarx::armem::attachment::ObjectAttachment attachment;
+ fromAron(*aronAttachment, attachment);
+
+ attachments.push_back(attachment);
+ }
+ }
+
+ return attachments;
+ }
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3817bc22a51227422848ebbe1e4296aa570fee24
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -0,0 +1,107 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::obj::attachments
+{
+ class Visu;
+
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
+
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreClassSegmentName = "Attachments";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 38b143f2baded43c3a4f028174380abaef288dcf..93989e18d6c5cd39d0f878cd2caea652fadb41b3 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -31,7 +31,7 @@ namespace armarx::armem::server::obj::clazz
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.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
"If true, load the objects from the objects package on startup.");
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 54314054a38f5f104552c045fe37eb8b401651ae..5b7c268bb28530b82906bc07cee31431d6738f5c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -159,7 +159,8 @@ namespace armarx::armem::server::obj::instance
struct Properties
{
std::string coreSegmentName = "Instance";
- long maxHistorySize = -1;
+ // -1 would be infinite, but this can let the RAM grow quickly.
+ long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
};
Properties p;
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..976f438e7c39009fd5d37fb6afdf19378da488c4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::attachment
+{
+ using AgentID = armem::MemoryID;
+ using ObjectID = armem::MemoryID;
+
+ struct AgentDescription
+ {
+ /**
+ * @brief id either pointing to a arondto::Robot or arondto::ArticulatedObject
+ *
+ */
+ AgentID id;
+
+ std::string frame;
+ };
+
+ /**
+ * @brief ObjectAttachment describes a fixed transformation between an agent and an object.
+ *
+ * The transformation is defined as follows:
+ *
+ * agent.frame -> object root frame
+ *
+ */
+ struct ObjectAttachment
+ {
+ AgentDescription agent;
+
+ Eigen::Affine3f transformation;
+
+ ObjectID object;
+
+ armem::Time timestamp;
+
+ bool active;
+ };
+
+ /**
+ * @brief ArticulatedObjectAttachment describes a fixed transformation between an agent and an articulated object.
+ *
+ * The transformation is defined as follows:
+ *
+ * agent.frame -> object.frame
+ *
+ */
+ struct ArticulatedObjectAttachment
+ {
+ AgentDescription agent;
+
+ Eigen::Affine3f transformation;
+
+ AgentDescription object;
+
+ armem::Time timestamp;
+
+ bool active;
+
+ };
+
+} // namespace armarx::armem::attachment
+
+namespace armarx::armem::articulated_object
+{
+ using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
+
+ using ArticulatedObject = armarx::armem::robot::Robot;
+ using ArticulatedObjects = armarx::armem::robot::Robots;
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eb0c4fb7d37a4bb43830f224d3a81714f1175f39
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_robot)
+
+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::armem
+ HEADERS
+ types.h
+
+ aron_conversions.h
+ robot_conversions.h
+
+ SOURCES
+
+ aron_conversions.cpp
+ robot_conversions.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
+)
+
+add_library(${PROJECT_NAME}::armem_robot ALIAS armem_robot)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
new file mode 100644
index 0000000000000000000000000000000000000000..63e87ab3c4ba7193a8b7800ed4c24881c09e1b45
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -0,0 +1,35 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <!-- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotDescription.xml>" /> -->
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::Robot">
+
+ <ObjectChild key='description'>
+ <armarx::armem::arondto::MemoryID />
+ <!-- <armarx::armem::arondto::RobotDescription /> -->
+ </ObjectChild>
+
+ <ObjectChild key='state'>
+ <armarx::armem::arondto::RobotState />
+ </ObjectChild>
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
new file mode 100644
index 0000000000000000000000000000000000000000..edce8c802a4e77cb571492700422b7c04eb7e1a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
@@ -0,0 +1,42 @@
+<!--This class cotains information on how to create a VirtualRobot::Robot -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name='armarx::armem::arondto::RobotDescription'>
+
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='xml'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <!-- <ObjectChild key='scaling'>
+ <float />
+ </ObjectChild> -->
+
+ <!--
+ <ObjectChild key='attachments'>
+ <List>
+ <armarx::arondto::Attachment />
+ </List>
+ <ObjectChild/>
+ -->
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
new file mode 100644
index 0000000000000000000000000000000000000000..eaf9b41a75f58cfe9b5a9cedbbdfcb5bd6774499
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
@@ -0,0 +1,30 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::RobotState">
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='jointMap'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a56d19f385af667974269c72c0de37af406da9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -0,0 +1,66 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+
+namespace armarx::armem::robot
+{
+
+ /* to be moved */
+ void fromAron(const long& dto, IceUtil::Time& time)
+ {
+ time = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& time)
+ {
+ dto = time.toMicroSeconds();
+ }
+
+
+ /* Robot */
+
+ void fromAron(const arondto::Robot& dto, Robot& bo)
+ {
+ // fromAron(dto.description, bo.description);
+ fromAron(dto.state, bo.config);
+ }
+
+ void toAron(arondto::Robot& dto, const Robot& bo)
+ {
+ // toAron(dto.description, bo.description);
+ toAron(dto.state, bo.config);
+ }
+
+ /* RobotDescription */
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ fromAron(dto.xml, bo.xml);
+ }
+
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ toAron(dto.xml, bo.xml);
+ }
+
+ /* RobotState */
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ fromAron(dto.timestamp, bo.timestamp);
+ bo.globalPose.matrix() = dto.globalPose;
+ bo.jointMap = dto.jointMap;
+ }
+
+ void toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ toAron(dto.timestamp, bo.timestamp);
+ dto.globalPose = bo.globalPose.matrix();
+ dto.jointMap = bo.jointMap;
+ }
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..70e06ce8eedb7de9538f5249b3eedf797aec6060
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+namespace armarx::armem::robot
+{
+ // TODO move the following
+ void fromAron(const long& dto, IceUtil::Time& time);
+ void toAron(long& dto, const IceUtil::Time& time);
+ // end TODO
+
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..d127fbf51eee8d842ee259211fd63a467d0a2f9b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -0,0 +1,29 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+namespace armarx::armem::robot
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ };
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ virtual bool store(const Robot& obj) = 0;
+ };
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3bf4171486e35c33a1cca23a666baac2233a8356
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -0,0 +1,63 @@
+#include "robot_conversions.h"
+
+#include <filesystem>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::robot
+{
+
+ std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
+ {
+ arondto::RobotDescription aronRobotDescription;
+ try
+ {
+ aronRobotDescription.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to RobotDescription failed!";
+ return std::nullopt;
+ }
+
+ RobotDescription robotDescription
+ {
+ .name = "",
+ .xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
+ };
+
+ fromAron(aronRobotDescription, robotDescription);
+
+ return robotDescription;
+ }
+
+
+ std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
+ {
+ arondto::Robot aronRobot;
+ try
+ {
+ aronRobot.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to RobotState failed!";
+ return std::nullopt;
+ }
+
+ RobotState robotState;
+ fromAron(aronRobot.state, robotState);
+
+ return robotState;
+ }
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.h b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..3692ae048dacb94d2f24469f5265e998ab23fb34
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+
+#include "types.h"
+
+namespace armarx::armem::wm
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem::robot
+{
+ std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
+ std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fed4d1cb277ab316a318cbc13b4f32222fd8939
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <map>
+#include <vector>
+#include <filesystem>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+
+namespace armarx::armem::robot
+{
+ struct RobotDescription
+ {
+ // IceUtil::Time timestamp;
+
+ std::string name;
+ PackagePath xml{"", std::filesystem::path("")};
+ };
+
+ struct RobotState
+ {
+ using JointMap = std::map<std::string, float>;
+ using Pose = Eigen::Affine3f;
+
+ IceUtil::Time timestamp;
+
+ Pose globalPose;
+ JointMap jointMap;
+ };
+
+ struct Robot
+ {
+ RobotDescription description;
+ std::string instance;
+
+ RobotState config;
+
+ IceUtil::Time timestamp;
+ };
+
+ using Robots = std::vector<Robot>;
+ using RobotDescriptions = std::vector<RobotDescription>;
+ using RobotStates = std::vector<RobotState>;
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
deleted file mode 100644
index 19a7f00199d06462123d94d345f54fec030672cc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-set(LIB_NAME armem_robot_localization)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- ArmarXCoreInterfaces
- ArmarXGuiComponentPlugins
- # This package
- RobotAPICore
- RobotAPIInterfaces
- RobotAPI::libraries::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./TransformInterfaces.h
- ./TransformReader.h
- ./TransformWriter.h
- ./MemoryConnector.h
- ./aron_conversions.h
- SOURCES
- ./TransformReader.cpp
- ./TransformWriter.cpp
- ./MemoryConnector.cpp
- ./aron_conversions.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/Transform.xml
-)
-
-
-add_library(RobotAPI::armem_robot_localization ALIAS armem_robot_localization)
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
deleted file mode 100644
index 4fadf6f65ebb0ade9b91d49d36860bf6e9b0ff64..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-#include "MemoryConnector.h"
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx::armem
-{
-
- MemoryConnector::MemoryConnector(ManagedIceObject& component) : component(component) {}
-
- MemoryConnector::~MemoryConnector() = default;
-
- void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
- {
- const std::string prefix = getPropertyPrefix();
-
- ARMARX_INFO << "Memory connector: registerPropertyDefinitions with prefix " << prefix;
-
-
- def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
- }
-
- armem::data::WaitForMemoryResult MemoryConnector::useMemory(const std::string& memoryName)
- {
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
-
- ARMARX_CHECK_NOT_NULL(memoryNameSystem);
-
- ARMARX_INFO << "Waiting for memory ...";
- armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
-
- if (result.success)
- {
- // Add dependency.
- component.usingProxy(result.proxy->ice_getIdentity().name);
- }
- else
- {
- ARMARX_WARNING << "Use memory: Failure";
- }
- return result;
- }
-
- const std::string& MemoryConnector::getPropertyPrefix() const
- {
- return propertyPrefix;
- }
-
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h b/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
deleted file mode 100644
index 6962593814414267635efc425637266c7fbcd34a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
+++ /dev/null
@@ -1,111 +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::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <Eigen/Geometry>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-namespace armarx
-{
-
- struct TransformHeader
- {
- std::string parentFrame;
- std::string frame;
-
- std::string agent;
-
- std::int64_t timestamp; // [µs]
- };
-
- struct Transform
- {
- TransformHeader header;
-
- Eigen::Affine3f transform = Eigen::Affine3f::Identity();
- };
-
- namespace armem
- {
-
- struct TransformResult
- {
- Transform transform;
-
- enum class Status
- {
- Success,
- ErrorLookupIntoFuture,
- ErrorFrameNotAvailable
- } status;
-
- explicit operator bool() const
- {
- return status == Status::Success;
- }
-
- std::string errorMessage = "";
- };
-
- struct TransformQuery
- {
- TransformHeader header;
-
- // bool exact;
- };
-
- class TransformInterface
- {
- public:
- virtual ~TransformInterface() = default;
-
- virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
- };
-
- class TransformReaderInterface : virtual public TransformInterface
- {
- public:
- virtual ~TransformReaderInterface() = default;
-
- virtual TransformResult
- getGlobalPose(const std::string& agentName, const std::string& robotRootFrame,
- const std::int64_t& timestamp) const = 0;
-
- virtual TransformResult
- lookupTransform(const TransformQuery& query) const = 0;
- // waitForTransform()
- };
-
- class TransformWriterInterface : virtual public TransformInterface
- {
- public:
- ~TransformWriterInterface() override = default;
-
- virtual bool commitTransform(const Transform& transform) = 0;
- };
-
- } // namespace armem
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
deleted file mode 100644
index c3b24c4895328e951bc4950339639200206a9daf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
+++ /dev/null
@@ -1,449 +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::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "TransformReader.h"
-
-#include <Eigen/src/Geometry/Transform.h>
-#include <algorithm>
-#include <iterator>
-#include <numeric>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// Eigen
-#include <Eigen/Geometry>
-
-// Simox
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <SimoxUtility/color/cmaps.h>
-#include <SimoxUtility/math/pose/interpolate.h>
-
-// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-// this package
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.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>
-#include <RobotAPI/libraries/armem_robot_localization/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-
-namespace armarx::armem
-{
-
- TransformReader::TransformReader(ManagedIceObject& component) : MemoryConnector(component) {}
-
- TransformReader::~TransformReader() = default;
-
- void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
-
- def->optional(properties.localizationMemoryName,
- prefix + "LocalizationMemoryName",
- "Name of the localization memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void TransformReader::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
- << "' ...";
- auto result = useMemory(properties.memoryName);
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
- return;
- }
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
- }
-
- TransformResult TransformReader::getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const std::int64_t& timestamp) const
- {
- const TransformQuery query{.header = {.parentFrame = GlobalFrame,
- .frame = robotRootFrame,
- .agent = agentName,
- .timestamp = timestamp
- }};
-
- return lookupTransform(query);
- }
-
- // std::vector<std::string> buildTransformChainArbitrary(const
- // std::map<std::string, armem::ProviderSegment>& providerSegments, const
- // std::string& parentFrame, const std::string& frame){
-
- // std::map<std::string, std::string> tf;
-
- // // build string representation of transforms
- // for(const auto& [name, segment]: providerSegments){
- // const auto frames = simox::alg::split(name, ",");
- // if (frames.size() != 2){
- // ARMARX_WARNING << "Segment name " << name << " not understood";
- // continue;
- // }
-
- // tf.insert(frame[0], frame[1]);
- // }
-
- // // find
-
- // }
-
- std::vector<std::string> TransformReader::buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const
- {
- ARMARX_DEBUG << "Building transform chain";
-
- auto join = [](const std::string & parentFrame, const std::string & frame)
- {
- return parentFrame + "," + frame;
- };
-
- std::vector<std::string> chain;
-
- const auto& agentProviderSegment = memory.getCoreSegment(properties.localizationMemoryName)
- .getProviderSegment(query.header.agent);
-
- auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
- {
- const auto entityName = join(parentFrame, frame);
-
- if (agentProviderSegment.hasEntity(entityName))
- {
- chain.push_back(entityName);
- }
- else
- {
- ARMARX_WARNING << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
- }
- };
-
- std::array<std::string, 3> knownChain
- {
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
-
- auto* frameBeginIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
- auto* const frameEndIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.frame);
-
- if (frameBeginIt == knownChain.end())
- {
- ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
- return {};
- }
-
- if (frameEndIt == knownChain.end())
- {
- ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
- }
-
- const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
- ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
- << "' across " << nFrames << " frames";
-
- for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
- {
- addToChain(*frameBeginIt, *(frameBeginIt + 1));
- }
-
- if (frameEndIt == knownChain.end())
- {
- addToChain(knownChain.back(), query.header.frame);
- }
-
- if (chain.empty())
- {
- ARMARX_WARNING << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "'";
- return {};
- }
-
- return chain;
- }
-
- inline Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
- {
- aron::Transform aronTransform;
- aronTransform.fromAron(item.data());
-
- Transform transform;
- fromAron(aronTransform, transform);
-
- return transform;
- }
-
- decltype(auto) findFirstElementAfter(const std::vector<Transform>& transforms,
- const int64_t timestamp)
- {
- auto timestampBeyond = [timestamp](const Transform & transform)
- {
- return transform.header.timestamp > timestamp;
- };
-
- const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
- return poseNextIt;
- }
-
- Eigen::Affine3f interpolateTransform(const std::vector<Transform>& queue, int64_t timestamp)
- {
- ARMARX_TRACE;
-
- ARMARX_DEBUG << "Entering";
-
-
- ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
-
-
- ARMARX_DEBUG << "Entering ... "
- << "Q front " << queue.front().header.timestamp << " "
- << "Q back " << queue.back().header.timestamp << " "
- << "query timestamp " << timestamp;
-
- // TODO(fabian.reister): sort queue.
-
-
- ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
-
- // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
- ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
- // => now we know that there is an element right after and before the timestamp within our queue
-
- ARMARX_DEBUG << "Entering 2";
-
- const auto poseNextIt = findFirstElementAfter(queue, timestamp);
-
- ARMARX_DEBUG << "it ari";
-
- const auto posePreIt = poseNextIt - 1;
-
- ARMARX_DEBUG << "deref";
-
- // the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp);
-
- ARMARX_DEBUG << "interpolate";
-
- return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
- }
-
- 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());
-
- // if (entity.history.empty())
- // {
- // // TODO(fabian.reister): fixme boom
- // ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
- // }
-
-
- std::vector<Transform> transforms;
- transforms.reserve(entity.history().size());
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- std::transform(entitySnapshots.begin(),
- entitySnapshots.end(),
- std::back_inserter(transforms),
- [](const auto & entity)
- {
- return convertEntityToTransform(entity.getInstance(0));
- });
-
- ARMARX_DEBUG << "obtaining transform";
-
- if (transforms.size() > 1)
- {
- // TODO(fabian.reister): remove
- return transforms.front().transform;
-
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
- const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
- return p;
- }
-
-
- // accept this to fail (will raise armem::error::MissingEntry)
- if (transforms.empty())
- {
- ARMARX_DEBUG << "empty transform";
-
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
- }
-
- ARMARX_DEBUG << "single transform";
-
-
- return transforms.front().transform;
- }
-
- std::vector<Eigen::Affine3f>
- 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());
-
- const auto& agentProviderSegment =
- memory.getCoreSegment(properties.localizationMemoryName).getProviderSegment(agent);
-
- ARMARX_DEBUG << "Provider segments"
- << simox::alg::get_keys(
- memory.getCoreSegment(properties.localizationMemoryName)
- .providerSegments());
-
- ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
-
- try
- {
- std::vector<Eigen::Affine3f> transforms;
- transforms.reserve(tfChain.size());
- std::transform(tfChain.begin(),
- tfChain.end(),
- std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return obtainTransform(entityName, agentProviderSegment, timestamp);
- });
- return transforms;
- }
- catch (const armem::error::MissingEntry& missingEntryError)
- {
- ARMARX_WARNING << missingEntryError.what();
- }
- catch (const ::armarx::exceptions::local::ExpressionException& ex)
- {
- ARMARX_WARNING << "local exception: " << ex.what();
- }
-
- return {};
-
- }
-
- TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
- {
-
- const auto timestamp = IceUtil::Time::microSeconds(query.header.timestamp);
-
- const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
-
- ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
-
- // Query all entities from provider.
- armem::client::query::Builder qb;
-
- // clang-format off
- qb
- .coreSegments().withName(properties.localizationMemoryName)
- .providerSegments().withName(query.header.agent) // agent
- .entities().all() // parentFrame,frame
- .snapshots().timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
- // clang-format on
-
- // TODO(fabian.reister): remove latest() and add atTime
- // .atTime(timestamp); // transformation
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success)
- {
- return {.transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage};
- }
-
- const std::vector<std::string> tfChain = buildTransformChain(qResult.memory, query);
-
- if (tfChain.empty())
- {
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'"};
- }
-
- const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
- qResult.memory, tfChain, query.header.agent, timestamp.toMicroSeconds());
-
- if (transforms.empty())
- {
- ARMARX_WARNING << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'. No memory data in time range."};
- }
-
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
-
- ARMARX_DEBUG << "Found valid transform";
-
- return {.transform = {.header = query.header, .transform = transform},
- .status = TransformResult::Status::Success};
- }
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
deleted file mode 100644
index 40832ae90e2686d76a1d2256b8e427c2cafc88a2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#include "aron_conversions.h"
-
-// STL
-#include <string>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-
-
-namespace armarx
-{
-
- TransformHeader fromAron(const aron::TransformHeader& aronHeader)
- {
- TransformHeader header;
-
- header.parentFrame = aronHeader.parentFrame;
- header.frame = aronHeader.frame;
- header.agent = aronHeader.agent;
- header.timestamp = aronHeader.timestamp;
-
- return header;
- }
-
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform)
- {
- transform.header = fromAron(aronTransform.header);
- transform.transform = aronTransform.transform;
- }
-
-
- aron::TransformHeader toAron(const TransformHeader& header)
- {
- aron::TransformHeader aronHeader;
-
- aronHeader.parentFrame = header.parentFrame;
- aronHeader.frame = header.frame;
- aronHeader.agent = header.agent;
- aronHeader.timestamp = header.timestamp;
-
- return aronHeader;
- }
-
- void toAron(const Transform& transform, aron::Transform& aronTransform)
- {
- aronTransform.header = toAron(transform.header);
- aronTransform.transform = transform.transform.matrix();
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
deleted file mode 100644
index 6a5379fe759f73fd03ad7e274a9a2211ba89d846..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-
-namespace armarx
-{
- struct Transform;
-
- namespace aron
- {
- struct Transform;
- } // namespace aron
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform);
- void toAron(const Transform& transform, aron::Transform& aronTransform);
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
index 5712d4c15f338783ac913f3416d6b8fcdb900003..145ced819b68db659a92e232747c645c24037ce5 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
@@ -9,9 +9,7 @@ armarx_add_library(
ArmarXCore
# This package
RobotAPICore
- RobotAPI::libraries::armem
- # TODO(fabian.reister): remove this dependency!
- RobotAPI::armem_robot_localization
+ RobotAPI::armem
# System / External
Eigen3::Eigen
HEADERS
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 99ed2fa4129d1caea1ea52960e0c37fe4ec9fd44..fd8250c5b2b086f3869729e2bf6836e5e3c5dc32 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -44,7 +44,7 @@
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
+
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
@@ -52,8 +52,8 @@
namespace armarx::armem
{
- MappingDataReader::MappingDataReader(ManagedIceObject& component)
- : armarx::armem::MemoryConnector(component) {}
+ MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
+ : memoryClient(memoryClient) {}
MappingDataReader::~MappingDataReader() = default;
@@ -61,9 +61,9 @@ namespace armarx::armem
armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
+ registerPropertyDefinitions(def);
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = propertyPrefix;
def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
"Name of the mapping memory core segment to use.");
@@ -76,7 +76,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
<< properties.memoryName << "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -156,7 +156,7 @@ namespace armarx::armem
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.size();
+ ARMARX_DEBUG << "History size: " << entity.size();
for (const auto &[ss, entitySnapshot] : entity)
{
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
index 359b88fe8550d9b64f066fe942a5c6fda7dce172..aee783764f4b5720e4ae0d8d69fbf6faed297e98 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -27,9 +27,8 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-// TODO(fabian.reister): move MemoryConnector to armem library
-#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
@@ -60,11 +59,10 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader :
- virtual public armarx::armem::MemoryConnector
+ class MappingDataReader
{
public:
- MappingDataReader(ManagedIceObject& component);
+ MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
virtual ~MappingDataReader();
@@ -103,10 +101,6 @@ namespace armarx::armem
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
client::query::Builder buildQuery(const Query& query) const ;
@@ -123,7 +117,9 @@ namespace armarx::armem
} properties;
- const std::string propertyPrefix = "mem.mapping.read.";
+ const std::string propertyPrefix = "mem.mapping.";
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
};
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
index dd0299ece229eaac2fb4f44758fae23f763f612d..351d71398a6959f346aadf45c6c604edcd865a53 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
@@ -7,8 +7,8 @@
namespace armarx::armem
{
- MappingDataWriter::MappingDataWriter(ManagedIceObject& component)
- : MemoryConnector(component) {}
+ MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
+ : component(component) {}
MappingDataWriter::~MappingDataWriter() = default;
@@ -17,9 +17,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = propertyPrefix;
def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
"Name of the mapping memory core segment to use.");
@@ -32,7 +30,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = component.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -71,7 +69,6 @@ namespace armarx::armem
armem::EntityUpdate update;
update.entityID = entityID;
- update.timeCreated = armem::Time::now();
arondto::LaserScanStamped aronSensorData;
// currently only sets the header
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
index 5d5f21185ba6bfad4cb319bef81853ef3bd551f5..e5de996746b7d54b46faa891cf2cd26007fbaf02 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
@@ -22,13 +22,12 @@
#pragma once
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <mutex>
-#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
-// TODO(fabian.reister): move MemoryConnector to armem lib
-#include "RobotAPI/libraries/armem_robot_localization/MemoryConnector.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client.h>
namespace armarx::armem
@@ -45,11 +44,10 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter :
- virtual public ::armarx::armem::MemoryConnector
+ class MappingDataWriter
{
public:
- MappingDataWriter(ManagedIceObject& component);
+ MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
virtual ~MappingDataWriter();
void connect();
@@ -63,11 +61,6 @@ namespace armarx::armem
bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
-
private:
armem::client::Writer memoryWriter;
@@ -81,7 +74,9 @@ namespace armarx::armem
std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.mapping.write.";
+ const std::string propertyPrefix = "mem.mapping.";
+
+ armem::ClientWriterComponentPluginUser& component;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..498a422fb201bf435cfb8ddbfdc43b71ba1944a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -0,0 +1,82 @@
+set(LIB_NAME armem_robot_state)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXGuiComponentPlugins
+ # This package
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPI::armem
+ RobotAPI::armem_robot
+ # System / External
+ Eigen3::Eigen
+ aroncommon
+ HEADERS
+ ./common/localization/types.h
+ ./common/localization/TransformHelper.h
+
+ ./client/common/RobotReader.h
+ ./client/common/VirtualRobotReader.h
+
+ ./client/localization/interfaces.h
+ ./client/localization/TransformReader.h
+ ./client/localization/TransformWriter.h
+
+ ./server/common/Visu.h
+
+ ./server/localization/Segment.h
+ # ./server/localization/Visu.h
+
+ ./server/proprioception/Segment.h
+ # ./server/proprioception/Visu.h
+
+ ./server/description/Segment.h
+
+
+ ./aron_conversions.h
+ ./utils.h
+ SOURCES
+ ./common/localization/TransformHelper.cpp
+
+ ./client/common/RobotReader.cpp
+ ./client/common/VirtualRobotReader.cpp
+
+ ./client/localization/TransformReader.cpp
+ ./client/localization/TransformWriter.cpp
+
+ ./server/common/Visu.cpp
+
+ ./server/localization/Segment.cpp
+ # ./server/localization/Visu.cpp
+
+ ./server/proprioception/Segment.cpp
+ # ./server/proprioception/Visu.cpp
+
+ ./server/description/Segment.cpp
+
+ ./aron_conversions.cpp
+ ./utils.cpp
+
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/TransformHeader.xml
+ aron/Transform.xml
+ aron/JointState.xml
+)
+
+
+add_library(RobotAPI::armem_robot_state ALIAS armem_robot_state)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c7b78a90cfa452579dfea76227833b66c5befbe4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
@@ -0,0 +1,14 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name="armarx::armem::arondto::JointState">
+ <ObjectChild key='name'>
+ <String/>
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Float />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
new file mode 100644
index 0000000000000000000000000000000000000000..02ebe91b35094809953de6cfd0252c8a2048cf98
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
@@ -0,0 +1,21 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+ <Object name="armarx::armem::arondto::Transform">
+ <ObjectChild key='header'>
+ <armarx::armem::arondto::TransformHeader />
+ </ObjectChild>
+ <ObjectChild key='transform'>
+ <Pose />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
similarity index 66%
rename from source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
index 1e4e4c7d61c14d70a94499f8491254a20d336c89..c6bd5b94f93fa10e0638c75d17e8dbed0ad5bc60 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
@@ -5,8 +5,7 @@
<Include include="<Eigen/Core>" />
</CodeIncludes>
<GenerateTypes>
-
- <Object name="armarx::aron::TransformHeader">
+ <Object name="armarx::armem::arondto::TransformHeader">
<ObjectChild key='parentFrame'>
<string />
</ObjectChild>
@@ -20,15 +19,5 @@
<Time />
</ObjectChild>
</Object>
-
- <Object name="armarx::aron::Transform">
- <ObjectChild key='header'>
- <armarx::aron::TransformHeader />
- </ObjectChild>
- <ObjectChild key='transform'>
- <Pose />
- </ObjectChild>
- </Object>
-
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27fecee2e4f4ae9137a4e7f6281bef679d308da4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -0,0 +1,66 @@
+#include "aron_conversions.h"
+
+// STL
+#include <string>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// RobotAPI
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "RobotAPI/libraries/armem_robot_state/types.h"
+
+namespace armarx::armem
+{
+
+ /* Transform */
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ {
+ fromAron(dto.header, bo.header);
+ aron::fromAron(dto.transform, bo.transform);
+ }
+
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ {
+ toAron(dto.header, bo.header);
+ aron::toAron(dto.transform, bo.transform);
+ }
+
+ /* TransformHeader */
+
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ {
+ aron::toAron(dto.parentFrame, bo.parentFrame);
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.agent, bo.agent);
+ armarx::toAron(dto.timestamp, bo.timestamp);
+ }
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ {
+ aron::fromAron(dto.parentFrame, bo.parentFrame);
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.agent, bo.agent);
+ armarx::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+ /* JointState */
+
+ void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ aron::fromAron(dto.position, bo.position);
+ }
+
+ void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ aron::toAron(dto.position, bo.position);
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..202a81f089702e07155232200911fa0e0afa197d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::armem
+{
+ namespace robot_state
+ {
+ struct Transform;
+ struct TransformHeader;
+
+ struct JointState;
+
+ } // namespace robot_state
+
+ namespace arondto
+ {
+ struct Transform;
+ struct TransformHeader;
+
+ struct JointState;
+
+ } // namespace arondto
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
+
+ void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
+ void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d8319dc9f6517c41e9a38225ad4dbd11b9ae607
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -0,0 +1,330 @@
+#include "RobotReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/PackagePath.h>
+
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::robot_state
+{
+
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) :
+ memoryClient(component), transformReader(component)
+ {
+ }
+
+ void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ transformReader.registerPropertyDefinitions(def);
+
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
+ def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
+ def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
+ def->optional(properties.proprioceptionCoreSegment,
+ propertyPrefix + "proprioceptionSegment");
+ }
+
+ void RobotReader::connect()
+ {
+ transformReader.connect();
+
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ std::optional<robot::Robot> RobotReader::get(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ ARMARX_ERROR << "Unknown object " << name;
+ return std::nullopt;
+ }
+
+ return get(*description, timestamp);
+ }
+
+ robot::Robot RobotReader::get(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
+ {
+ robot::Robot robot{.description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
+
+ synchronize(robot, timestamp);
+
+ return robot;
+ }
+
+ bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ {
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return false;
+ }
+
+ obj.config = std::move(*state);
+ return true;
+ }
+
+ std::optional<robot::RobotDescription>
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().all() // withName(properties.descriptionCoreSegment)
+ .providerSegments().all() // .withName(name)
+ .entities().all()
+ .snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
+ // clang-format on
+
+ ARMARX_INFO << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null";
+ return std::nullopt;
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_INFO << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotDescription(qResult.memory, name);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
+ }
+
+ return std::nullopt;
+ }
+
+ std::optional<robot::RobotState>
+ RobotReader::queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
+ {
+ const auto jointMap = queryJointState(description, timestamp);
+ if (not jointMap)
+ {
+ ARMARX_WARNING << "Failed to query joint state";
+ return std::nullopt;
+ }
+
+ const auto globalPose = queryGlobalPose(description, timestamp);
+ if (not globalPose)
+ {
+ ARMARX_WARNING << "Failed to query global pose";
+ return std::nullopt;
+ }
+
+ return robot::RobotState
+ {
+ .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
+ }
+
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotJointState(qResult.memory, description.name);
+ }
+
+ std::optional<robot::RobotState::Pose>
+ RobotReader::queryGlobalPose(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
+ if (not result)
+ {
+ return std::nullopt;
+ }
+
+ return result.transform.transform;
+ }
+
+ std::optional<robot::RobotState>
+ RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment)
+ .getProviderSegment(name);
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ // TODO entitiesToRobotState()
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ // Here, we access the RobotUnit streaming data stored in the proprioception segment.
+ return robot::convertRobotState(instance);
+ }
+
+ // FIXME remove this, use armem/util/util.h
+ template <typename AronClass>
+ std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ try
+ {
+ AronClass t;
+ t.fromAron(item.data());
+ return t;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ return std::nullopt;
+ }
+ }
+
+ std::optional<robot::RobotState::JointMap>
+ RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+
+ robot::RobotState::JointMap jointMap;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ {
+
+ for (const auto &[name, entity] : providerSegment.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
+
+ jointMap.emplace(jointState->name, jointState->position);
+ }
+ }
+
+ if (jointMap.empty())
+ {
+ return std::nullopt;
+ }
+
+ return jointMap;
+ }
+
+ std::optional<robot::RobotDescription>
+ RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.descriptionCoreSegment)
+ .getProviderSegment(name); // TODO(fabian.reister): all
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
+ }
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5bd01e5d182a88cc6d36e8c8112e1683c017d16
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+#include "RobotAPI/libraries/armem_robot/client/interfaces.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
+
+namespace armarx::armem::robot_state
+{
+ /**
+ * @brief The RobotReader class.
+ *
+ * The purpose of this class is to synchronize the armem data structure armem::Robot
+ * with the memory.
+ */
+ class RobotReader : virtual public robot::ReaderInterface
+ {
+ public:
+ RobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~RobotReader() = default;
+
+ void connect();
+
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+
+ std::optional<robot::Robot> get(const std::string& name,
+ const armem::Time& timestamp) override;
+ robot::Robot get(const robot::RobotDescription& description,
+ const armem::Time& timestamp) override;
+
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name,
+ const armem::Time& timestamp);
+
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp);
+
+ std::optional<robot::RobotState::JointMap>
+ queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ std::optional<robot::RobotState::Pose>
+ queryGlobalPose(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ private:
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::RobotState::JointMap>
+ getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+ struct Properties
+ {
+ std::string memoryName = "RobotStateMemory";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
+ std::string proprioceptionCoreSegment = "Proprioception";
+ } properties;
+
+ const std::string propertyPrefix = "mem.robot_state.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
+
+ client::robot_state::localization::TransformReader transformReader;
+ };
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3655fee1ae2597b768424f0b89fd581a1bb0680
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -0,0 +1,86 @@
+#include "VirtualRobotReader.h"
+
+#include <optional>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+
+
+namespace armarx::armem::robot_state
+{
+
+ VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
+ RobotReader(component)
+ {
+ }
+
+ void VirtualRobotReader::connect()
+ {
+ RobotReader::connect();
+ }
+
+ // TODO(fabian.reister): register property defs
+ void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ RobotReader::registerPropertyDefinitions(def);
+ }
+
+ bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp)
+ {
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+
+ const robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{package, robot.getFilename()}};
+
+ const auto robotState = queryState(robotDescription, timestamp);
+ if (not robotState)
+ {
+ ARMARX_WARNING << "Querying robot state failed!";
+ return false;
+ }
+
+ robot.setJointValues(robotState->jointMap);
+ robot.setGlobalPose(robotState->globalPose.matrix());
+
+ return true;
+ }
+
+ VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ {
+ ARMARX_INFO << "Querying robot description for robot '" << name << "'";
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ return nullptr;
+ }
+
+ const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
+
+ return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+ }
+
+
+ VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ {
+ auto robot = getRobot(name, timestamp);
+
+ synchronizeRobot(*robot, timestamp);
+
+ return robot;
+ }
+
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..a280e71b6132f065aec28a008ebb5ab803845dad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -0,0 +1,64 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotReader.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+namespace armarx::armem::robot_state
+{
+ /**
+ * @brief The VirtualRobotReader class.
+ *
+ * The aim of this class is to obtain a virtual robot instance and synchronize it
+ * with the data (joint positions, global pose, ...) stored in the working memory.
+ *
+ * This is only a lightweight wrapper of @see RobotReader for Simox's VirtualRobot class.
+ */
+ class VirtualRobotReader : virtual public RobotReader
+ {
+ public:
+ VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~VirtualRobotReader() = default;
+
+ void connect();
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+
+ VirtualRobot::RobotPtr
+ getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ VirtualRobot::RobotPtr
+ getSynchronizedRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
+ };
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a37b2dec14cfc6721b7458fce984dac59bdfb21
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -0,0 +1,173 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TransformReader.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+
+#include <algorithm>
+#include <iterator>
+#include <numeric>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Eigen
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/interpolate.h>
+
+// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+// this package
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.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>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+namespace armarx::armem::client::robot_state::localization
+{
+
+ TransformReader::TransformReader(armem::ClientReaderComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+
+ TransformReader::~TransformReader() = default;
+
+ void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.localizationSegment,
+ prefix + "localizationSegment",
+ "Name of the localization memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "Memory");
+ }
+
+ void TransformReader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ TransformResult TransformReader::getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const
+ {
+ const TransformQuery query{.header = {.parentFrame = GlobalFrame,
+ .frame = robotRootFrame,
+ .agent = agentName,
+ .timestamp = timestamp
+ }};
+
+ return lookupTransform(query);
+ }
+
+ // std::vector<std::string> buildTransformChainArbitrary(const
+ // std::map<std::string, armem::ProviderSegment>& providerSegments, const
+ // std::string& parentFrame, const std::string& frame){
+
+ // std::map<std::string, std::string> tf;
+
+ // // build string representation of transforms
+ // for(const auto& [name, segment]: providerSegments){
+ // const auto frames = simox::alg::split(name, ",");
+ // if (frames.size() != 2){
+ // ARMARX_WARNING << "Segment name " << name << " not understood";
+ // continue;
+ // }
+
+ // tf.insert(frame[0], frame[1]);
+ // }
+
+ // // find
+
+ // }
+
+
+ TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
+ {
+ const auto& timestamp = query.header.timestamp;
+
+ const auto durationEpsilon = IceUtil::Time::milliSeconds(-1);
+
+ ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.localizationSegment)
+ .providerSegments().withName(query.header.agent) // agent
+ .entities().all() // parentFrame,frame
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success)
+ {
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
+ }
+
+ const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
+
+ using armarx::armem::common::robot_state::localization::TransformHelper;
+ return TransformHelper::lookupTransform(localizationCoreSegment, query);
+ }
+
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
similarity index 59%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 9b04899545e78bdad1b3d0dbcca03c0ba61b4dcd..2d1d98ddf82c5c137b934b3be91079bb87298503 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -24,28 +24,28 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include "MemoryConnector.h"
-#include "TransformInterfaces.h"
+#include <RobotAPI/libraries/armem/client.h>
-namespace armarx::armem
+#include "interfaces.h"
+
+namespace armarx::armem::client::robot_state::localization
{
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
* A description of the component ExampleClient.
*
- * @class ExampleClient
+ * @class TransformReader
* @ingroup Component-ExampleClient
* @brief Brief description of class ExampleClient.
*
* Detailed description of class ExampleClient.
*/
class TransformReader :
- virtual public ::armarx::armem::TransformReaderInterface,
- virtual public ::armarx::armem::MemoryConnector
+ virtual public TransformReaderInterface
{
public:
- TransformReader(ManagedIceObject& component);
+ TransformReader(armem::ClientReaderComponentPluginUser& memoryClient);
~TransformReader() override;
@@ -53,37 +53,27 @@ namespace armarx::armem
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
- const std::int64_t& timestamp) const override;
+ const armem::Time& timestamp) const override;
+
TransformResult lookupTransform(const TransformQuery& query) const override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
-
private:
- std::vector<std::string> buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const;
-
- 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::wm::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
-
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationMemoryName = "Localization";
+ std::string memoryName = "RobotStateMemory";
+ std::string localizationSegment = "Localization";
} properties;
- const std::string propertyPrefix = "mem.localization.read.";
+ const std::string propertyPrefix = "mem.robot_state.";
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
+
};
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
similarity index 61%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 1ac8ea58b31d24e14d7f2ee14eb9c34d61178e44..2327c141840bc74f8b43ab14df03b03f2b98ccdf 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -13,7 +13,6 @@
* 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::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -22,6 +21,7 @@
#include "TransformWriter.h"
+#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
@@ -44,15 +44,16 @@
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "aron_conversions.h"
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
- TransformWriter::TransformWriter(ManagedIceObject& component) : MemoryConnector(component) {}
+ TransformWriter::TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
TransformWriter::~TransformWriter() = default;
@@ -60,15 +61,11 @@ namespace armarx::armem
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
-
- def->optional(properties.localizationMemoryName,
- prefix + "LocalizationMemoryName",
+ def->optional(properties.localizationSegment,
+ propertyPrefix + "localizationSegment",
"Name of the localization memory core segment to use.");
- def->optional(properties.memoryName, prefix + "MemoryName");
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
}
void TransformWriter::connect()
@@ -76,7 +73,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -88,40 +85,56 @@ namespace armarx::armem
memoryWriter.setWritingMemory(result.proxy);
}
- bool TransformWriter::commitTransform(const Transform& transform)
+ bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
ARMARX_DEBUG << "Trying to create core segment + provider segment";
- const auto result =
- memoryWriter.addSegment(properties.localizationMemoryName, transform.header.agent);
-
- if (not result.success)
+ const auto providerId = [&]() -> std::optional<armem::MemoryID>
+ {
+ try
+ {
+ const auto result =
+ memoryWriter.addSegment(properties.localizationSegment, transform.header.agent);
+
+ if (not result.success)
+ {
+ ARMARX_WARNING << "Could not obtain provider id! Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ return armem::MemoryID(result.segmentID);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Could not obtain provider id!";
+ return std::nullopt;
+ }
+ }();
+
+ if (not providerId)
{
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
return false;
}
- const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
+ // const auto& timestamp = transform.header.timestamp;
+ const auto timestamp = IceUtil::Time::now(); // FIXME remove
+
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID = providerId.withEntityName(transform.header.parentFrame + "," +
+ const auto entityID = providerId->withEntityName(transform.header.parentFrame + "," +
transform.header.frame).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
- update.timeCreated = armem::Time::now();
- aron::Transform aronTransform;
- toAron(transform, aronTransform);
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
update.instancesData = {aronTransform.toAron()};
- update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
+ update.timeCreated = timestamp;
- ARMARX_DEBUG << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+ ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
ARMARX_DEBUG << updateResult;
@@ -134,10 +147,10 @@ namespace armarx::armem
return updateResult.success;
}
- const std::string& TransformWriter::getPropertyPrefix() const
- {
- return propertyPrefix;
- }
+ // const std::string& TransformWriter::getPropertyPrefix() const
+ // {
+ // return propertyPrefix;
+ // }
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
similarity index 69%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 2804fbc53e7a0f61c937dc0a1b6c65d558196e1c..aa9e295bf19b34229697ad43725884469c9d1172 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -25,11 +25,13 @@
#include <mutex>
#include <RobotAPI/libraries/armem/client/Writer.h>
+// #include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-#include "MemoryConnector.h"
-#include "TransformInterfaces.h"
+#include <RobotAPI/libraries/armem/client.h>
-namespace armarx::armem
+#include "interfaces.h"
+
+namespace armarx::armem::client::robot_state::localization
{
/**
@@ -44,11 +46,11 @@ namespace armarx::armem
* Detailed description of class ExampleClient.
*/
class TransformWriter :
- virtual public ::armarx::armem::TransformWriterInterface,
- virtual public ::armarx::armem::MemoryConnector
+ virtual public TransformWriterInterface
+ // virtual public ::armarx::armem::MemoryConnector
{
public:
- TransformWriter(ManagedIceObject& component);
+ TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient);
~TransformWriter() override;
@@ -59,22 +61,22 @@ namespace armarx::armem
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
- bool commitTransform(const Transform& transform) override;
-
- const std::string& getPropertyPrefix() const override;
+ bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
private:
armem::client::Writer memoryWriter;
+ std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationMemoryName = "Localization";
+ std::string memoryName = "RobotStateMemory";
+ std::string localizationSegment = "Localization";
} properties;
- std::mutex memoryWriterMutex;
+ const std::string propertyPrefix = "mem.robot_state.";
- const std::string propertyPrefix = "mem.localization.write.";
+ armem::ClientWriterComponentPluginUser& memoryClient;
};
-} // namespace armarx::armem
+
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae35822c6080701195c0e6d7d8b6c8c4ad282684
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -0,0 +1,68 @@
+/*
+ * 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::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::client::robot_state::localization
+{
+
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
+
+ class TransformInterface
+ {
+ public:
+ virtual ~TransformInterface() = default;
+
+ virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
+ virtual void connect() = 0;
+ };
+
+ class TransformReaderInterface : virtual public TransformInterface
+ {
+ public:
+ virtual ~TransformReaderInterface() = default;
+
+ virtual TransformResult getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const = 0;
+
+ virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
+ // waitForTransform()
+ };
+
+ class TransformWriterInterface : virtual public TransformInterface
+ {
+ public:
+ ~TransformWriterInterface() override = default;
+
+ virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
+ };
+
+} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8db83835ccc53f21e416b723745892a1455f801
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -0,0 +1,322 @@
+#include "TransformHelper.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/interpolate.h>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/Memory.h"
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
+namespace armarx::armem::common::robot_state::localization
+{
+
+ TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ const std::vector<std::string> tfChain =
+ buildTransformChain(localizationCoreSegment, query);
+
+ if (tfChain.empty())
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"};
+ }
+
+ const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ if (transforms.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
+
+ const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
+ transforms.end(),
+ Eigen::Affine3f::Identity(),
+ std::multiplies<>());
+
+ ARMARX_DEBUG << "Found valid transform";
+
+ return {.transform = {.header = query.header, .transform = transform},
+ .status = TransformResult::Status::Success};
+ }
+
+ std::vector<std::string>
+ TransformHelper::buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ ARMARX_DEBUG << "Building transform chain";
+
+ auto join = [](const std::string & parentFrame, const std::string & frame)
+ {
+ return parentFrame + "," + frame;
+ };
+
+ std::vector<std::string> chain;
+
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
+
+ auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
+ {
+ const auto entityName = join(parentFrame, frame);
+
+ if (agentProviderSegment.hasEntity(entityName))
+ {
+ chain.push_back(entityName);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ << "'";
+ }
+ };
+
+ std::array<std::string, 3> knownChain
+ {
+ GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+
+ auto* frameBeginIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
+ auto* const frameEndIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.frame);
+
+ if (frameBeginIt == knownChain.end())
+ {
+ ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
+ return {};
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
+ }
+
+ const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
+ ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
+ << "' across " << nFrames << " frames";
+
+ for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
+ {
+ addToChain(*frameBeginIt, *(frameBeginIt + 1));
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ addToChain(knownChain.back(), query.header.frame);
+ }
+
+ if (chain.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot create tf lookup chain '" << query.header.parentFrame
+ << " -> " << query.header.frame << "'";
+ return {};
+ }
+
+ return chain;
+ }
+
+ inline ::armarx::armem::robot_state::Transform
+ convertEntityToTransform(const armem::wm::EntityInstance& item)
+ {
+ arondto::Transform aronTransform;
+ aronTransform.fromAron(item.data());
+
+ ::armarx::armem::robot_state::Transform transform;
+ fromAron(aronTransform, transform);
+
+ return transform;
+ }
+
+ auto
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
+ const armem::Time& timestamp)
+ {
+ const auto comp = [](const armem::Time & timestamp, const auto & transform)
+ {
+ return transform.header.timestamp < timestamp;
+ };
+
+ const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
+
+ auto timestampBeyond =
+ [timestamp](const ::armarx::armem::robot_state::Transform & transform)
+ {
+ return transform.header.timestamp > timestamp;
+ };
+
+ const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
+
+ ARMARX_WARNING << "Fooasdfasdjfh";
+
+ ARMARX_CHECK(it == poseNextIt);
+
+ return poseNextIt;
+ }
+
+ Eigen::Affine3f
+ interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ armem::Time timestamp)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_DEBUG << "Entering";
+
+ ARMARX_CHECK(not queue.empty())
+ << "The queue has to contain at least two items to perform a lookup";
+
+ ARMARX_DEBUG << "Entering ... "
+ << "Q front " << queue.front().header.timestamp << " "
+ << "Q back " << queue.back().header.timestamp << " "
+ << "query timestamp " << timestamp;
+
+ // TODO(fabian.reister): sort queue.
+
+ ARMARX_CHECK(queue.back().header.timestamp > timestamp)
+ << "Cannot perform lookup into the future!";
+
+ // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
+ ARMARX_CHECK(queue.front().header.timestamp < timestamp)
+ << "Cannot perform lookup. Timestamp too old";
+ // => now we know that there is an element right after and before the timestamp within our queue
+
+ ARMARX_DEBUG << "Entering 2";
+
+ const auto poseNextIt = findFirstElementAfter(queue, timestamp);
+
+ ARMARX_DEBUG << "it ari";
+
+ const auto posePreIt = poseNextIt - 1;
+
+ ARMARX_DEBUG << "deref";
+
+ // the time fraction [0..1] of the lookup wrt to posePre and poseNext
+ const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
+ (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
+
+ ARMARX_DEBUG << "interpolate";
+
+ return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
+ }
+
+ std::vector<Eigen::Affine3f>
+ TransformHelper::obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp)
+ {
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
+
+ ARMARX_DEBUG << "Provider segments"
+ << simox::alg::get_keys(localizationCoreSegment.providerSegments());
+
+ ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
+
+ try
+ {
+ std::vector<Eigen::Affine3f> transforms;
+ transforms.reserve(tfChain.size());
+ std::transform(tfChain.begin(),
+ tfChain.end(),
+ std::back_inserter(transforms),
+ [&](const std::string & entityName)
+ {
+ return obtainTransform(
+ entityName, agentProviderSegment, timestamp);
+ });
+ return transforms;
+ }
+ catch (const armem::error::MissingEntry& missingEntryError)
+ {
+ ARMARX_WARNING << missingEntryError.what();
+ }
+ catch (const ::armarx::exceptions::local::ExpressionException& ex)
+ {
+ ARMARX_WARNING << "local exception: " << ex.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+ }
+
+
+
+ return {};
+ }
+
+ Eigen::Affine3f
+ TransformHelper::obtainTransform(const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ const armem::Time& timestamp)
+ {
+
+ ARMARX_DEBUG << "getEntity:" + entityName;
+ const auto& entity = agentProviderSegment.getEntity(entityName);
+
+ ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
+ << simox::alg::get_keys(entity.history());
+
+ // if (entity.history.empty())
+ // {
+ // // TODO(fabian.reister): fixme boom
+ // ARMARX_ERROR << "No snapshots received.";
+ // return Eigen::Affine3f::Identity();
+ // }
+
+ std::vector<::armarx::armem::robot_state::Transform> transforms;
+ transforms.reserve(entity.history().size());
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ std::transform(
+ entitySnapshots.begin(),
+ entitySnapshots.end(),
+ std::back_inserter(transforms),
+ [](const auto & entity)
+ {
+ return convertEntityToTransform(entity.getInstance(0));
+ });
+
+ ARMARX_DEBUG << "obtaining transform";
+
+ if (transforms.size() > 1)
+ {
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
+
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ const auto p = interpolateTransform(transforms, timestamp);
+ ARMARX_DEBUG << "Done interpolating transform";
+ return p;
+ }
+
+ // accept this to fail (will raise armem::error::MissingEntry)
+ if (transforms.empty())
+ {
+ ARMARX_DEBUG << "empty transform";
+
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ }
+
+ ARMARX_DEBUG << "single transform";
+
+ return transforms.front().transform;
+ }
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..8045c552e839b657c47dca5aabe6f76e0c384d0c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -0,0 +1,65 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+
+namespace armarx::armem::wm
+{
+ class CoreSegment;
+ class ProviderSegment;
+} // namespace armarx::armem::wm
+
+namespace armarx::armem::common::robot_state::localization
+{
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
+
+ class TransformHelper
+ {
+ public:
+ static TransformResult
+ lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ private:
+ static std::vector<std::string>
+ buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ static std::vector<Eigen::Affine3f>
+ obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp);
+
+ static Eigen::Affine3f
+ obtainTransform(const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
similarity index 50%
rename from source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
rename to source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index d3a9f288da15cefb71eb8bf5ecabf9bf7e250c4b..7f0c7c3396c7688ebe50b854579b107e9cd55b6f 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -22,48 +22,39 @@
#pragma once
+#include <Eigen/Geometry>
-// TODO(fabian.reister): remove
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx
+namespace armarx::armem::common::robot_state::localization
{
- class ManagedIceObject;
-}
-
-namespace armarx::armem
-{
-
- /**
- * @brief The MemoryConnector class simplifies connecting to the ArMem memory.
- *
- * Use this as the base class of any class that needs to connect to the memory.
- *
- *
- */
- class MemoryConnector
+ struct TransformResult
{
+ ::armarx::armem::robot_state::Transform transform;
+
+ enum class Status
+ {
+ Success,
+ Error,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
+
+ explicit operator bool() const
+ {
+ return status == Status::Success;
+ }
+
+ std::string errorMessage = "";
+ };
- public:
- MemoryConnector(ManagedIceObject& component);
- virtual ~MemoryConnector();
-
- protected:
- armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
- void registerPropertyDefinitions(PropertyDefinitionsPtr& def);
-
- void waitForMemory();
-
- virtual const std::string& getPropertyPrefix() const;
-
- private:
- ManagedIceObject& component;
-
- armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
+ struct TransformQuery
+ {
+ ::armarx::armem::robot_state::TransformHeader header;
- const std::string propertyPrefix;
+ // bool exact;
};
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73f17915af1c3497fb8b5627d629fd7b2373a90c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -0,0 +1,251 @@
+#include "Visu.h"
+
+#include <algorithm>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <exception>
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+#include "../localization/Segment.h"
+#include "../proprioception/Segment.h"
+#include "../description/Segment.h"
+
+namespace armarx::armem::server::robot_state
+{
+
+ void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.enabled, prefix + "enabled",
+ "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
+ "Frequency of visualization.");
+ }
+
+ void Visu::visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ )
+ {
+
+ const auto visualizeRobot = [&](const robot::Robot & robot)
+ {
+
+ const auto xmlPath = robot.description.xml.serialize();
+
+ // clang-format off
+ auto robotVisu = viz::Robot(robot.description.name)
+ // .file(xmlPath.package, xmlPath.path)
+ .file(xmlPath.package, xmlPath.path)
+ .joints(robot.config.jointMap)
+ .pose(robot.config.globalPose);
+
+ robotVisu.useFullModel();
+ // clang-format on
+
+ layer.add(robotVisu);
+ };
+
+ std::for_each(robots.begin(), robots.end(), visualizeRobot);
+
+ }
+
+ void Visu::init()
+ {
+
+ }
+
+ void Visu::connect(const viz::Client& arviz)
+ {
+ this->arviz = arviz;
+
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+ robot::Robots combine(const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
+ const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
+ const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ {
+
+ robot::Robots robots;
+
+ for (const auto& [robotName, robotDescription] : robotDescriptions)
+ {
+ const auto& globalPose = globalRobotPoseMap.find(robotName);
+ if (globalPose == globalRobotPoseMap.end())
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName << "'";
+ continue;
+ }
+
+ const auto& jointMap = robotJointPositionMap.find(robotName);
+ if (jointMap == robotJointPositionMap.end())
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '" << robotName << "'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": " << simox::alg::get_keys(jointMap->second);
+
+ // TODO(fabian.reister): based on data
+ const armem::Time timestamp = IceUtil::Time::now();
+
+ robots.emplace_back(
+ robot::Robot
+ {
+ .description = robotDescription,
+ .instance = "", // TODO(fabian.reister): set this properly
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = globalPose->second,
+ .jointMap = jointMap->second
+ },
+ .timestamp = timestamp,
+ }
+ );
+ }
+
+ return robots;
+
+ }
+
+
+ void Visu::visualizeRun()
+ {
+
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask && not updateTask->isStopped())
+ {
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_DEBUG << "Update task";
+
+ if (p.enabled)
+ {
+ TIMING_START(Visu);
+
+ // TODO(fabian.reister): use timestamp
+
+ const auto timestamp = IceUtil::Time::now();
+
+ try
+ {
+ const auto robotDescriptions = descriptionSegment.getRobotDescriptions(timestamp);
+ const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses(timestamp);
+ const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions(timestamp);
+
+
+ // we need all 3 informations:
+ // - robot description
+ // - global pose
+ // - joint positions
+ // => this is nothing but a armem::Robot
+
+ const robot::Robots robots = combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
+ viz::Layer layer = arviz.layer("Robots");
+
+ ARMARX_DEBUG << "visualizing robots";
+ visualizeRobots(layer, robots);
+
+ ARMARX_DEBUG << "Committing robots";
+
+ arviz.commit({layer});
+
+ ARMARX_DEBUG << "Done committing";
+
+
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+
+ }
+ catch (const std::exception& ex)
+ {
+ ARMARX_WARNING << ex.what();
+ continue;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Unknown exception";
+ continue;
+ }
+
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..dad4e49feb530109596a57fab7b52742b421e15c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -0,0 +1,123 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+
+namespace armarx::armem::server::robot_state
+{
+ namespace localization
+ {
+ class Segment;
+ }
+
+ namespace proprioception
+ {
+ class Segment;
+ }
+
+ namespace description
+ {
+ class Segment;
+ }
+
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ Visu(const description::Segment& descriptionSegment,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment)
+ : descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ void init();
+
+ void connect(const viz::Client& arviz);
+
+ protected:
+
+ static void visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ );
+
+
+ private:
+ viz::Client arviz;
+
+ const description::Segment& descriptionSegment;
+ const proprioception::Segment& proprioceptionSegment;
+ const localization::Segment& localizationSegment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+ void visualizeRun();
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::CheckBox enabled;
+
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..826c5b484768686e2edb8cee409df4136bcc85ab
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -0,0 +1,243 @@
+#include "Segment.h"
+
+#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <sstream>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <thread>
+
+namespace armarx::armem::server::robot_state::description
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+ std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ {
+ Logging::setTag("DescriptionSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment,
+ prefix + "seg.description.CoreSegment",
+ "Name of the robot description core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "seg.description.MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(
+ p.coreSegment, arondto::RobotDescription::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ robotUnit = robotUnitPrx;
+
+ // store the robot description linked to the robot unit in this segment
+ updateRobotDescription();
+ }
+
+ void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
+ coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toInitialAronType());
+
+ EntityUpdate update;
+ update.entityID = providerID.withEntityName("description");
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription dto;
+ robot::toAron(dto, robotDescription);
+ update.instancesData =
+ {
+ dto.toAron()
+ };
+
+ Commit commit;
+ commit.updates.push_back(update);
+
+ {
+ // std::lock_guard g{memoryMutex};
+ iceMemory.commit(commit);
+ }
+
+ }
+
+ void Segment::updateRobotDescription()
+ {
+ ARMARX_CHECK_NOT_NULL(robotUnit);
+
+ const auto waitForKinematicUnit = [&]()
+ {
+ while (true)
+ {
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+
+ if (kinematicUnit)
+ {
+ ARMARX_INFO << "Kinematic unit is now available.";
+ return kinematicUnit;
+ }
+
+ ARMARX_INFO << "Waiting for kinematic unit ...";
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ }
+ };
+
+ auto kinematicUnit = waitForKinematicUnit();
+ ARMARX_CHECK_NOT_NULL(kinematicUnit);
+
+ const auto robotName = kinematicUnit->getRobotName();
+ const auto robotFilename = kinematicUnit->getRobotFilename();
+
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+
+ const robot::RobotDescription robotDescription
+ {
+ .name = kinematicUnit->getRobotName(),
+ .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
+
+ storeRobotDescription(robotDescription);
+ }
+
+ Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
+ {
+
+ RobotDescriptionMap robotDescriptions;
+
+ {
+ // std::lock_guard g{memoryMutex};
+
+ for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto &[name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to "
+ "'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ robotDescriptions.emplace(description->name, *description);
+ }
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot descriptions: " << robotDescriptions.size();
+
+ return robotDescriptions;
+ }
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f603cf4961d90342fbd74770a0ef4d0d181c033
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -0,0 +1,114 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::description
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ /// mapping "robot name" -> "robot description"
+ using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+
+ RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
+
+ private:
+
+ void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void updateRobotDescription();
+
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ struct Properties
+ {
+ std::string coreSegment = "Description";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3137127aa112528fdf39351b9bfc52e4c4ece3b8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -0,0 +1,215 @@
+#include "Segment.h"
+
+// STL
+
+#include <iterator>
+#include <sstream>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+ std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ {
+ Logging::setTag("LocalizationSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment,
+ prefix + "seg.localization.CoreSegment",
+ "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "seg.localization.MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(
+ p.coreSegment, arondto::Transform::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+ {
+
+ using common::robot_state::localization::TransformHelper;
+ using common::robot_state::localization::TransformQuery;
+
+ std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
+
+ const auto& localizationCoreSegment =
+ iceMemory.workingMemory->getCoreSegment(p.coreSegment);
+ const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
+
+ for (const auto& robotName : knownRobots)
+ {
+
+ TransformQuery query{.header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }};
+
+ const auto result =
+ TransformHelper::lookupTransform(localizationCoreSegment, query);
+
+ if (not result)
+ {
+ // TODO
+ continue;
+ }
+
+ robotGlobalPoses.emplace(robotName, result.transform.transform);
+ }
+
+ ARMARX_INFO << deactivateSpam(10)
+ << "Number of known robot poses: " << robotGlobalPoses.size();
+
+ return robotGlobalPoses;
+ }
+
+ bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
+ {
+ const auto& timestamp = transform.header.timestamp;
+
+ const MemoryID providerID =
+ coreSegment->id().withProviderSegmentName(transform.header.agent);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
+
+ Commit commit;
+
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(transform.header.parentFrame + "," +
+ transform.header.frame);
+ update.timeArrived = update.timeCreated = update.timeSent = timestamp;
+
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
+ update.instancesData = {aronTransform.toAron()};
+
+ const armem::CommitResult result = iceMemory.commit(commit);
+ return result.allSuccess();
+ }
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..c23e9bcdce71f64288cec1a53c5c82d816327f92
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -0,0 +1,108 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
+
+ bool storeTransform(const armarx::armem::robot_state::Transform& transform);
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreSegment = "Localization";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ec63c12d9c3f6485839ff138b1ebe409dd1adc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -0,0 +1,168 @@
+#include "Segment.h"
+
+#include <mutex>
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem_robot_state/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/util/util.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ProprioceptionSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment, prefix + "seg.proprioception.CoreSegment", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
+ {
+ robotUnit = robotUnitPrx;
+
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+ const auto providerSegmentName = kinematicUnit->getRobotName();
+
+ // TODO what is the purpose?
+ auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
+ auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
+ auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
+ encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
+ encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
+ //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
+ //encoderEntryType->addMemberType("value", encoderValueType);
+
+ ARMARX_INFO << "Adding provider segment " << p.coreSegment << "/" << providerSegmentName;
+ armem::data::AddSegmentInput input;
+ input.coreSegmentName = p.coreSegment;
+ input.providerSegmentName = providerSegmentName;
+
+ {
+ std::lock_guard g{memoryMutex};
+ auto result = iceMemory.addSegments({input})[0];
+
+ if (!result.success)
+ {
+ ARMARX_ERROR << "Could not add segment " << p.coreSegment << "/" << providerSegmentName << ". The error message is: " << result.errorMessage;
+ }
+ }
+
+ robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
+ robotUnitProviderID.coreSegmentName = p.coreSegment;
+ robotUnitProviderID.providerSegmentName = providerSegmentName;
+ }
+
+ std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
+ {
+ std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+
+ for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
+
+ jointMap[robotName].emplace(jointState->name, jointState->position);
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
+
+ return jointMap;
+ }
+
+ const armem::MemoryID& Segment::getRobotUnitProviderID() const
+ {
+ return robotUnitProviderID;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc83a946bcadafbc85a15d55cff79c3a18af970f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
+
+ const armem::MemoryID& getRobotUnitProviderID() const;
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ armem::MemoryID robotUnitProviderID;
+
+ struct Properties
+ {
+ std::string coreSegment = "Proprioception";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..39daba5d2d7db0148cb9df264d8da2acac54d036
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+namespace armarx::armem::robot_state
+{
+ struct TransformHeader
+ {
+ std::string parentFrame;
+ std::string frame;
+
+ std::string agent;
+
+ armem::Time timestamp;
+ };
+
+ struct Transform
+ {
+ TransformHeader header;
+
+ Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ struct JointState
+ {
+ std::string name;
+ float position;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.cpp b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f4b84fdfbee87a4ca768837aa41039f1970115e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
@@ -0,0 +1,11 @@
+#include "utils.h"
+
+namespace armarx::armem::robot_state
+{
+ armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc)
+ {
+ return MemoryID("RobotState/Description")
+ .withProviderSegmentName(desc.name)
+ .withEntityName("description");
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.h b/source/RobotAPI/libraries/armem_robot_state/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..239e0ddb2d1b99a513ed97a3e1b8fb63122a4c70
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+
+namespace armarx::armem::robot_state
+{
+ armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
+}
+
diff --git a/source/RobotAPI/libraries/armem_skills/CMakeLists.txt b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bb6968ecb11991511f67f464843683a5bbacc8ef
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt
@@ -0,0 +1,31 @@
+set(LIB_NAME armem_skills)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::armem
+ SOURCES
+ ./aron_conversions.cpp
+ ./StatechartListener.cpp
+ HEADERS
+ ./aron_conversions.h
+ ./StatechartListener.h
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Statechart.xml
+)
+
+
+add_library(RobotAPI::armem_skills ALIAS armem_skills)
diff --git a/source/RobotAPI/libraries/armem_skills/StatechartListener.cpp b/source/RobotAPI/libraries/armem_skills/StatechartListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82d3369f3a18f88ec384a5f04677eb6deaea6d95
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/StatechartListener.cpp
@@ -0,0 +1,63 @@
+#include "StatechartListener.h"
+
+
+namespace armarx
+{
+ void StatechartListener::setName(const std::string& name)
+ {
+ armarx::Component::setName(name);
+ }
+
+ void StatechartListener::setTopicName(const std::string& name)
+ {
+ this->topicName = name;
+ }
+
+ std::string StatechartListener::getTopicName() const
+ {
+ return topicName;
+ }
+
+ StatechartListener::~StatechartListener() = default;
+
+ std::string StatechartListener::getDefaultName() const
+ {
+ return "StatechartListener";
+ }
+
+ void StatechartListener::onInitComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ usingTopic(topicName);
+ }
+ void StatechartListener::onConnectComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ }
+
+ void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+ {
+ callbacks.push_back(callback);
+ }
+
+ void StatechartListener::publish(const std::vector<Transition>& message)
+ {
+ for (Callback& callback : callbacks)
+ {
+ callback(message, *this);
+ }
+ }
+
+ void
+ StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
+ const Ice::Current&)
+ {
+ publish({transition});
+ }
+
+ void StatechartListener::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+ {
+ publish(transitions);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_skills/StatechartListener.h b/source/RobotAPI/libraries/armem_skills/StatechartListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0308773c3de18f0cced7f23325bab208e081898
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/StatechartListener.h
@@ -0,0 +1,65 @@
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+
+namespace armarx
+{
+ class StatechartListener :
+ virtual public armarx::Component
+ , virtual public armarx::ProfilerListener
+ {
+ public:
+ using Transition = armarx::ProfilerStatechartTransitionWithParameters;
+ using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+
+ public:
+ ~StatechartListener() override;
+
+ void setTopicName(const std::string& topicName);
+ std::string getTopicName() const;
+
+ void setName(const std::string& name);
+ void registerCallback(const Callback& callback);
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+ // ProfilerListener interface
+ public:
+ void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+
+ void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
+ void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
+ void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
+ void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
+ void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
+ void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
+
+ void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
+ void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
+ void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
+ void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
+ void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+
+
+ private:
+ std::string topicName;
+
+ std::vector<Callback> callbacks;
+ void publish(const std::vector<Transition>& message);
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml b/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c4b132e1a9867369e47ac3b6aad0c019c0f1f062
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml
@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+
+ <GenerateTypes>
+ <IntEnum name="armarx::armem::arondto::Statechart::StateType">
+ <EnumValue key="NORMAL" value="0" />
+ <EnumValue key="FINAL" value="1" />
+ <EnumValue key="REMOTE" value="2" />
+ <EnumValue key="DYNAMIC_REMOTE" value="3" />
+ <EnumValue key="UNDEFINED" value="4" />
+ </IntEnum>
+
+ <Object name='armarx::armem::arondto::Statechart::ParameterMap'>
+ <ObjectChild key='parameters'>
+ <dict>
+ <String />
+ </dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::arondto::Statechart::Transition'>
+ <ObjectChild key='processId'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key="sourceStateIdentifier">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="targetStateIdentifier">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="eventName">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="targetStateType">
+ <armarx::armem::arondto::Statechart::StateType />
+ </ObjectChild>
+
+ <ObjectChild key="inputParameters">
+ <armarx::armem::arondto::Statechart::ParameterMap />
+ </ObjectChild>
+
+ <ObjectChild key="localParameters">
+ <armarx::armem::arondto::Statechart::ParameterMap />
+ </ObjectChild>
+
+ <ObjectChild key="outputParameters">
+ <armarx::armem::arondto::Statechart::ParameterMap />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..db18f5ac2a93db5a20b5103aff547e607ff6c29d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
@@ -0,0 +1,88 @@
+#include "aron_conversions.h"
+
+namespace armarx::armem
+{
+ std::map<armarx::eStateType, armarx::armem::arondto::Statechart::StateType> toAronStateTypeMap =
+ {
+ {eNormalState, armarx::armem::arondto::Statechart::StateType::NORMAL},
+ {eFinalState, armarx::armem::arondto::Statechart::StateType::FINAL},
+ {eRemoteState, armarx::armem::arondto::Statechart::StateType::REMOTE},
+ {eDynamicRemoteState, armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE},
+ {eUndefined, armarx::armem::arondto::Statechart::StateType::UNDEFINED},
+ };
+
+ std::map<armarx::armem::arondto::Statechart::StateType, armarx::eStateType> fromAronStateTypeMap =
+ {
+ {armarx::armem::arondto::Statechart::StateType::NORMAL, eNormalState},
+ {armarx::armem::arondto::Statechart::StateType::FINAL, eFinalState},
+ {armarx::armem::arondto::Statechart::StateType::REMOTE, eRemoteState},
+ {armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE, eDynamicRemoteState},
+ {armarx::armem::arondto::Statechart::StateType::UNDEFINED, eUndefined},
+ };
+
+ void fromAron(const arondto::Statechart::StateType& dto, eStateType& bo)
+ {
+ if (fromAronStateTypeMap.find(dto) != fromAronStateTypeMap.end())
+ {
+ bo = fromAronStateTypeMap[dto];
+ }
+ else
+ {
+ bo = eStateType::eUndefined;
+ }
+ }
+
+ void toAron(arondto::Statechart::StateType& dto, const eStateType& bo)
+ {
+ if (toAronStateTypeMap.find(bo) != toAronStateTypeMap.end())
+ {
+ dto.value = toAronStateTypeMap[bo].value;
+ }
+ else
+ {
+ dto.value = arondto::Statechart::StateType::UNDEFINED;
+ }
+ }
+
+ void fromAron(const arondto::Statechart::ParameterMap& dto, StateParameterMap& bo)
+ {
+ // todo: implement
+ // for (auto const& [key, val] : dto.parameters)
+ // {
+ // fromAron(val, ...)
+ // bo.insert(key, val);
+ // }
+ }
+
+ void toAron(arondto::Statechart::ParameterMap& dto, const StateParameterMap& bo)
+ {
+ for (auto const& [key, val] : bo)
+ {
+ dto.parameters[key] = val->value->toString();
+ }
+ }
+
+ void fromAron(const arondto::Statechart::Transition& dto, ProfilerStatechartTransitionWithParameters& bo)
+ {
+ bo.processId = dto.processId;
+ bo.sourceStateIdentifier = dto.sourceStateIdentifier;
+ bo.targetStateIdentifier = dto.targetStateIdentifier;
+ bo.eventName = dto.eventName;
+ fromAron(dto.targetStateType, bo.targetStateType);
+ fromAron(dto.inputParameters, bo.inputParameters);
+ fromAron(dto.localParameters, bo.localParameters);
+ fromAron(dto.outputParameters, bo.outputParameters);
+ }
+
+ void toAron(arondto::Statechart::Transition& dto, const ProfilerStatechartTransitionWithParameters& bo)
+ {
+ dto.processId = bo.processId;
+ dto.sourceStateIdentifier = bo.sourceStateIdentifier;
+ dto.targetStateIdentifier = bo.targetStateIdentifier;
+ dto.eventName = bo.eventName;
+ toAron(dto.targetStateType, bo.targetStateType);
+ toAron(dto.inputParameters, bo.inputParameters);
+ toAron(dto.localParameters, bo.localParameters);
+ toAron(dto.outputParameters, bo.outputParameters);
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.h b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc6314b4613d8ed0d1fa0f49188a51aad47518ca
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+
+namespace armarx::armem
+{
+ void fromAron(const armarx::armem::arondto::Statechart::StateType& dto, armarx::eStateType& bo);
+ void toAron(armarx::armem::arondto::Statechart::StateType& dto, const armarx::eStateType& bo);
+
+ void fromAron(const armarx::armem::arondto::Statechart::ParameterMap& dto, armarx::StateParameterMap& bo);
+ void toAron(armarx::armem::arondto::Statechart::ParameterMap& dto, const armarx::StateParameterMap& bo);
+
+ void fromAron(const armarx::armem::arondto::Statechart::Transition& dto, armarx::ProfilerStatechartTransitionWithParameters& bo);
+ void toAron(armarx::armem::arondto::Statechart::Transition& dto, const armarx::ProfilerStatechartTransitionWithParameters& bo);
+}
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 7a4f383eb5118009de6cfb43299490e5fd8b83ab..ec6c74d9e669f9effb5825afda1cc1645aadf547 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -17,12 +17,14 @@ armarx_add_library(
aron_conversions/armarx.h
aron_conversions/simox.h
aron_conversions/stl.h
+ aron_conversions/eigen.h
SOURCES
aron_conversions/core.cpp
aron_conversions/armarx.cpp
aron_conversions/simox.cpp
aron_conversions/stl.cpp
+ aron_conversions/eigen.cpp
)
@@ -33,6 +35,7 @@ armarx_enable_aron_file_generation_for_target(
aron/AxisAlignedBoundingBox.xml
aron/OrientedBox.xml
aron/PackagePath.xml
+
)
diff --git a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
index c4a4e407fd7ce3aca8cdca5aacc991f05645023f..c7527c2cfc3e862f45bf5f69a039b27c96eb5d50 100644
--- a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
@@ -5,6 +5,7 @@ The ARON DTO of simox::OrientedBoxf.
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
</CodeIncludes>
<GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
index 2f96d0285b9fb4dbd96abdb92c12fe9518ee88b6..b771ef362aaf0a886aaabcc19027e74a9dfbeb9b 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -5,3 +5,4 @@
#include "aron_conversions/armarx.h"
#include "aron_conversions/simox.h"
#include "aron_conversions/stl.h"
+#include "aron_conversions/eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index da205a5238351e06ee78224198c66e6d37151df9..2cd6b7eb906d4b85cf6527979eb9573f4d9568ea 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -1,14 +1,34 @@
#include "armarx.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
-void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackagePath& bo)
+namespace armarx
{
- 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;
-}
+ /* PackagePath */
+
+ void fromAron(const arondto::PackagePath& dto, PackagePath& bo)
+ {
+ bo = {dto.package, dto.path};
+ }
+
+ void toAron(arondto::PackagePath& dto, const PackagePath& bo)
+ {
+ const data::PackagePath icedto = bo.serialize();
+ dto.package = icedto.package;
+ dto.path = icedto.path;
+ }
+
+ void fromAron(const long& dto, IceUtil::Time& bo)
+ {
+ bo = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& bo)
+ {
+ dto = bo.toMicroSeconds();
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index 241067b7b70ef2f0016c3bfbd33e7a5beff2624b..48c24690503cec91a17da37909a53c3228984e26 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -1,11 +1,17 @@
#pragma once
#include <ArmarXCore/core/PackagePath.h>
-#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+#include <IceUtil/Time.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);
-}
+
+ void fromAron(const long& dto, IceUtil::Time& bo);
+ void toAron(long& dto, const IceUtil::Time& bo);
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0bf14ec90439e50a4869234b81449adaf068d173
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -0,0 +1,17 @@
+#include "eigen.h"
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo)
+ {
+ bo.matrix() = dto;
+ }
+
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo)
+ {
+ dto = bo.matrix();
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f091dbfc40ee8e91b8d2922687273269ce790a8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
index 55a30cff91afbd74bbf4cbae769df34992843096..e99a7f91658f88611d9884d9795fb8e36b2a8485 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
@@ -41,6 +41,33 @@ namespace armarx::aron::visitor
return true;
}
+ template <class ToT, class FromT>
+ ToT cast(const FromT& from)
+ {
+ ToT to;
+ to.setValue(from.getValue());
+ return to;
+ }
+ template <class FromDataT>
+ std::optional<datanavigator::LongNavigator> castToLong(const FromDataT& data)
+ {
+ if (auto d = dynamic_cast<const datanavigator::IntNavigator*>(&data))
+ {
+ return cast<datanavigator::LongNavigator>(*d);
+ }
+ return std::nullopt;
+ }
+ template <class FromDataT>
+ std::optional<datanavigator::IntNavigator> castToInt(const FromDataT& data)
+ {
+ if (auto d = dynamic_cast<const datanavigator::LongNavigator*>(&data))
+ {
+ return cast<datanavigator::IntNavigator>(*d);
+ }
+ return std::nullopt;
+ }
+
+
bool TypedDataVisitor::applyTo(const std::string& key, TypeNavigator& type, DataNavigator& data)
{
if (auto t = dynamic_cast<ObjectTypeNavigator*>(&type))
@@ -63,70 +90,103 @@ namespace armarx::aron::visitor
ARMARX_CHECK_EQUAL(type.childrenSize(), 0) << simox::meta::get_type_name(type);
ARMARX_CHECK_EQUAL(data.childrenSize(), 0) << simox::meta::get_type_name(data);
- if (auto t = dynamic_cast<BoolTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<BoolDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<DoubleTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<DoubleDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<FloatTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<FloatDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<IntTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<IntDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<LongTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<StringTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ try
{
- return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
- }
- 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));
- }
- else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- 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));
+ if (auto t = dynamic_cast<BoolTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<BoolDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<DoubleTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<DoubleDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<FloatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<FloatDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<IntTypeNavigator*>(&type))
+ {
+ if (auto cast = castToInt(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<IntDataNavigator&>(data));
+ }
+ }
+ else if (auto t = dynamic_cast<LongTypeNavigator*>(&type))
+ {
+ if (auto cast = castToLong(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
+ }
+ else if (auto t = dynamic_cast<StringTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ {
+ if (auto cast = castToLong(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
+ }
+
+ 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));
+ }
+ else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ 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));
+ }
+
+ ARMARX_CHECK(false) << "Unhandled AronTypeNavigatorType '" << simox::meta::get_type_name(type) << "'."
+ << "\n(Data: '" << simox::meta::get_type_name(data) << "')";
}
- else if (auto t = dynamic_cast<OrientationTypeNavigator*>(&type))
+ catch (const std::bad_cast& e)
{
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ std::stringstream msg;
+ msg << "Got ARON data '" << simox::meta::get_type_name(data) << "'"
+ << " incompatible to ARON type '" << simox::meta::get_type_name(type) << "'"
+ << " (got std::bad_cast: '" << e.what() << "').";
+ throw error::AronException("TypedDataVisitor", __FUNCTION__, msg.str());
}
-
- ARMARX_CHECK(false) << "Unhandled AronTypeNavigatorType '" << simox::meta::get_type_name(type) << "'."
- << "\n(Data: '" << simox::meta::get_type_name(data) << "')";
}
bool TypedDataVisitor::applyToChildren(ObjectTypeNavigator& type, DictDataNavigator& data)