diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
new file mode 100644
index 0000000000000000000000000000000000000000..42330dcb254c68f07b3222f8f087f92659228fb1
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemObjectMemory" creation="2021-04-22.11:29:22" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemObjectMemory/config/ArVizStorage.cfg b/scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0dcbd8d5775b8c0f562bba62b5f2fe282e2da6d0
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage 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.ArVizStorage.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.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history are serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.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.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# 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/DebugObserver.cfg b/scenarios/ArMemObjectMemory/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver 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.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.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.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.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.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# 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/MemoryNameSystem.cfg b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f8b485477ed596fc7cbbe003e9aebf9371d23e0d
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -0,0 +1,455 @@
+# ==================================================================
+# ObjectMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# 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.ObjectMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectMemory.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.EnableProfiling = false
+
+
+# ArmarX.ObjectMemory.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.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
+
+
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
+# Attributes:
+# - Default: KinematicUnitObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+
+
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.MemoryName = Object
+
+
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Instance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName = Instance
+
+
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
+# Attributes:
+# - Default: Neck_2_Pitch
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
+
+
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# Attributes:
+# - Default: 0.0500000007
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# 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/ObjectPoseProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..45075e6114eae82fe89f50317c0bced824c96b46
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
@@ -0,0 +1,213 @@
+# ==================================================================
+# ObjectPoseProviderExample 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.ObjectPoseProviderExample.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.ObjectPoseProviderExample.EnableProfiling = false
+
+
+# ArmarX.ObjectPoseProviderExample.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.ObjectPoseProviderExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectPoseProviderExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseProviderExample.ObjectName = ""
+
+
+# ArmarX.ObjectPoseProviderExample.ObjectPoseTopicName: Name of the object pose topic.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseProviderExample.ObjectPoseTopicName = ObjectPoseTopic
+
+
+# ArmarX.ObjectPoseProviderExample.Objects: Object IDs of objects to be tracked.
+# Attributes:
+# - Default: KIT/Amicelli, KIT/YellowSaltCylinder
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {KIT/Amicelli, KIT/YellowSaltCylinder}
+# ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, KIT/YellowSaltCylinder
+
+
+# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# 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/RemoteGuiProviderApp.cfg b/scenarios/ArMemObjectMemory/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/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/ArMemObjectMemory/config/RobotStateComponent.cfg b/scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..2755127da3115e43c83b7d3e946a31020f3b71af
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
@@ -0,0 +1,324 @@
+# ==================================================================
+# RobotStateComponent 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.RobotStateComponent.AgentName: Name of the agent for which the sensor values are provided
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.RobotStateComponent.AgentName = ::_NOT_SET_::
+
+
+# ArmarX.RobotStateComponent.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.RobotStateComponent.EnableProfiling = false
+
+
+# ArmarX.RobotStateComponent.GlobalRobotPoseLocalizationTopicName: Topic where the global robot pose can be reported.
+# Attributes:
+# - Default: GlobalRobotPoseLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.GlobalRobotPoseLocalizationTopicName = GlobalRobotPoseLocalization
+
+
+# ArmarX.RobotStateComponent.HistoryLength: Number of entries in the robot state history
+# Attributes:
+# - Default: 10000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.HistoryLength = 10000
+
+
+# ArmarX.RobotStateComponent.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.RobotStateComponent.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateComponent.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.ObjectName = ""
+
+
+# ArmarX.RobotStateComponent.PlatformTopicName: Topic where platform state is published.
+# Attributes:
+# - Default: PlatformState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.PlatformTopicName = PlatformState
+
+
+# ArmarX.RobotStateComponent.RobotFileName: Filename of VirtualRobot robot model (e.g. robot_model.xml)
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.RobotStateComponent.RobotFileName = ::_NOT_SET_::
+
+
+# ArmarX.RobotStateComponent.RobotModelScaling: Scaling of the robot model
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.RobotModelScaling = 1
+
+
+# ArmarX.RobotStateComponent.RobotNodeSetName: Set of nodes that is controlled by the KinematicUnit
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.RobotStateComponent.RobotNodeSetName = ::_NOT_SET_::
+
+
+# ArmarX.RobotStateComponent.RobotStateReportingTopic: Name of the topic on which updates of the robot state are reported.
+# Attributes:
+# - Default: RobotStateUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.RobotStateReportingTopic = RobotStateUpdates
+
+
+# ArmarX.RobotStateComponent.TopicPrefix: Prefix for the sensor value topic name.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateComponent.TopicPrefix = ""
+
+
+# ArmarX.RobotStateComponent.inheritFrom: No Description
+# Attributes:
+# - Default: RobotConfig
+# - Case sensitivity: no
+# - Required: no
+ArmarX.RobotStateComponent.inheritFrom = RobotConfig
+
+
+# ArmarX.RobotStateObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.RobotStateObserver.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.RobotStateObserver.EnableProfiling = false
+
+
+# ArmarX.RobotStateObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.RobotStateObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.MaxHistorySize = 5000
+
+
+# ArmarX.RobotStateObserver.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.RobotStateObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.ObjectName = ""
+
+
+# ArmarX.RobotStateObserver.TCPsToReport: comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateObserver.TCPsToReport = ""
+
+
+# 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/RobotToArVizApp.cfg b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b023833a9d18ac3f4c1d92911b29ad1f8699a22c
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
@@ -0,0 +1,238 @@
+# ==================================================================
+# RobotToArVizApp 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.RobotToArViz.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotToArViz.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.RobotToArViz.EnableProfiling = false
+
+
+# ArmarX.RobotToArViz.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.RobotToArViz.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotToArViz.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ObjectName = ""
+
+
+# ArmarX.RobotToArViz.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RobotToArViz.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.RobotToArViz.ShowRobotNodeFrames: If true, show frames of robot nodes (can be changed in RemoteGui).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotToArViz.ShowRobotNodeFrames = false
+
+
+# ArmarX.RobotToArViz.UseCollisionModel: Use the collision model for visualization
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotToArViz.UseCollisionModel = false
+
+
+# ArmarX.RobotToArViz.updateFrequency: Target number of updates per second.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.updateFrequency = 100
+
+
+# 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/global.cfg b/scenarios/ArMemObjectMemory/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e6ba906d45b1ddbf6685ef3822da3fa3a2e76056
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/global.cfg
@@ -0,0 +1,34 @@
+# ==================================================================
+# Global Config from Scenario ArMemObjectMemory
+# ==================================================================
+
+# RobotConfig.AgentName:
+# Attributes:
+RobotConfig.AgentName = Armar6
+
+
+# RobotConfig.ForceTorqueTopicName:
+# Attributes:
+RobotConfig.ForceTorqueTopicName = Armar6ForceTorqueUnitState
+
+
+# RobotConfig.KinematicUnitName:
+# Attributes:
+RobotConfig.KinematicUnitName = Armar6KinematicUnit
+
+
+# RobotConfig.PlatformName:
+# Attributes:
+RobotConfig.PlatformName = Platform
+
+
+# RobotConfig.RobotFileName:
+# Attributes:
+RobotConfig.RobotFileName = Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml
+
+
+# RobotConfig.RobotNodeSetName:
+# Attributes:
+RobotConfig.RobotNodeSetName = Robot
+
+
diff --git a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg b/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
index c312408a4c48cd520d4a3797e42286b1b40c6b74..ff6a75df675b99f7e7c157fabe1b32026da9af96 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
+++ b/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
@@ -100,15 +100,6 @@
# ArmarX.ObjectPoseObserver.ArVizTopicName = ArVizTopic
-# ArmarX.ObjectPoseObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.CreateUpdateFrequenciesChannel = false
-
-
# ArmarX.ObjectPoseObserver.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -126,22 +117,6 @@
# ArmarX.ObjectPoseObserver.KinematicUnitObserverName = KinematicUnitObserver
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
-# Attributes:
-# - Default: 50
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency = 50
-
-
-# ArmarX.ObjectPoseObserver.MaxHistorySize: Maximum number of entries in the Observer history
-# Attributes:
-# - Default: 5000
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistorySize = 5000
-
-
# ArmarX.ObjectPoseObserver.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
@@ -273,6 +248,16 @@
# ArmarX.ObjectPoseObserver.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectPoseObserver.mem.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseObserver.mem.DiscardSnapshotsWhileAttached = true
+
+
# ArmarX.ObjectPoseObserver.mem.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
# Attributes:
# - Default: -1
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 7e1ec4053f83a4d33bb323bd129ff87f0d7b703a..5a21a1cd4e02935799bdd4788aa7e2cac5ca12c8 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -89,6 +89,9 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
+add_library(RobotAPI::ArViz ALIAS ArViz)
+
+
armarx_component_set_name("ArVizStorage")
set(COMPONENT_LIBS
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index cec5e53f76ea3d9d9a52745e3e50923e7ca01b2d..612dc821619309383956439639faa370ba473be0 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -18,8 +18,6 @@ add_subdirectory(KITHandUnit)
add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
add_subdirectory(NaturalIKTest)
-add_subdirectory(ObjectPoseObserver)
-add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(RobotHealth)
add_subdirectory(RobotNameService)
add_subdirectory(RobotState)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
deleted file mode 100644
index 7451e6f55845a9d94db43fb789d63a68361a1f1c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ /dev/null
@@ -1,49 +0,0 @@
-armarx_component_set_name("ObjectPoseObserver")
-
-
-set(COMPONENT_LIBS
- # ArmarXCore
- ArmarXCore ArmarXCoreInterfaces
- # ArmarXGui
- ArmarXGuiComponentPlugins
- # RobotAPI
- RobotAPIArmarXObjects RobotAPIComponentPlugins
- ArViz armem
-
- # This project
- ${PROJECT_NAME}Interfaces
-)
-
-set(SOURCES
- ObjectPoseObserver.cpp
-
- detail/Data.cpp
- detail/Decay.cpp
- detail/RobotHeadMovement.cpp
- detail/Visu.cpp
-
- plugins/ObjectPoseProviderPlugin.cpp
- plugins/ObjectPoseClientPlugin.cpp
- plugins/RequestedObjects.cpp
-)
-set(HEADERS
- ObjectPoseObserver.h
-
- detail/Data.h
- detail/Decay.h
- detail/RobotHeadMovement.h
- detail/Visu.h
-
- plugins/ObjectPoseProviderPlugin.h
- plugins/ObjectPoseClientPlugin.h
- plugins/RequestedObjects.h
-)
-
-armarx_add_component("${SOURCES}" "${HEADERS}")
-
-
-# add unit tests
-add_subdirectory(test)
-
-# generate the application
-armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
deleted file mode 100644
index 40a529a910c6622055880b2ada42d632ace774fb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ /dev/null
@@ -1,605 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "ObjectPoseObserver.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-
-
-namespace armarx
-{
-
- armarx::PropertyDefinitionsPtr ObjectPoseObserver::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr defs(new ObserverPropertyDefinitions(getConfigIdentifier()));
-
- defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
- defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
- defs->topic(debugObserver);
-
- calibration.defineProperties(defs, "calibration.");
- data.defineProperties(defs);
- robotHead.defineProperties(defs, "head.");
- visu.defineProperties(defs, "visu.");
-
- return defs;
- }
-
- ObjectPoseObserver::ObjectPoseObserver() :
- data(memory, iceMemory)
- {
- }
-
- std::string ObjectPoseObserver::getDefaultName() const
- {
- return "ObjectPoseObserver";
- }
-
- void ObjectPoseObserver::onInitObserver()
- {
- data.setTag(getName());
- data.decay.setTag(getName());
- robotHead.setTag(getName());
- visu.setTag(getName());
-
- data.init();
-
- usingTopicFromProperty("ObjectPoseTopicName");
- }
-
- void ObjectPoseObserver::onConnectObserver()
- {
- // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
- // So we need to always make sure to guard a call to addRobot
- if (!RobotState::hasRobot("robot"))
- {
- data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
- }
- data.robotStateComponent = getRobotStateComponent();
-
- getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
- robotHead.debugObserver = debugObserver;
- robotHead.fetchDatafields();
-
- visu.arviz = arviz;
- if (!visu.updateTask)
- {
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- visu.updateTask->start();
- }
-
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
-
- void ObjectPoseObserver::onDisconnectComponent()
- {
- }
-
- void ObjectPoseObserver::onExitObserver()
- {
- }
-
-
- void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- {
- updateProviderInfo(providerName, info);
- }
-
-
- void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- {
- ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
- updateObjectPoses(providerName, providedPoses);
- }
-
-
- void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
- {
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
- {
- std::scoped_lock lock(dataMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- data.providers[providerName] = info;
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType),
- "The object type (known or unknown)");
- offerOrUpdateDataField(providerName, "numSupportedObjects", int(info.supportedObjects.size()),
- "Number of requestable objects.");
- }
-
-
- void ObjectPoseObserver::updateObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses)
- {
- TIMING_START(ReportObjectPoses);
-
- std::optional<IceUtil::Time> discardUpdatesUntil;
- bool discardAll = false;
- {
- std::scoped_lock lock(robotHeadMutex);
- if (robotHead.checkHeadVelocity)
- {
- if (robotHead.isMoving())
- {
- robotHead.movementStarts(robotHead.discardIntervalAfterMoveMS);
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << robotHead.discardUpdatesUntil;
- discardAll = true;
- }
- else if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
- {
- discardAll = true;
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << robotHead.discardUpdatesUntil;
- }
- else
- {
- discardUpdatesUntil = robotHead.discardUpdatesUntil;
- }
- }
- }
- if (debugObserver)
- {
- StringVariantBaseMap map;
- map["Discarding All Updates"] = new Variant(discardAll ? 1.f : 0.f);
- if (discardAll)
- {
- map["Proportion Updated Poses"] = new Variant(0.f);
- }
- debugObserver->setDebugChannel(getName(), map);
- }
-
- if (discardAll)
- {
- return;
- }
-
- {
- std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone(data.robot);
-
- if (data.robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = data.robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
-
- // This stays empty on the first report.
- const objpose::ObjectPoseSeq previousPoses = data.getStoredObjectPoses(providerName);
-
- // Build new poses.
- objpose::ObjectPoseSeq newObjectPoses;
- int numUpdated = 0;
- for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
- {
- IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
-
- // Check whether we have an old pose for this object.
- std::optional<objpose::ObjectPose> previousPose;
- for (const objpose::ObjectPose& prev : previousPoses)
- {
- if (prev.objectID == fromIce(provided.objectID))
- {
- previousPose = prev;
- }
- }
-
- if (discardUpdatesUntil && timestamp < *discardUpdatesUntil)
- {
- if (previousPose)
- {
- // Keep the old one
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- // Discard the new pose.
- // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
- // << "timestamp " << timestamp << " < " << robotHead.discardUpdatesUntil;
- }
- }
- else if (previousPose && timestamp == previousPose->timestamp)
- {
- // Keep the old one.
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- numUpdated++;
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- newPose.fromProvidedPose(provided, data.robot);
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(newPose.objectID))
- {
- newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
- }
- }
- if (!provided.localOOBB)
- {
- // Try to load oobb from disk.
- newPose.localOOBB = data.getObjectOOBB(newPose.objectID);
- }
- }
- }
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Discarding All Updates", new Variant(discardAll ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(numUpdated) / providedPoses.size()) }
- });
- }
-
- TIMING_START(MemorySetObjectPoses);
- {
- std::scoped_lock lock(memoryMutex);
- data.setObjectPoses(providerName, newObjectPoses);
- }
- TIMING_END_STREAM(MemorySetObjectPoses, ARMARX_VERBOSE);
- handleProviderUpdate(providerName);
-
- TIMING_END_STREAM(ReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "ReportObjectPoses [ms]", new Variant(ReportObjectPoses.toMilliSecondsDouble()) },
- { "MemorySetObjectPoses [ms]", new Variant(MemorySetObjectPoses.toMilliSecondsDouble()) },
- });
- }
- }
- }
-
-
- void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- if (data.providers.count(providerName) == 0)
- {
- data.providers[providerName] = objpose::ProviderInfo();
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectCount", Variant(int(data.getStoredObjectPoses(providerName).size())), "Number of provided object poses.");
- }
-
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPoses(now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPoses() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPoses() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPosesByProvider(providerName, now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
-
- objpose::observer::RequestObjectsOutput ObjectPoseObserver::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- {
- std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- objpose::observer::RequestObjectsOutput output;
-
- auto updateProxy = [&](const std::string & providerName)
- {
- if (proxies.count(providerName) == 0)
- {
- if (auto it = data.providers.find(providerName); it != data.providers.end())
- {
- proxies[providerName] = it->second.proxy;
- }
- else
- {
- ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- proxies[providerName] = nullptr;
- }
- }
- };
-
- if (input.provider.size() > 0)
- {
- providerRequests[input.provider] = input.request;
- updateProxy(input.provider);
- }
- else
- {
- std::scoped_lock lock(dataMutex);
- for (const auto& objectID : input.request.objectIDs)
- {
- bool found = true;
- for (const auto& [providerName, info] : data.providers)
- {
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- {
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
- }
- }
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- }
-
- for (const auto& [providerName, request] : providerRequests)
- {
- if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- {
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- int successful = 0;
- for (const auto& [objectID, result] : providerOutput.results)
- {
- objpose::observer::ObjectRequestResult& res = output.results[objectID];
- res.providerName = providerName;
- res.result = result;
- successful += int(result.success);
- }
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- }
- }
- return output;
- }
-
- objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers;
- }
-
- Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return simox::alg::get_keys(data.providers);
- }
-
- objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.getProviderInfo(providerName);
- }
-
- bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers.count(providerName) > 0;
- }
-
-
- objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.attachObjectToRobotNode(input);
- }
-
- objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachObjectFromRobotNode(input);
- }
-
- objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachAllObjectsFromRobotNodes();
- }
-
-
- objpose::AgentFramesSeq ObjectPoseObserver::getAttachableFrames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { data.robot };
- for (VirtualRobot::RobotPtr agent : agents)
- {
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- }
-
- objpose::SignalHeadMovementOutput
- ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(robotHeadMutex);
- return robotHead.signalHeadMovement(input);
- }
-
-
- void ObjectPoseObserver::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- while (visu.updateTask && !visu.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(visuMutex);
-
- if (visu.enabled)
- {
- TIMING_START(Visu);
-
- objpose::ObjectPoseSeq objectPoses;
- ObjectFinder objectFinder;
- visu.minConfidence = -1;
- {
- std::scoped_lock lock(dataMutex);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- objectPoses = data.getObjectPoses(now);
- objectFinder = data.objectFinder;
- if (data.decay.enabled)
- {
- visu.minConfidence = data.decay.removeObjectsBelowConfidence;
- }
- }
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
- arviz.commit(layers);
-
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
- }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
- {
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
- }
-
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- tab.visu.setup(this->visu);
- tab.decay.setup(this->data.decay);
- tab.robotHead.setup(this->robotHead);
-
- VBoxLayout root = {tab.visu.group, tab.decay.group, tab.robotHead.group, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- {
- std::scoped_lock lock(visuMutex);
- tab.visu.update(this->visu);
- }
- {
- std::scoped_lock lock(robotHeadMutex);
- tab.robotHead.update(this->robotHead);
- }
- {
- std::scoped_lock lock(dataMutex);
- tab.decay.update(this->data.decay);
- }
- }
-
- void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-}
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
deleted file mode 100644
index 2e85e946a5c4216694f0f85b12a1fd8647a6eb56..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ /dev/null
@@ -1,186 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-
-#include <ArmarXCore/observers/Observer.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-
-#include <RobotAPI/components/ObjectPoseObserver/detail/Data.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Decay.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Visu.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
-
-
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
-
-namespace armarx
-{
-
- /**
- * @defgroup Component-ObjectPoseObserver ObjectPoseObserver
- * @ingroup RobotAPI-Components
- * A description of the component ObjectPoseObserver.
- *
- * @class ObjectPoseObserver
- * @ingroup Component-ObjectPoseObserver
- * @brief Brief description of class ObjectPoseObserver.
- *
- * Detailed description of class ObjectPoseObserver.
- */
- class ObjectPoseObserver :
- virtual public Observer
- , virtual public armarx::objpose::ObjectPoseObserverInterface
- , virtual public armarx::armem::server::ComponentPluginUser
- , virtual public armarx::RobotStateComponentPluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- {
- class Data;
-
- public:
- using RobotState = armarx::RobotStateComponentPluginUser;
-
- ObjectPoseObserver();
-
-
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
-
-
- // ObjectPoseTopic interface
- public:
- void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
-
- // ObjectPoseObserverInterface interface
- public:
-
- // OBJECT POSES
-
- objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
-
- // PROVIDER INFORMATION
-
- bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
- Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
- objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
-
-
- // REQUESTING
-
- objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
- // ATTACHING
-
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(ICE_CURRENT_ARG) override;
-
- objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
-
- // HEAD MOVEMENT SIGNALS
-
- objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
-
-
-
- // Remote GUI
- void createRemoteGuiTab();
- void RemoteGui_update() override;
-
-
- protected:
-
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- void onInitObserver() override;
- void onConnectObserver() override;
-
- void onDisconnectComponent() override;
- void onExitObserver() override;
-
-
- private:
-
- void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
-
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
- void handleProviderUpdate(const std::string& providerName);
-
- objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
-
-
-
- // Visualization
-
- void visualizeRun();
-
-
- private:
-
- DebugObserverInterfacePrx debugObserver;
-
- objpose::observer::Data data;
- std::mutex dataMutex;
-
- objpose::observer::RobotHeadMovement robotHead;
- std::mutex robotHeadMutex;
-
- objpose::observer::Visu visu;
- std::mutex visuMutex;
-
-
- struct Calibration
- {
- std::string robotNode = "Neck_2_Pitch";
- float offset = 0.0f;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
- };
- Calibration calibration;
-
-
- struct RemoteGuiTab : RemoteGui::Client::Tab
- {
- objpose::observer::Visu::RemoteGui visu;
- objpose::observer::Decay::RemoteGui decay;
- objpose::observer::RobotHeadMovement::RemoteGui robotHead;
- };
- RemoteGuiTab tab;
-
- };
-
-}
-
-#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
deleted file mode 100644
index 96c5afad46d8fb8edf959bd5c7c1178f4ad956ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
+++ /dev/null
@@ -1,445 +0,0 @@
-#include "Data.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <sstream>
-
-
-namespace armarx::objpose::observer
-{
-
- const std::string Data::defaultMemoryName = "Object";
-
- Data::Data(armem::Memory& memory, armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- memory(memory),
- memoryToIceAdapter(memoryToIceAdapter)
- {
- memory.name() = defaultMemoryName;
- }
-
- void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(memory.name(), "mem.MemoryName", "Name of this memory server.");
- defs->optional(maxHistorySize, "mem.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
-
- decay.defineProperties(defs, prefix + "decay.");
- }
-
- void Data::init()
- {
- coreSegment = &memory.addCoreSegment("ObjectPose", objpose::arondto::ObjectPose::toInitialAronType());
- coreSegment->setMaxHistorySize(maxHistorySize);
- }
-
- ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
- {
- bool agentSynchronized = false;
- ObjectPoseSeq latestObjectPoses = getLatestObjectPoses();
-
- ObjectPoseSeq result;
- for (ObjectPose& objectPose : latestObjectPoses)
- {
- updateObjectPose(objectPose, now, robot, agentSynchronized);
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- return result;
- }
-
-
- ObjectPoseSeq Data::getObjectPosesByProvider(
- const std::string& providerName,
- IceUtil::Time now)
- {
- bool synchronized = false;
-
- // Update data.
- ObjectPoseSeq& objectPoses = this->objectPoses.at(providerName);
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- ObjectPoseSeq result;
- for (const ObjectPose& objectPose : objectPoses)
- {
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- return result;
- }
-
- ObjectPoseSeq Data::getStoredObjectPoses(const std::string& providerName) const
- {
- auto it = objectPoses.find(providerName);
- if (it != objectPoses.end())
- {
- return it->second;
- }
- else
- {
- return {};
- }
- }
-
- std::map<std::string, ObjectPoseSeq> Data::getStoredObjectPoses() const
- {
- return objectPoses;
- }
-
-
- void Data::setObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
- {
- // Update legacy container.
- this->objectPoses[providerName] = objectPoses;
-
- // Update memory.
- if (coreSegment)
- {
- const armem::MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- }
-
- armem::Commit commit;
- for (const ObjectPose& pose : objectPoses)
- {
- armem::EntityUpdate& update = commit.updates.emplace_back();
- {
- update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
- update.timeArrived = TimeUtil::GetTime();
- update.timeCreated = pose.timestamp;
- update.confidence = pose.confidence;
-
- arondto::ObjectPose dto;
- toAron(dto, pose);
- update.instancesData.push_back(dto.toAron());
- }
-
- memoryToIceAdapter.commit(commit);
- }
- }
- }
-
-
-
- void Data::updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- for (ObjectPose& pose : objectPoses)
- {
- updateObjectPose(pose, now, agent, agentSynchronized);
- }
- }
-
-
- void Data::updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- updateAttachement(objectPose, agent, agentSynchronized);
-
- if (decay.enabled)
- {
- decay.updateConfidence(objectPose, now);
- }
- }
-
-
- void Data::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
- {
- if (!objectPose.attachment)
- {
- // Fetch attachment info from internal data structure.
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- // Store it in the objectPose.
- objectPose.attachment = it->second;
- }
- else
- {
- // No attachment, nothing to do.
- return;
- }
- }
- ARMARX_CHECK(objectPose.attachment);
-
- if (!synchronized) // Synchronize only once.
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
- synchronized = true;
- }
- objectPose.updateAttached(agent);
- }
-
- ObjectPoseSeq Data::getLatestObjectPoses() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
-
- ObjectPoseSeq result;
-
- for (const auto& [_, provSegment] : *coreSegment)
- {
- for (const auto& [_, entity] : provSegment)
- {
- if (!entity.empty())
- {
- for (const armem::EntityInstance& instance : entity.getLatestSnapshot())
- {
- arondto::ObjectPose dto;
- dto.fromAron(instance.data());
-
- ObjectPose& pose = result.emplace_back();
- fromAron(dto, pose);
- }
- }
- }
- }
-
- return result;
- }
-
-
- ObjectPose* Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
- {
- ObjectPose* pose = nullptr;
- if (!providerName.empty())
- {
- pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
- }
- else
- {
- for (auto& [_, poses] : objectPoses)
- {
- pose = findObjectPoseByID(poses, objectID);
- if (pose)
- {
- break;
- }
- }
- }
- return pose;
- }
-
-
- std::optional<simox::OrientedBoxf> Data::getObjectOOBB(const ObjectID& id)
- {
- return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- try
- {
- return objectInfo->loadOOBB();
- }
- catch (const std::ios_base::failure& e)
- {
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- return std::nullopt;
- }
- }
- else
- {
- return std::nullopt;
- }
- });
- }
-
- ProviderInfo Data::getProviderInfo(const std::string& providerName)
- {
- try
- {
- return providers.at(providerName);
- }
- catch (const std::out_of_range&)
- {
- std::stringstream ss;
- ss << "No provider with name '" << providerName << "' available.\n";
- ss << "Available are:\n";
- for (const auto& [name, _] : providers)
- {
- ss << "- '" << name << "'\n";
- }
- throw std::out_of_range(ss.str());
- }
- }
-
-
-
- AttachObjectToRobotNodeOutput
- Data::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input)
- {
- AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
-
- ObjectID objectID = armarx::fromIce(input.objectID);
-
- if (input.agentName != "" && input.agentName != this->robot->getName())
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
- << "\n(You can leave the agent name empty if there is only one agent.)\n"
- << "\nKnown agents: " << std::vector<std::string> {this->robot->getName()};
- return output;
- }
- VirtualRobot::RobotPtr agent = this->robot;
-
- if (!agent->hasRobotNode(input.frameName))
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
- return output;
- }
- std::string frameName = input.frameName;
-
-
- // Find object pose provider name can be empty.
- ObjectPose* currentObjectPose = this->findObjectPose(objectID, input.providerName);
- if (!currentObjectPose)
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
- return output;
- }
-
- ObjectAttachmentInfo info;
- info.agentName = agent->getName();
- info.frameName = frameName;
-
- if (input.poseInFrame)
- {
- info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
- }
- else
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
-
- armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
- if (frameName == armarx::GlobalFrame)
- {
- info.poseInFrame = framed.toGlobalEigen(this->robot);
- }
- else
- {
- framed.changeFrame(this->robot, info.frameName);
- info.poseInFrame = framed.toEigen();
- }
- }
- this->attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
-
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << currentObjectPose->providerName << "' "
- << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
-
- output.success = true;
- output.attachment = new data::ObjectAttachmentInfo();
- output.attachment->frameName = info.frameName;
- output.attachment->agentName = info.agentName;
- output.attachment->poseInFrame = new Pose(info.poseInFrame);
-
- return output;
- }
-
- DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input)
- {
- ObjectID objectID = armarx::fromIce(input.objectID);
- std::string providerName = input.providerName;
-
- std::optional<ObjectAttachmentInfo> attachment;
- {
- // Remove from latest pose (if it was cached).
- ObjectPose* objectPose = this->findObjectPose(objectID, input.providerName);
- if (objectPose)
- {
- objectPose->attachment = std::nullopt;
- }
-
- if (providerName.empty() && objectPose)
- {
- providerName = objectPose->providerName;
- }
- // Remove from attachment map.
- if (input.providerName.size() > 0)
- {
- auto it = this->attachments.find(std::make_pair(input.providerName, objectID));
- if (it != this->attachments.end())
- {
- attachment = it->second;
- this->attachments.erase(it);
- }
- }
- else
- {
- // Search for entry with matching object ID.
- for (auto it = this->attachments.begin(); it != this->attachments.end(); ++it)
- {
- const ObjectID& id = it->first.second;
- if (id == objectID)
- {
- attachment = it->second;
- this->attachments.erase(it);
- break;
- }
- }
- }
- }
-
- DetachObjectFromRobotNodeOutput output;
- output.wasAttached = bool(attachment);
- if (attachment)
- {
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
- }
- else
- {
- ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
- << "from robot node, but it was not attached.";
- }
-
- return output;
- }
-
- DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes()
- {
- DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = int(this->attachments.size());
-
- // Clear attachment map.
- this->attachments.clear();
-
- // Remove from poses (if it was cached).
- for (auto& [prov, poses] : this->objectPoses)
- {
- for (auto& pose : poses)
- {
- pose.attachment = std::nullopt;
- }
- }
-
- ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
-
- return output;
- }
-
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
deleted file mode 100644
index f538cc284f788ab76a86a03f99d4c725f53dc201..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
+++ /dev/null
@@ -1,134 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-#include <optional>
-
-#include <SimoxUtility/caching/CacheMap.h>
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include <RobotAPI/libraries/armem/core/Memory.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include "Decay.h"
-
-
-namespace armarx::objpose::observer
-{
-
- /**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
- */
- class Data : public armarx::Logging
- {
- public:
-
- static const std::string defaultMemoryName;
-
-
- public:
-
- Data(armem::Memory& memory, armem::server::MemoryToIceAdapter& memoryToIceAdapter);
-
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
-
- ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
-
- /// Get the stored object poses by the given provider.
- /// Return an empty list if the provider is unknown.
- ObjectPoseSeq getStoredObjectPoses(const std::string& providerName) const;
- /// Get the stored object poses grouped by provider.
- std::map<std::string, ObjectPoseSeq> getStoredObjectPoses() const;
-
- ObjectPose* findObjectPose(const ObjectID& objectID, const std::string& providerName = "");
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
-
- ProviderInfo getProviderInfo(const std::string& providerName);
-
-
-
- AttachObjectToRobotNodeOutput attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input);
- DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input);
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
-
-
- void setObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
-
-
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
-
- /**
- * @brief If the object is attached to a robot node, update it according to the current robot state.
- *
- * If there is no attachement info in `objectPose` itself, the internal data
- * structure `attachments` is queried. If an attachment is found there,
- * it is written into the given `objectPose` (thus, it is "cached" in the
- * info `objectPose`).
- *
- * @param synchronized Indicates whether the agent is already synchronized to the current time.
- */
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
- bool& synchronized) const;
-
- private:
-
- ObjectPoseSeq getLatestObjectPoses() const;
-
-
- public:
-
- RobotStateComponentInterfacePrx robotStateComponent;
- VirtualRobot::RobotPtr robot;
-
- ProviderInfoMap providers;
-
-
- std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments;
-
-
- ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
- /// Decay model.
- Decay decay;
-
- private:
-
-
- std::map<std::string, ObjectPoseSeq> objectPoses;
-
- armem::Memory& memory;
- armem::server::MemoryToIceAdapter& memoryToIceAdapter;
-
- armem::CoreSegment* coreSegment = nullptr;
- long maxHistorySize = -1;
-
- };
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
index 75058206bfaa38f82ed1d8fd5b3f6d60a83beb19..9eb2a0dac0aadc72946c163c9d79148f799e5aaf 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -1,76 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-namespace armarx::plugins
-{
- class ObjectPoseClientPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
-
- template<class...Ts>
- std::optional<ObjectInfo> findObject(Ts&& ...ts) const
- {
- return _finder.findObject(std::forward<Ts>(ts)...);
- }
-
- template<class...Ts>
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(Ts&& ...ts) const
- {
- return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
- }
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(const std::optional<ObjectInfo>& ts) const
- {
- return _finder.loadManipulationObject(ts);
- }
-
- const ObjectFinder& setObjectFinderPath(const std::string& path);
- const ObjectFinder& getObjectFinder() const;
-
- private:
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- ObjectFinder _finder;
- };
-}
-
-namespace armarx
-{
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseClientPluginUser :
- virtual public ManagedIceObject
- {
- public:
- /// Allow usage like: ObjectPoseClient::getObjects()
- using ObjectPoseClient = ObjectPoseClientPluginUser;
-
- ObjectPoseClientPluginUser();
-
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
- objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
-
- objpose::ObjectPoseSeq getObjectPoses();
-
- plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
- const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
-
- const ObjectFinder& getObjectFinder() const;
- private:
- armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
index 332064711b688d0e81969f3d1cc2ced472d817b1..55f6366e85ef20f50833f0889c6cf4b88c3614d2 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
@@ -1,60 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
-
-
-namespace armarx::plugins
-{
-
- class ObjectPoseProviderPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
- void postOnConnectComponent() override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
-
- private:
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- };
-
-}
-
-
-namespace armarx
-{
-
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseProviderPluginUser :
- virtual public ManagedIceObject
- , virtual public objpose::ObjectPoseProvider
- {
- public:
-
- ObjectPoseProviderPluginUser();
-
- /// Implement to process object requests (empty default implementation).
- objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
- objpose::ObjectPoseTopicPrx objectPoseTopic;
-
- private:
-
- armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
-
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
deleted file mode 100644
index 878921745f8b8cfd10cc67cbcc594d2199c84ea3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ObjectPoseObserver)
-
-armarx_add_test(ObjectPoseObserverTest ObjectPoseObserverTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
deleted file mode 100644
index 903d51bfb830771b2b70b0ef4bdacc414ecee3ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseObserver
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <iostream>
-
-using namespace armarx;
-
-
-BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
-{
- Eigen::Vector3f pos(-100, -200, -300);
- Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
- Eigen::Vector3f extents(40, 50, 60);
-
- armarx::objpose::Box box;
- box.position = new Vector3(pos);
- box.orientation = new Quaternion(ori);
- box.extents = new Vector3(extents);
-
-
- const float prec = 1e-3;
-
- simox::OrientedBoxf oobb;
- armarx::objpose::fromIce(box, oobb);
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
-
- armarx::objpose::Box boxOut;
- armarx::objpose::toIce(boxOut, oobb);
-
- Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
- Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
- Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
-
- ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
- ARMARX_CHECK(oriOut.isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
-}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
deleted file mode 100644
index fae15ee0afc1b1577bf808383a75faaa2d814ec3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
+++ /dev/null
@@ -1,33 +0,0 @@
-armarx_component_set_name("ObjectPoseProviderExample")
-
-
-set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
- # RobotAPICore RobotAPIInterfaces
- # RobotAPIComponentPlugins # for ArViz and other plugins
- RobotAPIArmarXObjects ObjectPoseObserver
-)
-
-set(SOURCES
- ObjectPoseProviderExample.cpp
-)
-set(HEADERS
- ObjectPoseProviderExample.h
-)
-
-
-armarx_add_component("${SOURCES}" "${HEADERS}")
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(ObjectPoseProviderExample PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-# add unit tests
-add_subdirectory(test)
-
-#generate the application
-armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index ed590589f17e297037348c6ef57ec23d23b14c52..b5d8b580fda915310ee03aa5255e7c7c25704fad 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -1,6 +1,7 @@
# memory servers
add_subdirectory(server/ExampleMemory)
add_subdirectory(server/GeneralPurposeMemory)
+add_subdirectory(server/ObjectMemory)
add_subdirectory(server/RobotSensorMemory)
add_subdirectory(server/SkillsMemory)
add_subdirectory(server/GraspMemory)
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index a8e816bf283467a0a0efbadf9fff40d555ec153b..9864a39c862c352c880c12609fc277fd265771fd 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1 +1,2 @@
+add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(ExampleClient)
diff --git a/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f67b7891d9978df279f9702d9f41fa5a999da72e
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/CMakeLists.txt
@@ -0,0 +1,27 @@
+armarx_component_set_name("ObjectPoseProviderExample")
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # RobotAPI
+ RobotAPI::ArmarXObjects
+ RobotAPI::armem_objects
+)
+
+set(SOURCES
+ ObjectPoseProviderExample.cpp
+)
+set(HEADERS
+ ObjectPoseProviderExample.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
similarity index 95%
rename from source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
rename to source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index 590bad8d596e06666405a3a631b78a790263cd10..f759697348f61927af7df5c9297e2081b0c1db91 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -30,16 +30,13 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
namespace armarx
{
- ObjectPoseProviderExamplePropertyDefinitions::ObjectPoseProviderExamplePropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ObjectPoseProviderExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.h
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
rename to source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index aac11a5f1c9fd6d8a920a85166495e29346e6772..7ed814eb06c219f3280f1d5d4e9473ef56344fb0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -31,26 +31,14 @@
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
// Include the ProviderPlugin
-#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h>
-#include <RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
namespace armarx
{
- /**
- * @class ObjectPoseProviderExamplePropertyDefinitions
- * @brief Property definitions of `ObjectPoseProviderExample`.
- */
- class ObjectPoseProviderExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- ObjectPoseProviderExamplePropertyDefinitions(std::string prefix);
- };
-
-
/**
* @defgroup Component-ObjectPoseProviderExample ObjectPoseProviderExample
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/test/CMakeLists.txt
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
rename to source/RobotAPI/components/armem/client/ObjectPoseProviderExample/test/CMakeLists.txt
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp b/source/RobotAPI/components/armem/client/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
rename to source/RobotAPI/components/armem/client/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
index 28ade93141eef153f7ed6dd8972cc0adda2b9f3b..1c90c244bf872fdc4931e0b9067a92c189d9b694 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
@@ -2,12 +2,16 @@ armarx_component_set_name("ObjectMemory")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarXGui
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ # RobotAPI
+ RobotAPI::ComponentPlugins
+ RobotAPI::armem_objects
- ${IVT_LIBRARIES}
+ # This project
+ ${PROJECT_NAME}Interfaces
)
set(SOURCES
@@ -19,8 +23,11 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#generate the application
-armarx_generate_and_add_component_executable()
-
+# add unit tests
+# add_subdirectory(test)
+# generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server::obj
+)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 93977f9d9a04181f9b22c24d0c6639571d591412..5e95d6cb264a63600214ce0ec384e4b5dc0131bf 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -15,78 +15,114 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "ObjectMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/algorithm/string.h>
+namespace armarx::armem::server::obj
+{
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+ const std::string ObjectMemory::defaultMemoryName = "Object";
-namespace armarx
-{
- ObjectMemory::ObjectMemory()
- {
- }
armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+
+ // Offer
+ defs->topic(debugObserver);
+
+ // Subscribe
+ defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
+
+ // Use
+ // defs->component(kinematicUnitObserver); // Optional dependency.
+ defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
+ "Name of the kinematic unit observer.");
+
+ const std::string prefix = "mem.";
+
+ memory.name() = defaultMemoryName;
+ defs->optional(memory.name(), prefix + "MemoryName", "Name of this memory server.");
+
+ instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+
return defs;
}
+ ObjectMemory::ObjectMemory() :
+ server::ComponentPluginUser(),
+ instance::SegmentAdapter(server::ComponentPluginUser::iceMemory, server::ComponentPluginUser::memoryMutex)
+ {
+ }
std::string ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
-
void ObjectMemory::onInitComponent()
{
- memory.name() = memoryName;
- }
+ memory.name() = defaultMemoryName;
+ instance::SegmentAdapter::init();
+ }
void ObjectMemory::onConnectComponent()
{
+ // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
+ // So we need to always make sure to guard a call to addRobot
+ if (!RobotState::hasRobot("robot"))
+ {
+ robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
+ }
+ robotStateComponent = getRobotStateComponent();
+
+ getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+
+ instance::SegmentAdapter::connect(
+ robotStateComponent, robot,
+ kinematicUnitObserver,
+ ArVizComponentPluginUser::arviz,
+ debugObserver
+ );
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
-
void ObjectMemory::onDisconnectComponent()
{
}
-
void ObjectMemory::onExitComponent()
{
}
-
- // WRITING
- armem::data::AddSegmentsResult ObjectMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ void ObjectMemory::createRemoteGuiTab()
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
- return result;
- }
+ using namespace armarx::RemoteGui::Client;
+ tab.instance.setup(*this);
- armem::data::CommitResult ObjectMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
- {
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
- return result;
+ VBoxLayout root =
+ {
+ tab.instance.group,
+ VSpacer()
+ };
+ RemoteGui_createTab(Component::getName(), root, &tab);
}
-
- // READING
- // Inherited from Plugin
-
+ void ObjectMemory::RemoteGui_update()
+ {
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ tab.instance.update(*this);
+ }
}
+
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index ca194dfbf6c2f0c3b527a2170aa1308f883780ae..52062816fbdf18079bba724d2a025d9ee0f55e8d 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -15,25 +15,33 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
+#include <mutex>
-#include <ArmarXCore/core/Component.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-namespace armarx
+
+namespace armarx::armem::server::obj
{
+
/**
* @defgroup Component-ObjectMemory ObjectMemory
* @ingroup RobotAPI-Components
@@ -46,41 +54,65 @@ namespace armarx
* Detailed description of class ObjectMemory.
*/
class ObjectMemory :
- virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ virtual public Component
+
+ , virtual public armarx::armem::server::ObjectMemoryInterface
+ , virtual public armarx::armem::server::ComponentPluginUser
+ , virtual public armarx::armem::server::obj::instance::SegmentAdapter
+
+ , virtual public armarx::RobotStateComponentPluginUser
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
{
public:
+
+ using RobotState = armarx::RobotStateComponentPluginUser;
+
+ static const std::string defaultMemoryName;
+
+
+ public:
+
ObjectMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- // WritingInterface interface
+
public:
- armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- protected:
- /// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
-
- /// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ // Remote GUI
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
private:
- std::string memoryName = "ObjectMemory";
- bool addCoreSegmentOnUsage = true;
+
+ DebugObserverInterfacePrx debugObserver;
+ RobotStateComponentInterfacePrx robotStateComponent;
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+
+ VirtualRobot::RobotPtr robot;
+
+
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ instance::SegmentAdapter::RemoteGui instance;
+ };
+ RemoteGuiTab tab;
+
};
+
}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5f5ecdf18b6c4abf1aa08213ee23c920491cf01b
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ObjectMemory)
+
+armarx_add_test(ObjectMemoryTest ObjectMemoryTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40329162efb831da65f1dcd1671dc458e5d6d1b0
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
@@ -0,0 +1,40 @@
+/*
+ * 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::ObjectMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectMemory
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/ObjectMemory/ObjectMemory.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <iostream>
+
+using namespace armarx;
+
+
+BOOST_AUTO_TEST_CASE(test_ObjectMemory)
+{
+}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
index f1ccdd08058d73791e7a832a8dcb8ee645a3c75b..855c88fb33c089de09965fd69b34c0537ec8f956 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
@@ -27,7 +27,7 @@ set(GUI_UIS
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
SimpleConfigDialog
- ObjectPoseObserver
+ RobotAPI::armem_objects
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 80404797fa4176aff345a9acc6946024ff04574b..bff75caba9f9b6208b1ba132f201285ae4bb2b89 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -393,8 +393,17 @@ namespace armarx
input.agentName = agentName;
input.frameName = frameName;
- objpose::AttachObjectToRobotNodeOutput output = objectPoseObserver->attachObjectToRobotNode(input);
- ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ try
+ {
+ objpose::AttachObjectToRobotNodeOutput output = objectPoseObserver->attachObjectToRobotNode(input);
+ ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to attach object '" << input.objectID << "' to robot node '"
+ << input.frameName << "' of agent '" << input.agentName << "'."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
@@ -405,8 +414,16 @@ namespace armarx
input.providerName = providerName.toStdString();
input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
- objpose::DetachObjectFromRobotNodeOutput output = objectPoseObserver->detachObjectFromRobotNode(input);
- ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ try
+ {
+ objpose::DetachObjectFromRobotNodeOutput output = objectPoseObserver->detachObjectFromRobotNode(input);
+ ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to detach object '" << input.objectID << "' from a robot node."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::requestSelectedObjects()
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index c15d822bda5f602547797e3da2a1f8585dc524c0..7c66f2d32f096e5db98a97cfc962f97b35d374c8 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -121,6 +121,9 @@ set(SLICE_FILES
armem/server/ReadingMemoryInterface.ice
armem/server/WritingMemoryInterface.ice
+ # Special Servers
+ armem/server/ObjectMemoryInterface.ice
+
armem/mns.ice
armem/mns/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..3418f7d2f3b6eb46b2132ee1b85a01b6e9f480ae
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
@@ -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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI
+* @author Rainer Kartmann
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice>
+
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+
+ interface ObjectInstanceSegmentInterface extends
+ armarx::objpose::ObjectPoseObserverInterface
+ {
+
+ };
+
+ interface ObjectMemoryInterface extends
+ ObjectInstanceSegmentInterface
+ {
+
+ };
+
+ };
+ };
+};
+
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
index 6fe63e5f97c3cdca0a17611f4dc7f5948c3564cd..65d24a3ff8156be3245a4583d8ee84fd01e38190 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
@@ -89,12 +89,27 @@ module armarx
{
string providerName;
armarx::data::ObjectID objectID;
+
+ /**
+ * @brief If true, the object will stay at the position before
+ * detaching until it is provided again.
+ */
+ bool commitAttachedPose = true;
};
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
bool wasAttached;
};
+
+ struct DetachAllObjectsFromRobotNodesInput
+ {
+ /**
+ * @brief If true, the objects will stay at the position before
+ * detaching until they are provided again.
+ */
+ bool commitAttachedPose = true;
+ }
struct DetachAllObjectsFromRobotNodesOutput
{
/// Number of objects that have been detached.
@@ -138,8 +153,7 @@ module armarx
};
interface ObjectPoseObserverInterface extends
- ObserverInterface
- , ObjectPoseTopic
+ ObjectPoseTopic
, armem::server::MemoryInterface
{
// Object poses
@@ -165,7 +179,7 @@ module armarx
/// Detach an attached object from a robot node.
DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
/// Detach all objects from robot nodes.
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(DetachAllObjectsFromRobotNodesInput input);
AgentFramesSeq getAttachableFrames();
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index e4f5bb40c1eac258c36963b1628d716475b04826..6805888c903874647339cea35cc2422c44064a4f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -4,7 +4,8 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
- RobotAPICore
+ # RobotAPI
+ RobotAPI::Core
)
set(LIB_FILES
@@ -16,6 +17,10 @@ set(LIB_FILES
json_conversions.cpp
ice_conversions.cpp
aron_conversions.cpp
+
+ plugins/ObjectPoseProviderPlugin.cpp
+ plugins/ObjectPoseClientPlugin.cpp
+ plugins/RequestedObjects.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -28,10 +33,16 @@ set(LIB_HEADERS
json_conversions.h
ice_conversions.h
aron_conversions.h
+
+ plugins/ObjectPoseProviderPlugin.h
+ plugins/ObjectPoseClientPlugin.h
+ plugins/RequestedObjects.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
@@ -39,6 +50,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/ObjectID.xml
aron/ObjectPose.xml
+ aron/ObjectType.xml
aron/OrientedBox.xml
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index ac04344c87e0e17e8854b2e05a669370951567ca..859e141a7f6b439f2fc7930c086958bdd2ca1758 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -4,20 +4,16 @@
<CodeIncludes>
<Include include="<Eigen/Core>" />
<Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
<Include include="<RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
<Include include="<RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml>" />
</AronIncludes>
<GenerateTypes>
- <IntEnum name="armarx::objpose::arondto::ObjectType">
- <EnumValue key="AnyObject" value="0" />
- <EnumValue key="KnownObject" value="1" />
- <EnumValue key="UnknownObject" value="2" />
- </IntEnum>
-
<Object name="armarx::objpose::arondto::ObjectAttachmentInfo">
<ObjectChild key='frameName'>
<string />
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
new file mode 100644
index 0000000000000000000000000000000000000000..263293ba53cc864c50927fa8e49bc590ea1cfcba
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
@@ -0,0 +1,14 @@
+<!-- ARON DTO of armarx::objpose::ObjectTypeEnum. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <IntEnum name="armarx::objpose::arondto::ObjectType">
+ <EnumValue key="AnyObject" value="0" />
+ <EnumValue key="KnownObject" value="1" />
+ <EnumValue key="UnknownObject" value="2" />
+ </IntEnum>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml
index 9ecf50c4066c3ed32d4edeecc71a065530576266..336e0a587feb7e302e317930f9d8a7b46f2824c9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.xml
@@ -7,10 +7,13 @@
<GenerateTypes>
<Object name="simox::arondto::OrientedBox">
- <ObjectChild key='centerPose'>
- <Pose />
+ <ObjectChild key='center'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
</ObjectChild>
- <ObjectChild key='extends'>
+ <ObjectChild key='extents'>
<Position />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
index e24325158807546c2e447db8a750a5e5413369f5..d2eab3af10f99d24ec6e9be171a0dcaefe53dcff 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
@@ -15,13 +15,14 @@
void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
{
- bo = OrientedBoxf(dto.centerPose, dto.extends);
+ bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
}
void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
{
- dto.centerPose = bo.transformation();
- dto.extends = bo.dimensions();
+ dto.center = bo.center();
+ dto.orientation = bo.rotation();
+ dto.extents = bo.dimensions();
}
void armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
@@ -103,6 +104,10 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
bo.attachment = ObjectAttachmentInfo();
fromAron(dto.attachment, *bo.attachment);
}
+ else
+ {
+ bo.attachment = std::nullopt;
+ }
bo.confidence = dto.confidence;
@@ -113,6 +118,10 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
bo.localOOBB = simox::OrientedBoxf();
fromAron(dto.localOOBB, *bo.localOOBB);
}
+ else
+ {
+ bo.localOOBB = std::nullopt;
+ }
}
@@ -136,6 +145,11 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
dto.attachmentValid = true;
toAron(dto.attachment, *bo.attachment);
}
+ else
+ {
+ dto.attachmentValid = false;
+ toAron(dto.attachment, ObjectAttachmentInfo());
+ }
dto.confidence = bo.confidence;
@@ -146,5 +160,10 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
dto.localOOBBValid = true;
toAron(dto.localOOBB, *bo.localOOBB);
}
+ else
+ {
+ dto.localOOBBValid = false;
+ toAron(dto.localOOBB, simox::OrientedBoxf());
+ }
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
index 043ef05614aed294872283a96508295b146c8086..d16b49fbb7dd659b66bc48d488b04d4286ccde98 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
@@ -7,6 +7,7 @@
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/OrientedBox.aron.generated.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
similarity index 95%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 95b046d87274b5562b43d1237cf2ffff9144f0aa..84f964d95fedf7471b1f083c915d86a2e209012b 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -8,8 +8,8 @@ namespace armarx::plugins
{
properties->defineOptionalProperty<std::string>(
makePropertyName(PROPERTY_NAME),
- "ObjectPoseObserver",
- "Name of the object pose observer.");
+ "ObjectMemory",
+ "Name of the object memory.");
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c901681f2b4d568ba65b8d8a24d2d24d0eeab2a
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::plugins
+{
+ class ObjectPoseClientPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+
+ template<class...Ts>
+ std::optional<ObjectInfo> findObject(Ts&& ...ts) const
+ {
+ return _finder.findObject(std::forward<Ts>(ts)...);
+ }
+
+ template<class...Ts>
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(Ts&& ...ts) const
+ {
+ return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
+ }
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(const std::optional<ObjectInfo>& ts) const
+ {
+ return _finder.loadManipulationObject(ts);
+ }
+
+ const ObjectFinder& setObjectFinderPath(const std::string& path);
+ const ObjectFinder& getObjectFinder() const;
+
+ private:
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
+
+ ObjectFinder _finder;
+ };
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseClientPluginUser :
+ virtual public ManagedIceObject
+ {
+ public:
+ /// Allow usage like: ObjectPoseClient::getObjects()
+ using ObjectPoseClient = ObjectPoseClientPluginUser;
+
+ ObjectPoseClientPluginUser();
+
+ objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
+ objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+
+ objpose::ObjectPoseSeq getObjectPoses();
+
+ plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
+ const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
+
+ const ObjectFinder& getObjectFinder() const;
+ private:
+ armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..332064711b688d0e81969f3d1cc2ced472d817b1
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -0,0 +1,60 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
+
+
+namespace armarx::plugins
+{
+
+ class ObjectPoseProviderPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+ void postOnConnectComponent() override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+
+ private:
+
+ static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+
+ };
+
+}
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseProviderPluginUser :
+ virtual public ManagedIceObject
+ , virtual public objpose::ObjectPoseProvider
+ {
+ public:
+
+ ObjectPoseProviderPluginUser();
+
+ /// Implement to process object requests (empty default implementation).
+ objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+ objpose::ObjectPoseTopicPrx objectPoseTopic;
+
+ private:
+
+ armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 06c12b18a610b8b05998daea8bc2100cb6839078..1778ba42d2c67c1d6967485ee5fbcaa05241488a 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -19,6 +19,7 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
+add_subdirectory(armem_objects)
add_subdirectory(armem_robot_localization)
add_subdirectory(armem_robot_mapping)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 8e61ccd8332b8ef0c8cc4fc891dd2548a68a5682..a4ca9b08ad6d3b0937294c5a9050adb6680abfcc 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -44,5 +44,7 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(RobotAPI::ComponentPlugins ALIAS RobotAPIComponentPlugins)
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 0d1c51febc1fadc67eb793363e9a0fc172975716..4fef1082d1919af56746dd4320fbbe63defafca3 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -10,10 +10,6 @@ set(LIBS
aron
)
-set(ARON_FILES
- core/aron/MemoryID.xml
-)
-
set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
@@ -29,6 +25,7 @@ set(LIB_FILES
core/ice_conversions.cpp
core/Memory.cpp
core/ProviderSegment.cpp
+ core/Visitor.cpp
core/detail/MemoryItem.cpp
core/detail/MaxHistorySize.cpp
@@ -91,6 +88,7 @@ set(LIB_HEADERS
core/EntitySnapshot.h
core/ice_conversions.h
core/Memory.h
+ core/Visitor.h
core/ProviderSegment.h
core/detail/EntityContainer.h
@@ -150,8 +148,7 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${LIB_NAME}
ARON_FILES
- ${ARON_FILES}
- #ENABLE_DEBUG_INFO
+ aron/MemoryID.xml
)
diff --git a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
similarity index 58%
rename from source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
rename to source/RobotAPI/libraries/armem/aron/MemoryID.xml
index 2cfc7d52c05fb53aec1f8b51a711b80c34478a31..996259c63c7b66028bd8b853714fa3147fd64ed6 100644
--- a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
+++ b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
@@ -1,23 +1,23 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::armem::arondto::MemoryID'>
- <ObjectChild key='memoryName'>
+ <Object name="armarx::armem::arondto::MemoryID">
+ <ObjectChild key="memoryName">
<string />
</ObjectChild>
- <ObjectChild key='coreSegmentName'>
+ <ObjectChild key="coreSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='providerSegmentName'>
+ <ObjectChild key="providerSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='entityName'>
+ <ObjectChild key="entityName">
<string />
</ObjectChild>
- <ObjectChild key='timestamp'>
+ <ObjectChild key="timestamp">
<Time />
</ObjectChild>
- <ObjectChild key='instanceIndex'>
+ <ObjectChild key="instanceIndex">
<int />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem/core/Visitor.cpp b/source/RobotAPI/libraries/armem/core/Visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e6417391ae3ba4f7d1adeea884bc334e0f42570
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/Visitor.cpp
@@ -0,0 +1,151 @@
+#include "Visitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem
+{
+
+ Visitor::Visitor()
+ {
+ }
+
+ Visitor::~Visitor()
+ {
+ }
+
+ bool Visitor::applyTo(Memory& memory)
+ {
+ for (auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(CoreSegment& coreSegment)
+ {
+ for (auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(ProviderSegment& providerSegment)
+ {
+ for (auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(Entity& entity)
+ {
+ for (auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntitySnapshot& snapshot)
+ {
+ for (auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+
+ bool Visitor::applyTo(const Memory& memory)
+ {
+ for (const auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const CoreSegment& coreSegment)
+ {
+ for (const auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const ProviderSegment& providerSegment)
+ {
+ for (const auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const Entity& entity)
+ {
+ for (const auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntitySnapshot& snapshot)
+ {
+ for (const auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/Visitor.h b/source/RobotAPI/libraries/armem/core/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9ec9629d320ec95a4a4a58a71f566e23d84f2c0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/Visitor.h
@@ -0,0 +1,151 @@
+#pragma once
+
+namespace armarx::armem
+{
+ class Memory;
+ class CoreSegment;
+ class ProviderSegment;
+ class Entity;
+ class EntitySnapshot;
+ class EntityInstance;
+
+
+ /**
+ * @brief A visitor for the hierarchical Memory data structure.
+ */
+ class Visitor
+ {
+
+ public:
+
+ Visitor();
+ virtual ~Visitor();
+
+ bool applyTo(Memory& memory);
+ bool applyTo(CoreSegment& coreSegment);
+ bool applyTo(ProviderSegment& providerSegment);
+ bool applyTo(Entity& entity);
+ bool applyTo(EntitySnapshot& snapshot);
+ bool applyTo(EntityInstance& instance);
+
+
+ virtual bool visitEnter(Memory& memory)
+ {
+ return visitEnter(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitEnter(CoreSegment& coreSegment)
+ {
+ return visitEnter(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitEnter(ProviderSegment& providerSegment)
+ {
+ return visitEnter(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitEnter(Entity& entity)
+ {
+ return visitEnter(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitEnter(EntitySnapshot& snapshot)
+ {
+ return visitEnter(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visitExit(Memory& memory)
+ {
+ return visitExit(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitExit(CoreSegment& coreSegment)
+ {
+ return visitExit(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitExit(ProviderSegment& providerSegment)
+ {
+ return visitExit(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitExit(Entity& entity)
+ {
+ return visitExit(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitExit(EntitySnapshot& snapshot)
+ {
+ return visitExit(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visit(EntityInstance& instance)
+ {
+ return visit(const_cast<const EntityInstance&>(instance));
+ }
+
+
+
+ // Const versions
+
+ bool applyTo(const Memory& memory);
+ bool applyTo(const CoreSegment& coreSegment);
+ bool applyTo(const ProviderSegment& providerSegment);
+ bool applyTo(const Entity& entity);
+ bool applyTo(const EntitySnapshot& snapshot);
+ bool applyTo(const EntityInstance& instance);
+
+
+ virtual bool visitEnter(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitEnter(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitEnter(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitEnter(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitEnter(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visitExit(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitExit(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitExit(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitExit(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitExit(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visit(const EntityInstance& instance)
+ {
+ (void) instance;
+ return true;
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h b/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h
index 873a15e6ef8379defd24696b216be7e2ceba7190..fd9b6a986decd0b7bb8b831f8ef431542745a387 100644
--- a/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h
+++ b/source/RobotAPI/libraries/armem/core/detail/EntityContainer.h
@@ -123,12 +123,15 @@ namespace armarx::armem::detail
protected:
- /// @throw `armem::error::ContainerNameMismatch` Of `expectedName != actualName`.
- void _checkContainerName(const std::string& expectedName, const std::string& actualName) const
+ /**
+ * @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
+ */
+ void _checkContainerName(const std::string& gottenName, const std::string& actualName,
+ bool emptyOk = true) const
{
- if (expectedName != actualName)
+ if (!((emptyOk && gottenName.empty()) || gottenName == actualName))
{
- throw armem::error::ContainerNameMismatch(expectedName, this->getLevelName(), actualName);
+ throw armem::error::ContainerNameMismatch(gottenName, this->getLevelName(), actualName);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 2b61bfbac513a1854859abecf81697e5baff3aae..c44a40758a73ba64d8827c6f7a751b9df8fc00c3 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -35,16 +35,15 @@ namespace armarx::armem::error
}
- ContainerNameMismatch::ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
const std::string& ownTerm, const std::string& containerName) :
- ArMemError(makeMsg(expectedName, ownTerm, containerName))
+ ArMemError(makeMsg(gottenName, ownTerm, containerName))
{}
- std::string ContainerNameMismatch::makeMsg(
- const std::string& expectedName, const std::string& containerTerm, const std::string& containerName)
+ std::string ContainerNameMismatch::makeMsg(const std::string& gottenName, const std::string& containerTerm, const std::string& containerName)
{
std::stringstream ss;
- ss << "Name '" << expectedName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
+ ss << "Name '" << gottenName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
return ss.str();
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 068283a519d74cfe86d7ffb31345d4404e4e0d7f..3456c7a350835e6c67ca1d439d2fc0c44a8be507 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -45,10 +45,10 @@ namespace armarx::armem::error
{
public:
- ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
- static std::string makeMsg(const std::string& expectedName,
+ static std::string makeMsg(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
};
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index a3fe99e2f0423cb2c03e5ed44dc53d6fb47be9d5..3a448d8735b7ba46487e24e3f0d7a33aec0654be 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -23,9 +23,19 @@ set(SOURCES
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
- instance/TreeDataVisitorBase.cpp
- instance/TreeDataVisitor.cpp
- instance/TreeTypedDataVisitor.cpp
+ instance/sanitize_typename.cpp
+ instance/serialize_path.cpp
+
+ instance/display_visitors/DataDisplayVisitor.cpp
+ instance/display_visitors/TypedDataDisplayVisitor.cpp
+
+ instance/tree_builders/DataTreeBuilder.cpp
+ instance/tree_builders/DataTreeBuilderBase.cpp
+ instance/tree_builders/TypedDataTreeBuilder.cpp
+
+ instance/tree_visitors/TreeDataVisitorBase.cpp
+ instance/tree_visitors/TreeDataVisitor.cpp
+ instance/tree_visitors/TreeTypedDataVisitor.cpp
memory/GroupBox.cpp
memory/TreeWidget.cpp
@@ -45,9 +55,19 @@ set(HEADERS
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
- instance/TreeDataVisitorBase.h
- instance/TreeDataVisitor.h
- instance/TreeTypedDataVisitor.h
+ instance/sanitize_typename.h
+ instance/serialize_path.h
+
+ instance/display_visitors/DataDisplayVisitor.h
+ instance/display_visitors/TypedDataDisplayVisitor.h
+
+ instance/tree_builders/DataTreeBuilder.h
+ instance/tree_builders/DataTreeBuilderBase.h
+ instance/tree_builders/TypedDataTreeBuilder.h
+
+ instance/tree_visitors/TreeDataVisitorBase.h
+ instance/tree_visitors/TreeDataVisitor.h
+ instance/tree_visitors/TreeTypedDataVisitor.h
memory/GroupBox.h
memory/TreeWidget.h
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 83d528d4bee1946760a56e74688d1eca348a31ca..ce0b625977833afb5235a5060e9f9f0699f8382e 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -9,6 +9,7 @@
#include <QLayout>
#include <QMenu>
#include <QSplitter>
+#include <QTreeWidget>
#include <QVBoxLayout>
#include <SimoxUtility/algorithm/string.h>
@@ -19,9 +20,10 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/ImageView.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
namespace armarx::armem::gui::instance
@@ -147,23 +149,23 @@ namespace armarx::armem::gui::instance
void InstanceView::updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
{
- armarx::gui::clearItem(treeItemData);
if (!data)
{
+ armarx::gui::clearItem(treeItemData);
QTreeWidgetItem* item = new QTreeWidgetItem({"(No data.)"});
treeItemData->addChild(item);
}
else if (useTypeInfo && aronType)
{
- TreeTypedDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*aronType, *data);
+ TypedDataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *aronType, *data);
}
else
{
- TreeDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*data);
+ DataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *data);
}
treeItemData->setExpanded(true);
}
@@ -211,7 +213,7 @@ namespace armarx::armem::gui::instance
case aron::type::Descriptor::eIVTCByteImage:
{
QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = TreeTypedDataVisitor::deserializePath(qpath);
+ aron::Path path = deserializePath(qpath);
QAction* viewAction = new QAction("Show image");
menu.addAction(viewAction);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f41f5d4cb447e9571d5145f26af5cbff6c9db2cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -0,0 +1,76 @@
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string DataDisplayVisitor::getValue(DataNavigator& n)
+ {
+ DataDisplayVisitor v;
+ v.applyTo(n);
+ return v.value.str();
+ }
+
+ bool DataDisplayVisitor::visitEnter(DictDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visitEnter(ListDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(BoolDataNavigator& b)
+ {
+ if (b.getValue())
+ {
+ value << "true";
+ }
+ else
+ {
+ value << "false";
+ }
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(DoubleDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(FloatDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(IntDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(LongDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(StringDataNavigator& n)
+ {
+ value << "'" << n.getValue() << "'";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(NDArrayDataNavigator& n)
+ {
+ value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
+ return false;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f48a57f0cd42a7e69dec44dd37ec489b50b440d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <sstream>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class DataDisplayVisitor : public aron::visitor::DataVisitor
+ {
+ public:
+
+ static std::string getValue(DataNavigator& n);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictDataNavigator& n) override;
+ bool visitEnter(ListDataNavigator& n) override;
+
+ bool visit(BoolDataNavigator& b) override;
+ bool visit(DoubleDataNavigator& n) override;
+ bool visit(FloatDataNavigator& n) override;
+ bool visit(IntDataNavigator& n) override;
+ bool visit(LongDataNavigator& n) override;
+ bool visit(StringDataNavigator& n) override;
+
+ bool visit(NDArrayDataNavigator& n) override;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..827d14a599af43e6067fd1b3fa18bb136dfcbed8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -0,0 +1,147 @@
+#include "TypedDataDisplayVisitor.h"
+
+#include <iomanip> // std::setprecision
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string TypedDataDisplayVisitor::getValue(TypeNavigator& type, DataNavigator& data)
+ {
+ TypedDataDisplayVisitor v;
+ bool r = v.applyTo(type, data);
+ ARMARX_CHECK(!r);
+ return v.value.str();
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ObjectTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ListTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(TupleTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(BoolTypeNavigator&, BoolDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IntTypeNavigator&, IntDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(LongTypeNavigator&, LongDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(StringTypeNavigator&, StringDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
+ {
+ armem::Time time = armem::Time::microSeconds(data.getValue());
+ armem::toDateTimeMilliSeconds(time);
+ value << armem::toDateTimeMilliSeconds(time);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ value << std::setprecision(2) << std::fixed;
+ value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ return false;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3923b7d0d74a1ee9c4b1ea7a5664ad3f83396051
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sstream>
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class TypedDataDisplayVisitor : public aron::visitor::TypedDataVisitor
+ {
+ public:
+
+ static std::string getValue(TypeNavigator& type, DataNavigator& data);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictTypeNavigator&, DictDataNavigator& data) override;
+ bool visitEnter(ObjectTypeNavigator&, DictDataNavigator& data) override;
+
+ bool visitEnter(ListTypeNavigator&, ListDataNavigator& data) override;
+ bool visitEnter(TupleTypeNavigator&, ListDataNavigator& data) override;
+
+
+ bool visit(BoolTypeNavigator&, BoolDataNavigator& data) override;
+ bool visit(DoubleTypeNavigator&, DoubleDataNavigator& data) override;
+ bool visit(FloatTypeNavigator&, FloatDataNavigator& data) override;
+ bool visit(IntTypeNavigator&, IntDataNavigator& data) override;
+ bool visit(LongTypeNavigator&, LongDataNavigator& data) override;
+ bool visit(StringTypeNavigator&, StringDataNavigator& data) override;
+ bool visit(TimeTypeNavigator&, LongDataNavigator& data) override;
+
+
+ bool visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PoseTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PositionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OrientationTypeNavigator&, NDArrayDataNavigator& data) override;
+
+
+ protected:
+
+ std::string coeffSep = " ";
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac5494864cfb7b2f27500b7d556bdd36e7eca749
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -0,0 +1,21 @@
+#include "sanitize_typename.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+
+std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
+{
+ namespace s = simox::alg;
+ std::string n = typeName;
+ n = s::replace_all(n, "Aron", "");
+ n = s::replace_all(n, "Type", "");
+ n = s::replace_all(n, "type::", "");
+ if (s::starts_with(n, "Object<") && s::ends_with(n, ">"))
+ {
+ std::string begin = "Object<";
+ std::string end = ">";
+ n = n.substr(begin.size(), n.size() - begin.size() - end.size());
+ }
+ return n;
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
new file mode 100644
index 0000000000000000000000000000000000000000..80c77acf229ecb6d70b55b6ac1c5b951a0cb89bd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <string>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ std::string sanitizeTypeName(const std::string& typeName);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a01b0b4620748b581db928c4e7ddcbd7af1a3f46
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
@@ -0,0 +1,35 @@
+#include "serialize_path.h"
+
+#include <RobotAPI/libraries/aron/core/Path.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <QString>
+#include <QStringList>
+
+
+QStringList armarx::armem::gui::instance::serializePath(const aron::Path& path)
+{
+ QStringList qpath;
+ qpath.append(QString::fromStdString(path.getRootIdentifier()));
+ qpath.append(QString::fromStdString(path.getDelimeter()));
+ for (const std::string& item : path.getPath())
+ {
+ qpath.append(QString::fromStdString(item));
+ }
+ return qpath;
+}
+
+armarx::aron::Path armarx::armem::gui::instance::deserializePath(const QStringList& qpath)
+{
+ ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
+ std::vector<std::string> pathItems;
+ for (int i = 2; i < qpath.size(); ++i)
+ {
+ pathItems.push_back(qpath.at(i).toStdString());
+ }
+ aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
+ return path;
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
new file mode 100644
index 0000000000000000000000000000000000000000..26517c6d11e3ee25071bb6f5e01da3996c6553cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
@@ -0,0 +1,18 @@
+#pragma once
+
+
+namespace armarx::aron
+{
+ class Path;
+}
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ QStringList serializePath(const aron::Path& path);
+ aron::Path deserializePath(const QStringList& qpath);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07ccac8866d870b153fba28915006d3dfd46cdae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -0,0 +1,58 @@
+#include "DataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilder::DataTreeBuilder()
+ {
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto child = data.getElement(key);
+ this->update(item, key, *child);
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item)
+ {
+ this->update(item, std::to_string(key), *children.at(key));
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+
+
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ this->setRowTexts(item, key, data);
+
+ if (auto cast = dynamic_cast<aron::datanavigator::DictNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ else if (auto cast = dynamic_cast<aron::datanavigator::ListNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9aa8471e5d4720c8e747844deb45459d68b1b90
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilder();
+
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f837fe2fa6451cf8edd82793f2c6ec773560ce2c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -0,0 +1,97 @@
+#include "DataTreeBuilderBase.h"
+
+#include <QTreeWidgetItem>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilderBase::DataTreeBuilderBase()
+ {
+ }
+
+ DataTreeBuilderBase::~DataTreeBuilderBase()
+ {}
+
+ void DataTreeBuilderBase::setColumns(int key, int value, int type)
+ {
+ this->columnKey = key;
+ this->columnType = type;
+ this->columnValue = value;
+ }
+
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(const std::string& key) const
+ {
+ return new QTreeWidgetItem(QStringList{QString::fromStdString(key)});
+ }
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(size_t key) const
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem();
+ item->setData(0, Qt::UserRole, static_cast<int>(key));
+ return item;
+ }
+
+ void DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName) const
+ {
+ item->setText(columnKey, QString::fromStdString(key));
+ item->setText(columnValue, QString::fromStdString(value));
+ item->setText(columnType, QString::fromStdString(typeName));
+ }
+
+
+ void DataTreeBuilderBase::setRowTexts(
+ QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ const std::string value = armarx::aron::DataDisplayVisitor::getValue(data);
+ setRowTexts(item, key, value, data.getName());
+ }
+
+ DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
+ {
+ DictBuilder builder;
+ builder.setCompareFn([](const std::string & key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(key, item->text(0).toStdString());
+ });
+ builder.setMakeItemFn([this](const std::string & key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ DataTreeBuilderBase::ListBuilder DataTreeBuilderBase::getListBuilder() const
+ {
+ ListBuilder builder;
+ builder.setCompareFn([](size_t key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(static_cast<int>(key), item->data(0, Qt::UserRole).toInt());
+ });
+ builder.setMakeItemFn([this](size_t key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ std::vector<size_t> DataTreeBuilderBase::getIndex(size_t size) const
+ {
+ std::vector<size_t> index;
+ index.reserve(size);
+ for (size_t i = 0; i < size; ++i)
+ {
+ index.push_back(i);
+ }
+ return index;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26a422ea71c90c2b769217a274eac7887eb8f9d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+
+
+namespace armarx
+{
+ template <class ContainerT> struct TreeWidgetBuilder;
+}
+
+class QTreeWidgetItem;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilderBase();
+ virtual ~DataTreeBuilderBase();
+
+ void setColumns(int key, int value, int type);
+
+
+ protected:
+
+ using DictBuilder = armarx::TreeWidgetBuilder<std::vector<std::string>>;
+ using ListBuilder = armarx::TreeWidgetBuilder<std::vector<size_t>>;
+
+ DictBuilder getDictBuilder() const;
+ ListBuilder getListBuilder() const;
+ std::vector<size_t> getIndex(size_t size) const;
+
+
+ QTreeWidgetItem* makeItem(const std::string& key) const;
+ QTreeWidgetItem* makeItem(size_t key) const;
+
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ public:
+
+ int columnKey = 0;
+ int columnValue = 1;
+ int columnType = 2;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abe50eb825167c5080799118de110bf14ef04983
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -0,0 +1,181 @@
+#include "TypedDataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ TypedDataTreeBuilder::TypedDataTreeBuilder()
+ {
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childType = type.getMemberType(key);
+ auto childData = data.getElement(key);
+
+ if (childType && childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, type.getAllKeys());
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
+ {
+ auto childData = children.at(key);
+ if (childData)
+ {
+ this->update(item, std::to_string(key), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ ARMARX_CHECK_EQUAL(data.childrenSize(), 2);
+ auto childTypes = type.getAcceptedTypes();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
+ {
+ auto childType = i == 0 ? childTypes.first : childTypes.second;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(data.childrenSize()));
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childTypes = type.getAcceptedTypes();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
+ {
+ auto childType = childTypes.at(i);
+ auto childData = data.getElement(static_cast<unsigned int>(i));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(type.getAcceptedTypes().size()));
+ }
+
+
+ void TypedDataTreeBuilder::update(
+ QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data)
+ {
+ using namespace aron::typenavigator;
+
+ const std::string value = armarx::aron::TypedDataDisplayVisitor::getValue(type, data);
+ const std::string typeName = sanitizeTypeName(type.getName());
+ ARMARX_IMPORTANT << VAROUT(typeName);
+ setRowTexts(item, key, value, typeName);
+
+ item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
+ item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
+
+ if (auto t = dynamic_cast<ObjectNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<DictNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<ListNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<PairNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<TupleNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ }
+
+ template <class DataT, class TypeT>
+ void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
+ {
+ DataT& dataCast = dynamic_cast<DataT&>(data);
+ updateTree(item, type, dataCast);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..57580a1f6d3a84ca639131e0da375a6129650e0a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class TypedDataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ TypedDataTreeBuilder();
+
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data);
+
+ template <class DataT, class TypeT>
+ void _updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data);
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
similarity index 72%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
index 0d1721ef244c8e4a44eba5003a19302c190518d7..8bc5889d29298e41dbb27cfa6114f9841c7d3f98 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
similarity index 94%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
index b751084bf8af57f2d7f6598e2ae2e835c0948b6c..61d76bcfc58a81d138a442d07e1f64bc0eb7b922 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
@@ -8,10 +8,10 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitor :
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
similarity index 98%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
index 70a84dd7d2b75aaa388992f397e5d4475b83b37f..87cd2ffb9bdee5de102b3b026fb508784f0ff0c0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
TreeDataVisitorBase::TreeDataVisitorBase()
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
similarity index 98%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
index cb6d4152afbf376ed4b1b63cb0a0e9bbf03ebf53..c9d4ab0cdaf462761b10e422ef41f415ec54d7a1 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
@@ -9,7 +9,7 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitorBase
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
similarity index 57%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
index 4dd70e8ac952addaf0da1bc4336bffd30fefca36..c4ed919f9b46be2c62966fccbd89fbefa0370449 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
@@ -11,24 +11,9 @@
#include <RobotAPI/libraries/armem/core.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
- std::string TreeTypedDataVisitor::sanitizeTypeName(const std::string& typeName) const
- {
- namespace s = simox::alg;
- std::string n = typeName;
- n = s::replace_all(n, "Aron", "");
- n = s::replace_all(n, "Type", "");
- if (s::starts_with(n, "Object<") && s::ends_with(n, ">"))
- {
- std::string begin = "Object<";
- std::string end = ">";
- n = n.substr(begin.size(), n.size() - begin.size() - end.size());
- }
- return n;
- }
-
QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const
{
QTreeWidgetItem* item = makeItem<NDArrayDataNavigator, PoseTypeNavigator>(key, data, type);
@@ -49,7 +34,7 @@ namespace armarx::armem::gui
(void) type;
const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
ss << std::setprecision(2) << std::fixed;
- ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", "\n", "", "", "", ""));
+ ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
}
void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const
@@ -57,7 +42,7 @@ namespace armarx::armem::gui
(void) type;
const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
ss << std::setprecision(2) << std::fixed;
- ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ", "", "", "(", ")"));
+ ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
}
void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const
@@ -65,31 +50,7 @@ namespace armarx::armem::gui
(void) type;
const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
ss << std::setprecision(2) << std::fixed;
- ss << quat.w() << " | " << quat.x() << " " << quat.x() << " " << quat.y() << " " << quat.z();
- }
-
- QStringList TreeTypedDataVisitor::serializePath(const aron::Path& path)
- {
- QStringList qpath;
- qpath.append(QString::fromStdString(path.getRootIdentifier()));
- qpath.append(QString::fromStdString(path.getDelimeter()));
- for (const std::string& item : path.getPath())
- {
- qpath.append(QString::fromStdString(item));
- }
- return qpath;
- }
-
- aron::Path TreeTypedDataVisitor::deserializePath(const QStringList& qpath)
- {
- ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
- std::vector<std::string> pathItems;
- for (int i = 2; i < qpath.size(); ++i)
- {
- pathItems.push_back(qpath.at(i).toStdString());
- }
- aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
- return path;
+ ss << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
similarity index 96%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index 9e38a1324f4e1dcd22499702fef39a534805f45e..fb1c1baa557167432ffaa19f46e71d223a5c080d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -7,10 +7,14 @@
#include <QLabel>
#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
-namespace armarx::armem::gui
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
+
+
+namespace armarx::armem::gui::instance
{
class TreeTypedDataVisitor :
@@ -123,9 +127,6 @@ namespace armarx::armem::gui
}
- std::string sanitizeTypeName(const std::string& typeName) const;
-
-
protected:
template <class DataNavigatorT, class TypeNavigatorT>
@@ -189,12 +190,7 @@ namespace armarx::armem::gui
void streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const;
- public:
-
- static QStringList serializePath(const aron::Path& path);
- static aron::Path deserializePath(const QStringList& qpath);
-
-
+ std::string coeffSep = " ";
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index 5ecf962dbef91f033435c532149c49285c408bc2..d1e0e4a011e05edc22c2ab8fb0835b95150c24da 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -29,13 +29,13 @@
#include <iostream>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+
+using namespace armarx::armem::gui::instance;
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName)
{
- armarx::armem::gui::TreeTypedDataVisitor v;
-
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronDictType<AronFloatType>"), "Dict<Float>");
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronListType<AronFloatType>"), "List<Float>");
+ BOOST_CHECK_EQUAL(sanitizeTypeName("AronDictType<AronFloatType>"), "Dict<Float>");
+ BOOST_CHECK_EQUAL(sanitizeTypeName("AronListType<AronFloatType>"), "List<Float>");
}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d7d6e97391aff7d7b6dd19a30def80b7c40fb49a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -0,0 +1,44 @@
+set(LIB_NAME armem_objects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::Core
+ RobotAPI::libraries::armem
+ HEADERS
+ aron_conversions.h
+
+ server/instance/SegmentAdapter.h
+ server/instance/Segment.h
+ server/instance/Decay.h
+ server/instance/RobotHeadMovement.h
+ server/instance/Visu.h
+ SOURCES
+ aron_conversions.cpp
+
+ server/instance/SegmentAdapter.cpp
+ server/instance/Segment.cpp
+ server/instance/Decay.cpp
+ server/instance/RobotHeadMovement.cpp
+ server/instance/Visu.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/ObjectInstance.xml
+)
+
+add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
new file mode 100644
index 0000000000000000000000000000000000000000..adcaa6b389a838e5d105e4fa1de4808c90ae67a9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
@@ -0,0 +1,30 @@
+<!--
+Core segment type of Object/Instance.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectInstance">
+
+ <ObjectChild key="pose">
+ <armarx::objpose::arondto::ObjectPose />
+ </ObjectChild>
+
+ <ObjectChild key="sourceID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..859f3d601024ba97dd0e59e938afaba2718a95a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -0,0 +1,27 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
+{
+ bo = dto.pose;
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+{
+ dto.pose = bo;
+}
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+{
+ objpose::fromAron(dto.pose, bo);
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+{
+ objpose::toAron(dto.pose, bo);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ea733e99c75895ff7aba463f36a0f53d0c4bd38
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+namespace armarx::armem
+{
+ void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo);
+
+ void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
similarity index 93%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index 522a5cc13d83acf95a4efe1c3289c3aec2912487..f07f526d60dc409bc2bbf7e2dc37389237d9ec96 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -24,15 +24,15 @@ namespace armarx::objpose::observer
"Remove objects whose confidence is lower than this value.");
}
- void Decay::updateConfidence(ObjectPose& pose, IceUtil::Time now) const
+ void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
{
float confidence = calculateConfidence(pose.timestamp, now);
pose.confidence = confidence;
}
- void Decay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const
+ void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
- for (ObjectPose& pose : objectPoses)
+ for (objpose::ObjectPose& pose : objectPoses)
{
if (pose.attachment)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
index ec64221f3e202395ff4f0f8c83d3bb737b6571be..a44652d5528bceefed921dec184fe8b87fd19bfe 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -23,8 +23,8 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
- void updateConfidence(ObjectPose& pose, IceUtil::Time now) const;
- void updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
+ void updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const;
+ void updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
private:
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
index a5e78b5942704a7e53a3aae34d10c32b07baaa73..973cf5a551228705b918560ad48648db5795ab1f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -120,6 +120,30 @@ namespace armarx::objpose::observer
}
+ RobotHeadMovement::Discard RobotHeadMovement::getDiscard()
+ {
+ Discard discard;
+ if (checkHeadVelocity)
+ {
+ if (isMoving())
+ {
+ movementStarts(discardIntervalAfterMoveMS);
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << discardUpdatesUntil;
+ discard.all = true;
+ }
+ else if (TimeUtil::GetTime() < discardUpdatesUntil)
+ {
+ discard.all = true;
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << discardUpdatesUntil;
+ }
+ else
+ {
+ discard.updatesUntil = discardUpdatesUntil;
+ }
+ }
+ return discard;
+ }
+
void RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
similarity index 91%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index 263266cf48a815b72e7267e8e267f1512ba809a4..e98c3e91f68cb0b194bf23161ed06fd6bdcce502 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -21,7 +21,7 @@ namespace armarx
using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
class RobotHeadMovement : public armarx::Logging
{
@@ -40,6 +40,14 @@ namespace armarx::objpose::observer
objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
+ struct Discard
+ {
+ std::optional<IceUtil::Time> updatesUntil;
+ bool all = false;
+ };
+ Discard getDiscard();
+
+
public:
bool checkHeadVelocity = true;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ba07cabc12b22d7148b480446acb5e27458b945
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -0,0 +1,678 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/core/Visitor.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <sstream>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ iceMemory(memoryToIceAdapter)
+ {
+ oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ {
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ return objectInfo->loadOOBB();
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
+ return std::nullopt;
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ });
+
+ classNameToDatasetCache.setFetchFn([this](const std::string & className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
+ }
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
+ "If true, no new snapshots are stored while an object is attached to a robot node.\n"
+ "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+
+ decay.defineProperties(defs, prefix + "decay.");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.memory);
+
+ coreSegment = &iceMemory.memory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+
+ Segment::CommitStats Segment::commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<armem::Time> discardUpdatesUntil)
+ {
+ CommitStats stats;
+
+ // Build new poses.
+ objpose::ObjectPoseSeq newObjectPoses;
+ stats.numUpdated = 0;
+ for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
+ {
+ const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ // Check whether we have an old snapshot for this object.
+ std::optional<objpose::ObjectPose> previousPose;
+ const armem::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+
+ previousPose = objpose::ObjectPose();
+ fromAron(data, *previousPose);
+ }
+
+ bool discard = false;
+ if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
+ {
+ // Dicard updates temporarily (e.g. due to head movement).
+ discard = true;
+ }
+ else if (previousPose)
+ {
+ if (p.discardSnapshotsWhileAttached && previousPose->attachment)
+ {
+ // Discard update due to active attachemnt.
+ discard = true;
+ }
+ else if (timestamp == previousPose->timestamp)
+ {
+ // Discard update as it is not new.
+ discard = true;
+ }
+ }
+
+ if (!discard)
+ {
+ // Update the entity.
+ stats.numUpdated++;
+
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ newPose.fromProvidedPose(provided, robot);
+
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
+ {
+ newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ }
+ }
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
+ }
+ }
+
+ commitObjectPoses(providerName, newObjectPoses);
+
+ return stats;
+ }
+
+ void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ // Update memory.
+ const armem::MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
+ if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ armem::Commit commit;
+ for (const objpose::ObjectPose& pose : objectPoses)
+ {
+ armem::EntityUpdate& update = commit.updates.emplace_back();
+ {
+ update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
+ update.timeArrived = TimeUtil::GetTime();
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ update.instancesData.push_back(dto.toAron());
+ }
+
+ iceMemory.commit(commit);
+ }
+ }
+
+
+ armem::CoreSegment& Segment::getCoreSegment()
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ const armem::CoreSegment& Segment::getCoreSegment() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
+ {
+ ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+
+ objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
+ const std::string& providerName,
+ IceUtil::Time now)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+ armem::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
+ if (providerName.empty())
+ {
+ for (auto& [_, prov] : *coreSegment)
+ {
+ if (prov.hasEntity(entityID.entityName))
+ {
+ return &prov.getEntity(entityID);
+ }
+ }
+ return nullptr;
+ }
+ else
+ {
+ entityID.providerSegmentName = providerName;
+ if (coreSegment->hasProviderSegment(providerName))
+ {
+ armem::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
+ return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+ }
+
+
+ void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ {
+ bool agentSynchronized = false;
+
+ for (ObjectPose& objectPose : objectPoses)
+ {
+ updateObjectPose(objectPose, now, robot, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ for (ObjectPose& pose : objectPoses)
+ {
+ updateObjectPose(pose, now, agent, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ updateAttachement(objectPose, agent, agentSynchronized);
+
+ if (decay.enabled)
+ {
+ decay.updateConfidence(objectPose, now);
+ }
+ }
+
+
+ objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ {
+ ObjectPoseSeq result;
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ return result;
+ }
+
+
+ void Segment::updateAttachement(
+ ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ {
+ if (!objectPose.attachment)
+ {
+ // No attachment, nothing to do.
+ return;
+ }
+ ARMARX_CHECK(objectPose.attachment);
+
+ if (!synchronized) // Synchronize only once.
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ synchronized = true;
+ }
+ objectPose.updateAttached(agent);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return getLatestObjectPoses(*coreSegment);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::CoreSegment& coreSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(coreSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::ProviderSegment& provSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(provSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPose Segment::getLatestObjectPose(const armem::Entity& entity)
+ {
+ ObjectPose result;
+ getLatestObjectPose(entity, result);
+ return result;
+ }
+
+ void Segment::getLatestObjectPoses(const armem::CoreSegment& coreSeg, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, provSegment] : coreSeg)
+ {
+ getLatestObjectPoses(provSegment, out);
+ }
+ }
+
+ void Segment::getLatestObjectPoses(const armem::ProviderSegment& provSegment, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, entity] : provSegment)
+ {
+ if (!entity.empty())
+ {
+ ObjectPose& pose = out.emplace_back();
+ getLatestObjectPose(entity, pose);
+ }
+ }
+ }
+
+ void Segment::getLatestObjectPose(const armem::Entity& entity, ObjectPose& out)
+ {
+ for (const armem::EntityInstance& instance : entity.getLatestSnapshot())
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ fromAron(dto, out);
+ }
+ }
+
+
+ arondto::ObjectInstance Segment::getLatestInstanceData(const armem::Entity& entity)
+ {
+ ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
+ const armem::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+
+ ARMARX_CHECK_EQUAL(snapshot.size(), 1);
+ const armem::EntityInstance& instance = snapshot.getInstance(0);
+
+ arondto::ObjectInstance data;
+ data.fromAron(instance.data());
+
+ return data;
+ }
+
+ std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ {
+ return oobbCache.get(id);
+ }
+
+ objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ {
+ try
+ {
+ return providers.at(providerName);
+ }
+ catch (const std::out_of_range&)
+ {
+ std::stringstream ss;
+ ss << "No provider with name '" << providerName << "' available.\n";
+ ss << "Available are:\n";
+ for (const auto& [name, _] : providers)
+ {
+ ss << "- '" << name << "'\n";
+ }
+ throw std::out_of_range(ss.str());
+ }
+ }
+
+
+
+ objpose::AttachObjectToRobotNodeOutput
+ Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
+ {
+ const armem::Time now = armem::Time::now();
+
+ objpose::AttachObjectToRobotNodeOutput output;
+ output.success = false; // We are not successful until proven otherwise.
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+
+ if (input.agentName != "" && input.agentName != this->robot->getName())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ << "\n(You can leave the agent name empty if there is only one agent.)\n"
+ << "\nKnown agents: " << std::vector<std::string> {this->robot->getName()};
+ return output;
+ }
+ VirtualRobot::RobotPtr agent = this->robot;
+
+ if (!agent->hasRobotNode(input.frameName))
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
+ << "' of agent '" << agent->getName() << "'.";
+ return output;
+ }
+ std::string frameName = input.frameName;
+
+
+ // Find object pose (provider name can be empty).
+ armem::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
+ if (!objectEntity || objectEntity->empty())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
+ << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ return output;
+ }
+ arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
+
+ objpose::ObjectAttachmentInfo info;
+ info.agentName = agent->getName();
+ info.frameName = frameName;
+
+ if (input.poseInFrame)
+ {
+ info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
+ }
+ else
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+
+ armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ if (frameName == armarx::GlobalFrame)
+ {
+ info.poseInFrame = framed.toGlobalEigen(this->robot);
+ }
+ else
+ {
+ framed.changeFrame(this->robot, info.frameName);
+ info.poseInFrame = framed.toEigen();
+ }
+ }
+
+ // Store attachment in new entity snapshot.
+ {
+ armem::EntityUpdate update;
+ update.entityID = objectEntity->id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated = data;
+ toAron(updated.pose.attachment, info);
+ updated.pose.attachmentValid = true;
+ update.instancesData = { updated.toAron() };
+ }
+ objectEntity->update(update);
+ }
+
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
+ << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
+ << "Object pose in frame: \n" << info.poseInFrame;
+
+ output.success = true;
+ output.attachment = new objpose::data::ObjectAttachmentInfo();
+ output.attachment->frameName = info.frameName;
+ output.attachment->agentName = info.agentName;
+ output.attachment->poseInFrame = new Pose(info.poseInFrame);
+
+ return output;
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input)
+ {
+ const armem::Time now = armem::Time::now();
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+ std::string providerName = input.providerName;
+
+ std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
+ {
+ // Remove from latest pose (if it was cached).
+ // Find object pose (provider name can be empty).
+ armem::Entity* entity = this->findObjectEntity(objectID, input.providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+ if (data.pose.attachmentValid)
+ {
+ attachment = data.pose.attachment;
+
+ // Store non-attached pose in new snapshot.
+ storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
+ }
+
+ if (providerName.empty())
+ {
+ providerName = data.pose.providerName;
+ }
+ }
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput output;
+ output.wasAttached = bool(attachment);
+ if (attachment)
+ {
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
+ << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ }
+ else
+ {
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ << "from robot node, but it was not attached.";
+ }
+
+ return output;
+ }
+
+
+ struct DetachVisitor : public armem::Visitor
+ {
+ Segment& owner;
+ armem::Time now;
+ bool commitAttachedPose;
+
+ int numDetached = 0;
+
+ DetachVisitor(Segment& owner, armem::Time now, bool commitAttachedPose) :
+ owner(owner), now(now), commitAttachedPose(commitAttachedPose)
+ {
+ }
+
+ virtual bool visitEnter(armem::Entity& entity) override;
+ };
+
+ bool DetachVisitor::visitEnter(armem::Entity& entity)
+ {
+ const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ numDetached++;
+ // Store non-attached pose in new snapshot.
+ owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
+ }
+
+ return false; // Stop descending.
+ }
+
+
+ objpose::DetachAllObjectsFromRobotNodesOutput Segment::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ const armem::Time now = armem::Time::now();
+
+ DetachVisitor visitor(*this, now, input.commitAttachedPose);
+ visitor.applyTo(*coreSegment);
+
+ objpose::DetachAllObjectsFromRobotNodesOutput output;
+ output.numDetached = visitor.numDetached;
+
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
+
+ return output;
+ }
+
+ void Segment::storeDetachedSnapshot(
+ armem::Entity& entity,
+ const arondto::ObjectInstance& data,
+ armem::Time now,
+ bool commitAttachedPose)
+ {
+ armem::EntityUpdate update;
+ update.entityID = entity.id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated;
+ if (commitAttachedPose && data.pose.attachmentValid)
+ {
+ ObjectPose objectPose;
+ fromAron(data, objectPose);
+
+ bool agentSynchronized = false;
+ updateAttachement(objectPose, robot, agentSynchronized);
+
+ objectPose.attachment = std::nullopt;
+ toAron(updated, objectPose);
+ }
+ else
+ {
+ updated = data;
+ updated.pose.attachmentValid = false;
+ toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
+ }
+
+ update.instancesData = { updated.toAron() };
+ }
+ entity.update(update);
+ }
+
+
+
+ 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())
+ {
+ data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (data.coreSegment)
+ {
+ data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ }
+ }
+
+ data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..19646b5fab11ca8864a405d5d852b650c05dc415
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -0,0 +1,187 @@
+#pragma once
+
+#include <map>
+#include <string>
+#include <optional>
+
+#include <SimoxUtility/caching/CacheMap.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseObserverInterface.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+#include "Decay.h"
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ struct CommitStats
+ {
+ int numUpdated = 0;
+ };
+ using ObjectPose = objpose::ObjectPose;
+ using ObjectPoseSeq = objpose::ObjectPoseSeq;
+
+
+ public:
+
+ Segment(armem::server::MemoryToIceAdapter& iceMemory);
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+
+ CommitStats commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<armem::Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+
+
+ armem::CoreSegment& getCoreSegment();
+ const armem::CoreSegment& getCoreSegment() const;
+
+
+ objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
+ objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+
+ armem::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
+ std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+
+ objpose::ProviderInfo getProviderInfo(const std::string& providerName);
+
+ objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
+ objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
+ objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
+
+
+ /**
+ * @brief If the object is attached to a robot node, update it according to the current robot state.
+ *
+ * If there is no attachement info in `objectPose` itself, the internal data
+ * structure `attachments` is queried. If an attachment is found there,
+ * it is written into the given `objectPose` (thus, it is "cached" in the
+ * info `objectPose`).
+ *
+ * @param synchronized Indicates whether the agent is already synchronized to the current time.
+ */
+ void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ bool& synchronized) const;
+
+
+ static ObjectPoseSeq getLatestObjectPoses(const armem::CoreSegment& coreSeg);
+ static ObjectPoseSeq getLatestObjectPoses(const armem::ProviderSegment& provSeg);
+ static ObjectPose getLatestObjectPose(const armem::Entity& entity);
+
+ static void getLatestObjectPoses(const armem::CoreSegment& coreSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPoses(const armem::ProviderSegment& provSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPose(const armem::Entity& entity, ObjectPose& out);
+
+ static arondto::ObjectInstance getLatestInstanceData(const armem::Entity& entity);
+
+
+ private:
+
+ ObjectPoseSeq getLatestObjectPoses() const;
+
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now);
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+ void updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+
+
+ ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+
+
+ void storeDetachedSnapshot(
+ armem::Entity& entity,
+ const arondto::ObjectInstance& data,
+ armem::Time now,
+ bool commitAttachedPose);
+
+
+ friend struct DetachVisitor;
+
+
+ public:
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ VirtualRobot::RobotPtr robot;
+
+ objpose::ProviderInfoMap providers;
+
+
+ ObjectFinder objectFinder;
+
+ /// Decay model.
+ Decay decay;
+
+
+ private:
+
+ armem::server::MemoryToIceAdapter& iceMemory;
+
+ armem::CoreSegment* coreSegment = nullptr;
+
+
+ struct Properties
+ {
+ std::string coreSegmentName = "Instance";
+ long maxHistorySize = -1;
+ bool discardSnapshotsWhileAttached = true;
+ };
+ Properties p;
+
+
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ /// Class name -> dataset name.
+ simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ void setup(const Segment& data);
+ void update(Segment& data);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d44f5dfa0f1eb73ac3cf5a41af76b9b2532aa1d7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -0,0 +1,498 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem_objects::SegmentAdapter
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SegmentAdapter.h"
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
+ segment(iceMemory),
+ memoryMutex(memoryMutex)
+ {
+ }
+
+ std::string SegmentAdapter::getName() const
+ {
+ return Logging::tag.tagName;
+ }
+
+ void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ calibration.defineProperties(defs, prefix + "calibration.");
+ segment.defineProperties(defs, prefix);
+ robotHead.defineProperties(defs, prefix + "head.");
+ visu.defineProperties(defs, prefix + "visu.");
+ }
+
+ void SegmentAdapter::init()
+ {
+ segment.setTag(getName());
+ segment.decay.setTag(getName());
+ robotHead.setTag(getName());
+ visu.setTag(getName());
+
+ segment.init();
+ }
+
+ void SegmentAdapter::connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ )
+ {
+ this->debugObserver = debugObserver;
+ this->arviz = arviz;
+
+ segment.robot = robot;
+ segment.robotStateComponent = robotStateComponent;
+
+ robotHead.kinematicUnitObserver = kinematicUnitObserver;
+ robotHead.debugObserver = debugObserver;
+ robotHead.fetchDatafields();
+
+ visu.arviz = arviz;
+ if (!visu.updateTask)
+ {
+ visu.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ visu.updateTask->start();
+ }
+ }
+
+ void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+ {
+ updateProviderInfo(providerName, info);
+ }
+
+
+ void SegmentAdapter::reportObjectPoses(
+ const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
+ updateObjectPoses(providerName, providedPoses);
+ }
+
+
+ void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ {
+ if (!info.proxy)
+ {
+ ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+ return;
+ }
+ {
+ std::scoped_lock lock(memoryMutex);
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+ << "Supported objects: \n" << ss.str();
+ segment.providers[providerName] = info;
+ }
+ }
+
+
+ void SegmentAdapter::updateObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+ {
+ TIMING_START(tReportObjectPoses);
+
+ RobotHeadMovement::Discard discard;
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ discard = robotHead.getDiscard();
+ }
+ if (debugObserver)
+ {
+ StringVariantBaseMap map;
+ map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
+ if (discard.all)
+ {
+ map["Proportion Updated Poses"] = new Variant(0.f);
+ }
+ debugObserver->setDebugChannel(getName(), map);
+ }
+
+ if (discard.all)
+ {
+ return;
+ }
+
+ {
+ std::scoped_lock lock(memoryMutex);
+ RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
+
+ if (segment.robot->hasRobotNode(calibration.robotNode))
+ {
+ VirtualRobot::RobotNodePtr robotNode = segment.robot->getRobotNode(calibration.robotNode);
+ float value = robotNode->getJointValue();
+ robotNode->setJointValue(value + calibration.offset);
+ }
+
+ TIMING_START(tCommitObjectPoses);
+ Segment::CommitStats stats =
+ segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+ TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
+ { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
+ });
+ }
+
+ handleProviderUpdate(providerName);
+
+ TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
+ { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+
+
+ void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+ {
+ // Initialized to 0 on first access.
+ if (segment.providers.count(providerName) == 0)
+ {
+ segment.providers[providerName] = objpose::ProviderInfo();
+ }
+ }
+
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
+ {
+ TIMING_START(tGetObjectPoses);
+
+ TIMING_START(tGetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+
+ TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ {
+ TIMING_START(GetObjectPoses);
+
+ TIMING_START(GetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+
+ TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+
+ objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
+ const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
+ {
+ std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
+ std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
+
+ objpose::observer::RequestObjectsOutput output;
+
+ auto updateProxy = [&](const std::string & providerName)
+ {
+ if (proxies.count(providerName) == 0)
+ {
+ if (auto it = segment.providers.find(providerName); it != segment.providers.end())
+ {
+ proxies[providerName] = it->second.proxy;
+ }
+ else
+ {
+ ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
+ proxies[providerName] = nullptr;
+ }
+ }
+ };
+
+ if (input.provider.size() > 0)
+ {
+ providerRequests[input.provider] = input.request;
+ updateProxy(input.provider);
+ }
+ else
+ {
+ std::scoped_lock lock(memoryMutex);
+ for (const auto& objectID : input.request.objectIDs)
+ {
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
+ {
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ {
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
+ {
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
+ }
+ }
+ }
+
+ for (const auto& [providerName, request] : providerRequests)
+ {
+ if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
+ {
+ ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
+ << providerName << "' for " << request.relativeTimeoutMS << " ms.";
+ objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
+
+ int successful = 0;
+ for (const auto& [objectID, result] : providerOutput.results)
+ {
+ objpose::observer::ObjectRequestResult& res = output.results[objectID];
+ res.providerName = providerName;
+ res.result = result;
+ successful += int(result.success);
+ }
+ ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
+ }
+ }
+ return output;
+ }
+
+ objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers;
+ }
+
+ Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return simox::alg::get_keys(segment.providers);
+ }
+
+ objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.getProviderInfo(providerName);
+ }
+
+ bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers.count(providerName) > 0;
+ }
+
+
+ objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
+ const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.attachObjectToRobotNode(input);
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachObjectFromRobotNode(input);
+ }
+
+ objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachAllObjectsFromRobotNodes(input);
+ }
+
+
+ objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ }
+
+ objpose::SignalHeadMovementOutput
+ SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ return robotHead.signalHeadMovement(input);
+ }
+
+
+ void SegmentAdapter::visualizeRun()
+ {
+ CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+ while (visu.updateTask && !visu.updateTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(visuMutex);
+
+ if (visu.enabled)
+ {
+ TIMING_START(Visu);
+
+ objpose::ObjectPoseSeq objectPoses;
+ ObjectFinder objectFinder;
+ visu.minConfidence = -1;
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ objectPoses = segment.getObjectPoses(now);
+ objectFinder = segment.objectFinder;
+ if (segment.decay.enabled)
+ {
+ visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
+ }
+ }
+ const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+ arviz.commit(layers);
+
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+ }
+
+
+
+ void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ this->visu.setup(adapter.visu);
+ this->segment.setup(adapter.segment);
+ this->decay.setup(adapter.segment.decay);
+ this->robotHead.setup(adapter.robotHead);
+
+ layout = VBoxLayout
+ {
+ this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
+ VSpacer()
+ };
+
+ group.setLabel("Instance");
+ group.addChild(layout);
+ }
+
+ void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
+ {
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ {
+ std::scoped_lock lock(adapter.visuMutex);
+ this->visu.update(adapter.visu);
+ }
+ {
+ std::scoped_lock lock(adapter.memoryMutex);
+ this->segment.update(adapter.segment);
+ this->decay.update(adapter.segment.decay);
+ }
+ {
+ std::scoped_lock lock(adapter.robotHeadMutex);
+ this->robotHead.update(adapter.robotHead);
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1e356e53c77608937b0df952798d0427a3cc86b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -0,0 +1,169 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem_objects::Adapter
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Decay.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Visu.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ /**
+ * @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
+ */
+ class SegmentAdapter :
+ virtual public armarx::Logging
+ , virtual public armarx::armem::server::ObjectInstanceSegmentInterface
+ {
+ public:
+
+ SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ std::string getName() const;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+ void connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ );
+
+
+ // ObjectPoseTopic interface
+ public:
+ virtual void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
+ virtual void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+
+ // ObjectInstanceSegmentInterface interface
+ public:
+
+ // OBJECT POSES
+
+ virtual objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+ virtual objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+
+ // PROVIDER INFORMATION
+
+ virtual bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
+
+
+ // REQUESTING
+
+ virtual objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+ // ATTACHING
+
+ virtual objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
+
+ virtual objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
+
+ // HEAD MOVEMENT SIGNALS
+
+ virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+
+
+ private:
+
+ void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+ void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+ void handleProviderUpdate(const std::string& providerName);
+
+
+ // Visualization
+
+ void visualizeRun();
+
+
+ private:
+
+ viz::Client arviz;
+ DebugObserverInterfacePrx debugObserver;
+
+ instance::Segment segment;
+ std::mutex& memoryMutex;
+
+ instance::RobotHeadMovement robotHead;
+ std::mutex robotHeadMutex;
+
+ instance::Visu visu;
+ std::mutex visuMutex;
+
+
+ struct Calibration
+ {
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ };
+ Calibration calibration;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::VBoxLayout layout;
+
+ instance::Visu::RemoteGui visu;
+ instance::Segment::RemoteGui segment;
+ instance::Decay::RemoteGui decay;
+ instance::RobotHeadMovement::RemoteGui robotHead;
+
+ void setup(const SegmentAdapter& adapter);
+ void update(SegmentAdapter& adapter);
+ };
+
+ };
+
+}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
similarity index 85%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 11f8a3ebbda0911471d02e721f71f472e0b44c47..cc85dc53c51871fad99a57b044e823c49ed6f224 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -1,11 +1,13 @@
#include "Visu.h"
+#include <SimoxUtility/math/pose.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -30,7 +32,7 @@ namespace armarx::objpose::observer
std::vector<viz::Layer> Visu::visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
@@ -42,7 +44,7 @@ namespace armarx::objpose::observer
}
std::vector<viz::Layer> Visu::visualizeCommit(
- const ObjectPoseSeq& objectPoses,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
std::map<std::string, viz::Layer> layers;
@@ -57,7 +59,7 @@ namespace armarx::objpose::observer
return it->second;
};
- for (const ObjectPose& objectPose : objectPoses)
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
}
@@ -67,11 +69,11 @@ namespace armarx::objpose::observer
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
- for (const ObjectPose& objectPose : objectPoses)
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
visualizeObjectPose(layer, objectPose, objectFinder);
}
@@ -80,7 +82,7 @@ namespace armarx::objpose::observer
void Visu::visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
const bool show = not(objectPose.confidence < minConfidence);
@@ -91,7 +93,7 @@ namespace armarx::objpose::observer
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
- Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
{
viz::Object object = viz::Object(key).pose(pose);
if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
@@ -115,9 +117,10 @@ namespace armarx::objpose::observer
if (oobbs && objectPose.localOOBB)
{
- const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
- layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
- .color(simox::Color::lime(255, 64)));
+ const simox::OrientedBoxf oobb = inGlobalFrame
+ ? objectPose.oobbGlobal().value()
+ : objectPose.oobbRobot().value();
+ layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
}
if (objectFrames)
{
@@ -153,7 +156,7 @@ namespace armarx::objpose::observer
row++;
grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
row++;
- grid.add(Label("Alpha By Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
row++;
grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
row++;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index fcdfd4f240a903dc505bd7eb3732c6242838e0e4..1cd1d41c835e56e820ce3db8b3397b1e117d8202 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -14,7 +14,7 @@ namespace armarx
{
class ObjectFinder;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -28,26 +28,26 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
std::vector<viz::Layer> visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
const ObjectFinder& objectFinder
) const;
/// Visualize the given object poses, with one layer per provider.
std::vector<viz::Layer> visualizeCommit(
- const ObjectPoseSeq& objectPoses,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
void visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder
) const;
@@ -56,7 +56,7 @@ namespace armarx::objpose::observer
viz::Client arviz;
- bool enabled = false;
+ bool enabled = true;
float frequencyHz = 25;
bool inGlobalFrame = true;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index d9d22766cf2f51e163758c27c962661a3cb6758f..b038334fbb5dcc6ac39206c33174320f6b8e44d0 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -182,7 +182,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for copy \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + getFullCppTypename() + "& c)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(const " + getFullCppTypename() + "& c)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = c.value;");
m->setBlock(b);
@@ -195,7 +195,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + enumName + " v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(" + enumName + " v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = v;");
m->setBlock(b);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
index 77c73fe081061568605d3057e21651c77128ebf1..f577903a4524da4ce220d6d402e9719f1afdac7a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
@@ -229,153 +229,129 @@ namespace armarx::aron::visitor
virtual bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(TimeTypeNavigator& type, const std::string& key, const IceUtil::Time& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
@@ -392,76 +368,3 @@ namespace armarx::aron::visitor
};
}
-
-
-/* Copy-and-paste example below.
- * Remove functions you don't need.
- * Add `const std::string& key, ` to parameter list if you need an items's key
- * in the parent container.
- */
-
-#if 0
-
-struct MyDerivedAronTypeVisitor : public armarx::aron::visitor::AronTypeVisitor
-{
-
- bool visitEnter(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitExit(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitEnter(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
- bool visitExit(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
-
- bool visit(BoolTypeNavigator& b) override
- {
- (void) b;
- return true;
- }
- bool visit(DoubleTypeNavigator& d) override
- {
- (void) d;
- return true;
- }
- bool visit(FloatTypeNavigator& f) override
- {
- (void) f;
- return true;
- }
- bool visit(IntTypeNavigator& i) override
- {
- (void) i;
- return true;
- }
- bool visit(LongTypeNavigator& l) override
- {
- (void) l;
- return true;
- }
- bool visit(NDArrayTypeNavigator& array) override
- {
- (void) array;
- return true;
- }
- bool visit(StringTypeNavigator& string) override
- {
- (void) string;
- return true;
- }
-
-};
-
-#endif
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 5743f098b27b9de73ed56047ee020089a046af22..9babf0b23f1eeae6bc2bc01e9238d93809c6c40c 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -49,7 +49,7 @@ set(LIB_FILES
CartesianVelocityControllerWithRamp.cpp
CartesianNaturalPositionController.cpp
#CartesianNaturalVelocityController.cpp
-
+
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
@@ -102,7 +102,7 @@ set(LIB_HEADERS
CartesianNaturalPositionController.h
#CartesianNaturalVelocityController.h
EigenHelpers.h
-
+
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
@@ -114,3 +114,5 @@ set(LIB_HEADERS
add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_library(RobotAPI::Core ALIAS RobotAPICore)