diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3a4902c6d8abc0c4f29442364abd8d820d95ebfb..2185d2cc3a7ead117712b3c0ba01f57eb76ee6dc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ depends_on_armarx_package(ArmarXGui)
find_package(DMP QUIET)
find_package(OpenCV QUIET)
find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+find_package(manif QUIET)
add_subdirectory(source)
diff --git a/scenarios/ArVizExample/config/ArVizInteractExample.cfg b/scenarios/ArVizExample/config/ArVizInteractExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0892ab764555b0fba8759653513371035dc9b45d
--- /dev/null
+++ b/scenarios/ArVizExample/config/ArVizInteractExample.cfg
@@ -0,0 +1,204 @@
+# ==================================================================
+# ArVizInteractExample 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.ArVizInteractExample.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ArVizInteractExample.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ArVizInteractExample.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.ArVizInteractExample.EnableProfiling = false
+
+
+# ArmarX.ArVizInteractExample.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.ArVizInteractExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizInteractExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ObjectName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
new file mode 100644
index 0000000000000000000000000000000000000000..9e4ef2810eae6db0c1a475e9b220fae7e18418b6
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArticulatedObjectWriterExample" creation="2022-10-10.18:54:39" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/ArVizStorage.cfg b/scenarios/ArticulatedObjectWriterExample/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/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 is 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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e6c221120c7a8f33693caf9dc5fa229875dabb4a
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
@@ -0,0 +1,269 @@
+# ==================================================================
+# ArticulatedObjectLocalizerExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName:
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName = PriorKnowledgeData
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName: Name of this provider
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class:
+# Attributes:
+# - Default: mobile-dishwasher
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = cabinet
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset:
+# Attributes:
+# - Default: Kitchen
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = RBO-articulated-object-dataset
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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/ArticulatedObjectWriterExample/config/MemoryNameSystem.cfg b/scenarios/ArticulatedObjectWriterExample/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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/ArticulatedObjectWriterExample/config/ObjectMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..95961b39febd5772f9f3ee485552631f407b73cc
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
@@ -0,0 +1,701 @@
+# ==================================================================
+# 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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
+# 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.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.attachments.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Attachments
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName = Attachments
+
+
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Building/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
+
+
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize: Maximal size of the Class entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName: Name of the Class core segment.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName = Class
+
+
+# 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.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.robotName: Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
+# 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.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
+# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory: Directory in Package/data/Package/ containing the scene snapshots.
+# Attributes:
+# - Default: scenes
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/scenes/'.
+# You can also specify a ; separated list of scenes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName: Name of the Instance core segment.
+# Attributes:
+# - Default: Instance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName = Instance
+
+
+# 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: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
+
+
+# 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.gaussians.position: Enable showing pose gaussians (orientation part).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced: Displace center orientation (co)variance circle arrows along their rotation axis.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale: Scaling of pose gaussians (orientation part).
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
+# 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.mem.inst.visu.predictions.linear.ghostAlpha: Alpha of linear prediction ghosts.
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow: Show arrows from current object poses to the linearly predicted ones.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame: Show frames at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost: Show ghosts at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow: The time window (in seconds) into the past to perform the regression on.
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow = 2
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ObjectMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# 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.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
+
+
+# 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.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/ArticulatedObjectWriterExample/config/RemoteGuiProviderApp.cfg b/scenarios/ArticulatedObjectWriterExample/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5839620e5e3c5df7e080f39405a021bdbcd4a0ea
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
@@ -0,0 +1,361 @@
+# ==================================================================
+# RobotStateMemory 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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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.RobotStateMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.RobotStateMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotStateMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.EnableProfiling = false
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ObjectName = ""
+
+
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix: Prefix of all sensor values.
+# Attributes:
+# - Default: sens.*
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix = sens.*
+
+
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency: The frequency to store values in Hz. All other values get discarded. Minimum is 1, max is 100.000000.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency = 50
+
+
+# ArmarX.RobotStateMemory.RobotUnitName: Name of the RobotUnit
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnitName = ""
+
+
+# ArmarX.RobotStateMemory.WaitForRobotUnit: Add the robot unit as dependency to the component. This memory requires a running RobotUnit, therefore we should add it as explicit dependency.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.WaitForRobotUnit = false
+
+
+# ArmarX.RobotStateMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.MemoryName = RobotState
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize: Maximal size of the Description entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName: Name of the Description core segment.
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName = Description
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize: Maximal size of the Localization entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName: Name of the Localization core segment.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName = Localization
+
+
+# ArmarX.RobotStateMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.RobotStateMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.ltm.enabled = false
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize: Maximal size of the Proprioception entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName: Name of the Proprioception core segment.
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName = Proprioception
+
+
+# ArmarX.RobotStateMemory.mem.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.enabled = true
+
+
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz = 25
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStateMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.prediction.TimeWindow = 2
+
+
+# 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/ArticulatedObjectWriterExample/config/global.cfg b/scenarios/ArticulatedObjectWriterExample/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..fdfd38bb91a262ada278ef24d00788bd824755ed
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArticulatedObjectWriterExample
+# ==================================================================
+
diff --git a/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg b/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..827455bc124e50f821cb0552776f6a0335f5a5ee
--- /dev/null
+++ b/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg
@@ -0,0 +1,244 @@
+# ==================================================================
+# ObjectMemoryEditor 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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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.InteractiveMemoryEditor.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.InteractiveMemoryEditor.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.InteractiveMemoryEditor.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.InteractiveMemoryEditor.Editor.ConfidenceThreshold: Only objects with a confidence greater than this value are shown.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.Editor.ConfidenceThreshold = 0
+
+
+# ArmarX.InteractiveMemoryEditor.Editor.ObjectScaling: Scaling factor that is applied to all intractable objects.
+# Attributes:
+# - Default: 1.00999999
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.Editor.ObjectScaling = 1.00999999
+
+
+# ArmarX.InteractiveMemoryEditor.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.InteractiveMemoryEditor.EnableProfiling = false
+
+
+# ArmarX.InteractiveMemoryEditor.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.InteractiveMemoryEditor.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectName = ""
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectPoseTopicName: Name of the object pose topic.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectPoseTopicName = ObjectPoseTopic
+
+
+# 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/edit_object_memory/config/global.cfg b/scenarios/edit_object_memory/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f49a7112c97e55b4b0956b7811c87f912ed9886a
--- /dev/null
+++ b/scenarios/edit_object_memory/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario edit_object_memory
+# ==================================================================
+
diff --git a/scenarios/edit_object_memory/edit_object_memory.scx b/scenarios/edit_object_memory/edit_object_memory.scx
new file mode 100644
index 0000000000000000000000000000000000000000..3a4e04db004a526197b0571500276fca63c646a6
--- /dev/null
+++ b/scenarios/edit_object_memory/edit_object_memory.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="edit_object_memory" creation="2022-10-06.15:51:45" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemoryEditor" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cafd45782108fea5f823933938463751733671b6
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
@@ -0,0 +1,238 @@
+# ==================================================================
+# RobotStatePredictionClientExample 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_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# 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.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.EnableProfiling = false
+
+
+# ArmarX.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStatePredictionClientExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.ObjectName = ""
+
+
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds: How far into the future to predict [s].
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds = 1
+
+
+# ArmarX.RobotStatePredictionClientExample.p.robotName: Name of the robot.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.robotName = Armar6
+
+
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz: Frequency of predictions [Hz].
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz = 10
+
+
+# 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/robot_state_prediction_example/config/global.cfg b/scenarios/robot_state_prediction_example/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..06e504e09e3cd61958e2dc2c5d617882978b41d7
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario robot_state_prediction_example
+# ==================================================================
+
diff --git a/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
new file mode 100644
index 0000000000000000000000000000000000000000..2d5de4f038dc12a381b8bc22ae3113ff589b2707
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="robot_state_prediction_example" creation="2022-08-15.11:20:29" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="RobotStatePredictionClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a3a07e6eeea9b3ab866217b2d1c2973714f4fce
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
@@ -0,0 +1,70 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h>
+#include <RobotAPI/libraries/aron_component_config/RemoteGui.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "AronComponentConfigExample.h"
+
+namespace armarx
+{
+
+ void AronComponentConfigExample::onInitComponent()
+ {
+
+ }
+
+ void AronComponentConfigExample::onConnectComponent()
+ {
+ }
+
+ void AronComponentConfigExample::onDisconnectComponent()
+ {
+ }
+
+ void AronComponentConfigExample::onExitComponent()
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr AronComponentConfigExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new AronComponentConfigExamplePropertyDefinitions(getConfigIdentifier());
+ auto& config = getConfigWriteBuffer();
+ config.boolMember=true;
+ config.floatMember=100.0f;
+ config.intMember=1000;
+ config.stringMember="initial";
+ config.longMember=0;
+ config.subMember.subsubMember.intListMember.insert(config.subMember.subsubMember.intListMember.end(), { 1, 2, 3, 4, 5, 6 });
+ config.subMember.subsubMember.stringListMember.insert(config.subMember.subsubMember.stringListMember.end(), {"a", "b", "c", "d", "e"});
+ commitConfigWriteBuffer();
+ return defs;
+ }
+
+ std::string AronComponentConfigExample::getDefaultName() const
+ {
+ return "ComponentConfigTest";
+ }
+
+ AronComponentConfigExamplePropertyDefinitions::AronComponentConfigExamplePropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(std::move(prefix)) {}
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..d030e645b4762a8d75a7bcb2deac50bb3bd8ec7d
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
@@ -0,0 +1,62 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.aron.generated.h>
+#include <RobotAPI/libraries/aron_component_config/ComponentPlugin.h>
+
+namespace armarx
+{
+
+ class AronComponentConfigExamplePropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ AronComponentConfigExamplePropertyDefinitions(std::string prefix);
+ };
+
+ class AronComponentConfigExample :
+ virtual public armarx::AronComponentConfigPluginUser<armarx::component_config::aron::TestConfig>,
+ virtual public armarx::Component
+ {
+ public:
+ std::string getDefaultName() const override;
+
+ /// @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;
+
+ };
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt b/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c219bee7094345ac6eb814b9568f711d37431f59
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt
@@ -0,0 +1,26 @@
+armarx_component_set_name("AronComponentConfigExample")
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces
+ RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins # for ArViz and other plugins
+ aron_component_config
+ )
+
+set(SOURCES
+ AronComponentConfigExample.cpp
+ )
+set(HEADERS
+ AronComponentConfigExample.h
+ )
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ ${ARMARX_COMPONENT_NAME}
+ ARON_FILES
+ aron/ComponentConfig.xml
+)
+
+#generate the application
+armarx_generate_and_add_component_executable()
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e449a761672809f64dc903d108b7f6af13aa3878
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
@@ -0,0 +1,86 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <GenerateTypes>
+
+ <IntEnum name='armarx::component_config::aron::EnumTest'>
+ <EnumValue key="Foo" value="0"/>
+ <EnumValue key="Bar" value="1"/>
+ <EnumValue key="Baz" value="2"/>
+ <EnumValue key="Qux" value="3"/>
+ </IntEnum>
+
+ <Object name='armarx::component_config::aron::SubSubConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='longMember'>
+ <long/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='enumMember'>
+ <armarx::component_config::aron::EnumTest/>
+ </ObjectChild>
+ <ObjectChild key='stringListMember'>
+ <List>
+ <string/>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='intListMember'>
+ <List>
+ <int/>
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::component_config::aron::SubConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='doubleMember'>
+ <double/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='subsubMember'>
+ <armarx::component_config::aron::SubSubConfig />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::component_config::aron::TestConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='longMember'>
+ <long/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='subMember'>
+ <armarx::component_config::aron::SubConfig />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 8e172a97d95ed3ce515743a877c8c5bb3ed168d1..77943d887c6833b87a730b1ce45b926273795ad2 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,29 +32,25 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::articulated_object
{
- ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem())),
- articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem()))
+ ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample()
{
+ addPlugin(articulatedObjectReaderPlugin);
+ addPlugin(articulatedObjectWriterPlugin);
}
- armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ArticulatedObjectLocalizerExample::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -63,60 +59,74 @@ namespace armarx::articulated_object
defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
- // Reader will override some properties of writer.
- articulatedObjectWriter->registerPropertyDefinitions(defs);
- articulatedObjectReader->registerPropertyDefinitions(defs);
+ defs->optional(p.obj.dataset, "p.obj.dataset", "");
+ defs->optional(p.obj.className, "p.obj.class", "");
+
return defs;
}
- std::string ArticulatedObjectLocalizerExample::getDefaultName() const
+ std::string
+ ArticulatedObjectLocalizerExample::getDefaultName() const
{
return "ArticulatedObjectLocalizerExample";
}
- void ArticulatedObjectLocalizerExample::onInitComponent()
+ void
+ ArticulatedObjectLocalizerExample::onInitComponent()
{
+ auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
+ auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
+
// Reader overwrote property registered property of articulatedObjectWriter.
- articulatedObjectWriter->setProviderName(articulatedObjectReader->getProviderName());
+ articulatedObjectWriter.setProviderName(articulatedObjectReader.getProviderName());
}
- void ArticulatedObjectLocalizerExample::onConnectComponent()
+ void
+ ArticulatedObjectLocalizerExample::onConnectComponent()
{
- articulatedObjectWriter->connect();
- articulatedObjectReader->connect();
-
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::Now();
task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
- this, &ArticulatedObjectLocalizerExample::run,
+ this,
+ &ArticulatedObjectLocalizerExample::run,
static_cast<int>(1000.f / p.updateFrequency));
task->start();
}
- void ArticulatedObjectLocalizerExample::onDisconnectComponent()
+ void
+ ArticulatedObjectLocalizerExample::onDisconnectComponent()
{
task->stop();
}
- void ArticulatedObjectLocalizerExample::onExitComponent()
+ void
+ ArticulatedObjectLocalizerExample::onExitComponent()
{
}
- VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
+ VirtualRobot::RobotPtr
+ ArticulatedObjectLocalizerExample::createArticulatedObject()
{
- const std::string dishwasherName = "Kitchen/mobile-dishwasher";
+ auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
+
+ const std::string dishwasherName = p.obj.dataset + "/" + p.obj.className;
- const auto descriptions = articulatedObjectReader->queryDescriptions(armem::Time::Now());
+ const auto descriptions = articulatedObjectReader.queryDescriptions(armem::Time::Now());
ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ for(const auto& description: descriptions)
+ {
+ ARMARX_INFO << "- " << description.name;
+ }
+
const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
- { return desc.name == dishwasherName; });
+ descriptions.begin(),
+ descriptions.end(),
+ [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
+ { return desc.name == dishwasherName; });
if (it == descriptions.end())
{
@@ -133,21 +143,21 @@ namespace armarx::articulated_object
return nullptr;
}
- obj->setName("MobileDishwasher0");
- obj->setType(it->name);
+ obj->setName("0"); // aka instance name
+ obj->setType(it->name); // aka dataset/class name
return obj;
}
-
- void ArticulatedObjectLocalizerExample::run()
+ void
+ ArticulatedObjectLocalizerExample::run()
{
- if (dishwasher == nullptr)
+ if (articulatedObject == nullptr)
{
- dishwasher = createDishwasher();
+ articulatedObject = createArticulatedObject();
- if (dishwasher == nullptr) // still
+ if (articulatedObject == nullptr) // still
{
return;
}
@@ -156,21 +166,24 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const armem::Time now = armem::Time::Now();
- const float t = float((now - start).toSecondsDouble());
+ const float t = static_cast<float>((now - start).toSecondsDouble());
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
- const std::map<std::string, float> jointValues
+ auto jointValues = articulatedObject->getJointValues();
+
+ for (auto& [name, jointValue] : jointValues)
{
- {"dishwasher_door_joint", M_PIf32 / 2 * k},
- {"drawer_joint", 350 * k}
- };
+ const auto node = articulatedObject->getRobotNode(name);
+ jointValue = node->unscaleJointValue(k, 0, 1);
+ }
- dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
- dishwasher->setJointValues(jointValues);
+ articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+ articulatedObject->setJointValues(jointValues);
- articulatedObjectWriter->storeArticulatedObject(dishwasher, now);
+ auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
+ articulatedObjectWriter.storeArticulatedObject(articulatedObject, now);
}
} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index d401eed5e741d3d797dfaa8a47f347f4616a53db..384068048dd1ab8d1457065d15873de08ff7b82b 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -3,17 +3,19 @@
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
@@ -53,8 +55,8 @@ namespace armarx::articulated_object
void run();
private:
- VirtualRobot::RobotPtr createDishwasher();
- std::shared_ptr<VirtualRobot::Robot> dishwasher;
+ VirtualRobot::RobotPtr createArticulatedObject();
+ std::shared_ptr<VirtualRobot::Robot> articulatedObject;
/// Reference timestamp for object movement
armem::Time start;
@@ -63,13 +65,23 @@ namespace armarx::articulated_object
armarx::DebugObserverInterfacePrx debugObserver;
- std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectWriter> articulatedObjectWriter;
- std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectReader>
- articulatedObjectReader;
+ armem::client::plugins::ReaderWriterPlugin<
+ ::armarx::armem::articulated_object::ArticulatedObjectWriter>*
+ articulatedObjectWriterPlugin = nullptr;
+
+ armem::client::plugins::ReaderWriterPlugin<
+ ::armarx::armem::articulated_object::ArticulatedObjectReader>*
+ articulatedObjectReaderPlugin = nullptr;
struct Properties
{
float updateFrequency{25.F};
+
+ struct
+ {
+ std::string dataset = "Kitchen";
+ std::string className = "mobile-dishwasher";
+ } obj;
} p;
};
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 7debb7e6f4aa280c4e28cc1a40bbc59af0bf25c3..11908aeda6ade0ff4b46f63d0590c88d007e51c9 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(units)
add_subdirectory(armem)
add_subdirectory(skills)
+add_subdirectory(AronComponentConfigExample)
add_subdirectory(ArticulatedObjectLocalizerExample)
add_subdirectory(ArViz)
@@ -32,3 +33,5 @@ add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
+
+add_subdirectory(ObjectMemoryEditor)
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt b/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c9023550e1d93a2d4bd1b8a0a61b8a5fdc7bd5d
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("ObjectMemoryEditor")
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCoreComponentPlugins
+ # RobotAPI
+ RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+)
+
+set(SOURCES
+ ObjectMemoryEditor.cpp
+ InteractionObserver.cpp
+ Editor.cpp
+)
+set(HEADERS
+ ObjectMemoryEditor.h
+ InteractionObserver.h
+ Editor.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..74bdd96793868911322b960af2973a0576809e93
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
@@ -0,0 +1,547 @@
+#include "Editor.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <utility>
+
+namespace armarx
+{
+ Editor::Editor(viz::Client& client,
+ Properties properties,
+ std::function<void(objpose::ProvidedObjectPoseSeq &)> pushToMemory,
+ std::function<objpose::ObjectPoseSeq(void)> pullFromMemory)
+ : properties(std::move(properties))
+ , client(client)
+ , pushToMemory(std::move(pushToMemory))
+ , pullFromMemory(std::move(pullFromMemory))
+ , isCommitRequired(false)
+ , isUpdateRequired(true)
+ , isMemoryVizRequired(true)
+ , isMetaVizRequired(true)
+ {
+ placeholderOptions.emplace_back("Remove");
+ placeholderOptions.emplace_back(lineString);
+
+ for (auto const& availableObject : this->properties.availableObjects)
+ {
+ placeholderOptions.push_back(armarx::fromIce(availableObject).str());
+ }
+ }
+
+ void Editor::step()
+ {
+ viz::StagedCommit stage = client.stage();
+
+ if (isUpdateRequired)
+ {
+ storedPoses = update();
+
+ isUpdateRequired = false;
+ }
+
+ if (isResetRequired)
+ {
+ reset();
+
+ isResetRequired = false;
+ }
+
+ if (isMemoryVizRequired)
+ {
+ visualizeMemory();
+ stage.add(memoryLayer);
+
+ isMemoryVizRequired = false;
+ }
+
+ if (isMetaVizRequired)
+ {
+ visualizeMeta();
+ stage.add(metaLayer);
+
+ isMetaVizRequired = false;
+ }
+
+ if (isCommitRequired)
+ {
+ commit();
+
+ isCommitRequired = false;
+ }
+
+ observer.requestInteractions(stage);
+
+ viz::CommitResult result = client.commit(stage);
+
+ observer.process(result.interactions());
+ }
+
+ objpose::ObjectPoseSeq
+ Editor::update()
+ {
+ objpose::ObjectPoseSeq newRequestedPoses = pullFromMemory();
+
+ isMemoryVizRequired = true;
+
+ return newRequestedPoses;
+ }
+
+ void Editor::reset()
+ {
+ changes.clear();
+
+ isMemoryVizRequired = true;
+ }
+
+ void Editor::commit()
+ {
+ changes.moveNewObjectsTo(storedPoses);
+
+ objpose::ProvidedObjectPoseSeq providingPoses;
+ objpose::ObjectPoseSeq remainingPoses;
+
+ remainingPoses.reserve(storedPoses.size());
+ for (objpose::ObjectPose & current : storedPoses)
+ {
+ bool isChanged = changes.applyTo(current);
+
+ if (isChanged)
+ {
+ providingPoses.push_back(current.toProvidedObjectPoseGlobal());
+ objpose::ProvidedObjectPose& providing = providingPoses.back();
+
+ providing.providerName = properties.providerName;
+ providing.timestamp = DateTime::Now();
+ }
+
+ if (current.confidence > properties.confidenceThreshold)
+ {
+ remainingPoses.push_back(current);
+ }
+ }
+
+ pushToMemory(providingPoses);
+
+ changes.clear();
+ storedPoses = remainingPoses;
+
+ isMemoryVizRequired = true;
+ }
+
+ void Editor::visualizeMemory()
+ {
+ observer.clearObservedLayer(memoryLayer);
+
+ for (objpose::ObjectPose & objectPose: storedPoses)
+ {
+ if (objectPose.confidence > properties.confidenceThreshold)
+ {
+ visualizeObject(objectPose);
+ }
+ }
+
+ changes.visualizeNewObjects();
+ }
+
+ void Editor::visualizeObject(objpose::ObjectPose& objectPose)
+ {
+ VisualizationDescription description = changes.buildVisualizationDescription(objectPose);
+
+ viz::InteractionDescription interaction = viz::interaction().selection();
+
+ if (description.allowTransforming)
+ {
+ interaction.transform().hideDuringTransform();
+ }
+
+ if (not description.options.empty())
+ {
+ interaction.contextMenu(description.options);
+ }
+
+ viz::Object object =
+ viz::Object(objectPose.objectID.str())
+ .pose(description.transform * objectPose.objectPoseGlobal)
+ .scale(properties.objectScaling)
+ .fileByObjectFinder(objectPose.objectID)
+ .enable(interaction);
+
+ if (description.alpha.has_value())
+ {
+ object.alpha(description.alpha.value());
+ }
+
+ if (description.color.has_value())
+ {
+ object.overrideColor(description.color.value());
+ }
+
+ observer.addObserved(memoryLayer, object)
+ .onContextMenu(description.cloneIndex, [this, &objectPose]
+ {
+ changes.cloneObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.deleteIndex, [this, &objectPose]
+ {
+ changes.deleteObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.resetIndex, [this, &objectPose]
+ {
+ changes.resetObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.prototypeIndex, [this, &objectPose]
+ {
+ const float defaultExtents = 100;
+ simox::OrientedBoxf box(objectPose.objectPoseGlobal, Eigen::Vector3f(defaultExtents, defaultExtents, defaultExtents));
+
+ box = objectPose.oobbGlobal().value_or(box);
+ box = box.transformed(changes.getTransform(objectPose));
+
+ placeholders.addPlaceholder(box);
+ isMetaVizRequired = true;
+ })
+ .onContextMenu(description.commitIndex, [this]
+ {
+ isCommitRequired = true;
+ })
+ .onContextMenu(description.updateIndex, [this]
+ {
+ isUpdateRequired = true;
+ })
+ .onContextMenu(description.resetAllIndex, [this]
+ {
+ isResetRequired = true;
+ })
+ .onTransformEnd([this, &objectPose](const Eigen::Matrix4f& transform)
+ {
+ changes.moveObject(objectPose, transform);
+ isMemoryVizRequired = true;
+ });
+ }
+
+ void Editor::visualizeMeta()
+ {
+ observer.clearObservedLayer(metaLayer);
+
+ placeholders.visualizePlaceholders();
+ }
+
+ void Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
+ {
+ viz::InteractionDescription interaction = viz::interaction()
+ .selection().transform().hideDuringTransform().contextMenu(placeholderOptions);
+
+ viz::Box box = viz::Box("placeholder_" + std::to_string(id))
+ .set(placeholder.box.transformed(placeholder.transform))
+ .color(simox::Color::yellow(255, 128))
+ .enable(interaction);
+
+ auto& observation = observer.addObserved(metaLayer, box)
+ .onContextMenu(0, [this, id]()
+ {
+ placeholders.removePlaceholder(id);
+ isMetaVizRequired = true;
+ })
+ .onTransformEnd([this, id](Eigen::Matrix4f const& transform)
+ {
+ placeholders.movePlaceholder(id, transform);
+ isMetaVizRequired = true;
+ });
+
+ for (size_t index = 2; index < placeholderOptions.size(); index++)
+ {
+ std::string const& object = placeholderOptions[index];
+
+ observation.onContextMenu(index, [this, id, &object]
+ {
+ placeholders.specifyObject(id, object, changes);
+
+ isMetaVizRequired = true;
+ isMemoryVizRequired = true;
+ });
+ }
+ }
+
+ void ChangeState::clear()
+ {
+ changed.clear();
+ newPoses.clear();
+ }
+
+ void ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
+ {
+ seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
+ newPoses.clear();
+ }
+
+ bool ChangeState::applyTo(objpose::ObjectPose& pose)
+ {
+ auto iterator = changed.find(pose.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+
+ if (change.kind == DELETE)
+ {
+ pose.confidence = 0;
+ }
+
+ pose.objectPoseGlobal = change.transform * pose.objectPoseGlobal;
+ }
+
+ return isChanged;
+ }
+
+ void ChangeState::visualizeNewObjects()
+ {
+ for (objpose::ObjectPose & objectPose: newPoses)
+ {
+ editor->visualizeObject(objectPose);
+ }
+ }
+
+ VisualizationDescription ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
+ {
+ VisualizationDescription description;
+
+ auto iterator = changed.find(object.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ ChangeKind kind = MOVE;
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+ kind = change.kind;
+ }
+
+ description.allowTransforming = kind != DELETE;
+
+ size_t currentIndex = 0;
+
+ if (kind == MOVE)
+ {
+ description.options.emplace_back("Clone");
+ description.cloneIndex = currentIndex++;
+
+ description.options.emplace_back("Delete");
+ description.deleteIndex = currentIndex++;
+ }
+
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+
+ description.options.emplace_back("Reset");
+ description.resetIndex = currentIndex++;
+
+ description.transform = change.transform;
+
+ const float alpha = 0.5;
+
+ switch (kind)
+ {
+ case MOVE:
+ description.alpha = alpha;
+ break;
+
+ case CREATE:
+ description.color = simox::Color(0.0F, 1.0F, 0.0F, alpha);
+ break;
+
+ case DELETE:
+ description.color = simox::Color(1.0F, 0.0F, 0.0F, alpha);
+ break;
+ }
+ }
+
+ description.options.emplace_back("Create Placeholder");
+ description.prototypeIndex = currentIndex++;
+
+ description.options.emplace_back(Editor::lineString);
+ currentIndex++;
+
+ description.options.emplace_back("Commit All Changes");
+ description.commitIndex = currentIndex++;
+
+ description.options.emplace_back("Update Unchanged");
+ description.updateIndex = currentIndex++;
+
+ description.options.emplace_back("Reset All");
+ description.resetAllIndex = currentIndex; // ++
+
+ return description;
+ }
+
+ void ChangeState::cloneObject(objpose::ObjectPose const& object)
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back(object);
+ newPose.objectID = object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
+
+ const float minOffset = 100;
+ float offset = minOffset;
+ if (object.localOOBB.has_value())
+ {
+ Eigen::Vector3f size = object.localOOBB.value().corner_max() - object.localOOBB.value().corner_min();
+ float objectOffset = size.maxCoeff() / 2;
+
+ offset = std::max(minOffset, objectOffset);
+ }
+
+ Change& clonedChange = changed[newPose.objectID.str()];
+ clonedChange.kind = CREATE;
+ // Heuristic: Don't shift in Z direction to ease creation in a horizontal plane.
+ clonedChange.transform = Eigen::Affine3f(Eigen::Translation3f(offset, offset, 0)).matrix();
+ clonedChange.iterator = std::prev(newPoses.end());
+
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, originalChange] = *iterator;
+ clonedChange.transform *= originalChange.transform;
+ }
+ }
+
+ void ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back();
+
+ armarx::ObjectID id(objectID);
+ newPose.objectID = id.withInstanceName(id.instanceName() + suffix);
+
+ newPose.providerName = editor->properties.providerName;
+ newPose.objectType = objpose::ObjectType::KnownObject;
+ newPose.isStatic = true;
+
+ newPose.objectPoseGlobal = pose;
+ newPose.confidence = 1;
+ newPose.timestamp = DateTime::Now();
+
+ std::optional<armarx::ObjectInfo> info = editor->objectFinder.findObject(id);
+ if (info.has_value())
+ {
+ newPose.localOOBB = info->loadOOBB();
+
+ if (newPose.localOOBB.has_value())
+ {
+ newPose.objectPoseGlobal = pose * newPose.localOOBB->transformation_centered().inverse();
+ }
+ }
+
+ Change& createdChange = changed[newPose.objectID.str()];
+ createdChange.kind = CREATE;
+ createdChange.transform = Eigen::Matrix4f::Identity();
+ createdChange.iterator = std::prev(newPoses.end());
+ }
+
+ void ChangeState::deleteObject(const objpose::ObjectPose& object)
+ {
+ changed[object.objectID.str()].kind = DELETE;
+ }
+
+ void ChangeState::resetObject(const objpose::ObjectPose& object)
+ {
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, change] = *iterator;
+
+ if (change.kind == CREATE)
+ {
+ newPoses.erase(change.iterator);
+ }
+
+ changed.erase(iterator);
+ }
+ }
+
+ void ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
+ {
+ Change& change = changed[object.objectID.str()];
+ change.transform = transform * change.transform;
+ }
+
+ Eigen::Matrix4f ChangeState::getTransform(const objpose::ObjectPose &object)
+ {
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, change] = *iterator;
+ transform = change.transform;
+ }
+
+ return transform;
+ }
+
+ void PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
+ {
+ size_t id = getID();
+
+ auto& entry = placeholders[id];
+ entry.placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
+ entry.isActive = true;
+ }
+
+ void PlaceholderState::visualizePlaceholders()
+ {
+ for (size_t id = 0; id < placeholders.size(); id++)
+ {
+ auto& entry = placeholders[id];
+
+ if (entry.isActive)
+ {
+ editor->visualizePlaceholder(entry.placeholder, id);
+ }
+ }
+ }
+
+ void PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
+ {
+ auto& placeholder = placeholders[id].placeholder;
+ placeholder.transform = transform * placeholder.transform;
+ }
+
+ void PlaceholderState::removePlaceholder(size_t id)
+ {
+ placeholders[id].isActive = false;
+
+ unusedIDs.push(id);
+ }
+
+ size_t PlaceholderState::getID()
+ {
+ if (unusedIDs.empty())
+ {
+ size_t id = placeholders.size();
+ placeholders.push_back({Placeholder(), false});
+ return id;
+ }
+
+ size_t id = unusedIDs.top();
+ unusedIDs.pop();
+ return id;
+ }
+
+ void PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
+ {
+ auto& placeholder = placeholders[id].placeholder;
+
+ Eigen::Matrix4f pose = placeholder.box.transformed(placeholder.transform).transformation_centered();
+ changeState.createObject(objectID, pose);
+
+ removePlaceholder(id);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1a6ebc2a168de7938e552b44f1d2144a915dd35
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -0,0 +1,181 @@
+#pragma once
+
+#include <list>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h>
+
+namespace armarx
+{
+
+ class Editor;
+
+
+ struct VisualizationDescription
+ {
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ bool allowTransforming = true;
+
+ std::vector<std::string> options {};
+ std::optional<float> alpha {};
+ std::optional<simox::Color> color {};
+
+ // Context menu indices.
+ size_t cloneIndex = std::numeric_limits<size_t>::max();
+ size_t deleteIndex = std::numeric_limits<size_t>::max();
+ size_t resetIndex = std::numeric_limits<size_t>::max();
+ size_t prototypeIndex = std::numeric_limits<size_t>::max();
+ size_t commitIndex = std::numeric_limits<size_t>::max();
+ size_t updateIndex = std::numeric_limits<size_t>::max();
+ size_t resetAllIndex = std::numeric_limits<size_t>::max();
+ };
+
+
+
+ class ChangeState
+ {
+ public:
+ explicit ChangeState(Editor* editor) : editor(editor) {} ;
+
+ void clear();
+ void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
+ bool applyTo(objpose::ObjectPose& pose);
+ void visualizeNewObjects();
+ VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
+ Eigen::Matrix4f getTransform(objpose::ObjectPose const& object);
+
+ void cloneObject(objpose::ObjectPose const& object);
+ void createObject(std::string const& objectID, Eigen::Matrix4f const& pose);
+ void deleteObject(objpose::ObjectPose const& object);
+ void resetObject(objpose::ObjectPose const& object);
+ void moveObject(objpose::ObjectPose const& object, Eigen::Matrix4f const& transform);
+
+ private:
+ enum ChangeKind
+ {
+ MOVE,
+ CREATE,
+ DELETE
+ };
+
+ using ObjectPoseList = std::list<objpose::ObjectPose>;
+
+ struct Change
+ {
+ ChangeKind kind = MOVE;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ ObjectPoseList::iterator iterator {};
+ };
+
+ Editor* editor;
+
+ ObjectPoseList newPoses;
+ std::map<std::string, Change> changed;
+ };
+
+
+
+ class PlaceholderState
+ {
+ public:
+ explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
+
+ struct Placeholder
+ {
+ simox::OrientedBoxf box;
+ Eigen::Matrix4f transform;
+ };
+
+ void addPlaceholder(simox::OrientedBoxf box);
+ void visualizePlaceholders();
+ void movePlaceholder(size_t id, Eigen::Matrix4f const& transform);
+ void removePlaceholder(size_t id);
+ void specifyObject(size_t id, std::string const& objectID, ChangeState& changeState);
+
+ private:
+ size_t getID();
+
+ Editor* editor;
+
+ std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
+ struct PlaceholderEntry
+ {
+ Placeholder placeholder;
+ bool isActive;
+ };
+ std::vector<PlaceholderEntry> placeholders;
+ };
+
+
+
+ class Editor
+ {
+ public:
+ struct Properties
+ {
+ std::string providerName;
+
+ float const& objectScaling;
+ float const& confidenceThreshold;
+
+ std::vector<data::ObjectID> const& availableObjects;
+ };
+
+
+ public:
+
+ explicit Editor(viz::Client& client,
+ Properties properties,
+ std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory,
+ std::function<objpose::ObjectPoseSeq(void)> pullFromMemory);
+ void step();
+
+ private:
+
+ void visualizeMemory();
+ void visualizeMeta();
+
+ void commit();
+ objpose::ObjectPoseSeq update();
+ void reset();
+
+ void visualizeObject(objpose::ObjectPose &objectPose);
+ void visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id);
+
+ private:
+ static constexpr const char* memoryLayerName = "Memory";
+ static constexpr const char* metaLayerName = "Meta";
+ static constexpr const char* lineString = "---------------------------";
+
+ std::vector<std::string> placeholderOptions;
+
+ Properties properties;
+ ObjectFinder objectFinder;
+
+ viz::Client& client;
+ const std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory;
+ const std::function<objpose::ObjectPoseSeq(void)> pullFromMemory;
+
+ viz::Layer memoryLayer {client.layer(memoryLayerName)};
+ viz::Layer metaLayer {client.layer(metaLayerName)};
+
+ InteractionObserver observer;
+
+ objpose::ObjectPoseSeq storedPoses;
+
+ ChangeState changes{this};
+ PlaceholderState placeholders{this};
+
+ bool isCommitRequired;
+ bool isUpdateRequired;
+ bool isResetRequired;
+ bool isMemoryVizRequired;
+ bool isMetaVizRequired;
+
+ friend ChangeState;
+ friend PlaceholderState;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c226e57a72b071eaf93c45ea52e5b1906789a54c
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
@@ -0,0 +1,74 @@
+#include "InteractionObserver.h"
+
+#include <utility>
+
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action)
+{
+ contextMenuActions[index] = std::move(action);
+ return *this;
+}
+
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action)
+{
+ transformEndAction = std::make_optional(std::move(action));
+ return *this;
+}
+
+void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction)
+{
+ if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
+ {
+ auto iterator = contextMenuActions.find(interaction.chosenContextMenuEntry());
+
+ if (iterator != contextMenuActions.end())
+ {
+ auto [index, action] = *iterator;
+ action();
+ }
+ }
+
+ if (interaction.type() == viz::InteractionFeedbackType::Transform && interaction.isTransformEnd())
+ {
+ if (transformEndAction.has_value())
+ {
+ transformEndAction.value()(interaction.transformation());
+ }
+ }
+}
+
+void armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer & layer)
+{
+ layer.clear();
+ observedLayers.erase(layer.data_.name);
+}
+
+void armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit& stage)
+{
+ for (auto& [name, observedLayer] : observedLayers)
+ {
+ stage.requestInteraction(observedLayer.layer);
+ }
+}
+
+void armarx::InteractionObserver::process(viz::InteractionFeedbackRange const& interactions)
+{
+ for (viz::InteractionFeedback const& interaction: interactions)
+ {
+ auto layerIterator = observedLayers.find(interaction.layer());
+
+ if (layerIterator != observedLayers.end())
+ {
+ auto& [name, layer] = *layerIterator;
+
+ auto observationIterator = layer.observations.find(interaction.element());
+
+ if (observationIterator != layer.observations.end())
+ {
+ auto& [element, observation] = *observationIterator;
+ observation.process(interaction);
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26ed2aca18754f22f5eead0ad345981b5ec0ae4
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+namespace armarx
+{
+ class InteractionObserver
+ {
+ public:
+ class Observation
+ {
+ public:
+ Observation& onContextMenu(size_t index, std::function<void()> action);
+ Observation& onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action);
+
+ void process(viz::InteractionFeedback const& interaction);
+
+ private:
+ std::map<size_t, std::function<void()>> contextMenuActions;
+ std::optional<std::function<void(Eigen::Matrix4f const&)>> transformEndAction;
+ };
+
+ template <typename ElementT>
+ Observation& addObserved(viz::Layer & layer, ElementT const& element)
+ {
+ layer.add(element);
+
+ auto iterator = observedLayers.find(layer.data_.name);
+
+ if (iterator == observedLayers.end())
+ {
+ std::tie(iterator, std::ignore) = observedLayers.emplace(layer.data_.name, layer);
+ }
+
+ auto& [name, observedLayer] = *iterator;
+ return observedLayer.observations[element.data_->id];
+ }
+
+ void clearObservedLayer(viz::Layer & layer);
+
+ void requestInteractions(viz::StagedCommit & stage);
+ void process(viz::InteractionFeedbackRange const& interactions);
+
+ private:
+ struct ObservedLayer
+ {
+ explicit ObservedLayer(viz::Layer & layer) : layer(layer) {}
+
+ std::reference_wrapper<viz::Layer> layer;
+ std::map<std::string, Observation> observations;
+ };
+
+ std::map<std::string, ObservedLayer> observedLayers;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e723f5e467d7d6588084ee5366753d64ca112a03
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
@@ -0,0 +1,115 @@
+#include "ObjectMemoryEditor.h"
+
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+
+#include "Editor.h"
+
+namespace armarx
+{
+ armarx::PropertyDefinitionsPtr ObjectMemoryEditor::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->optional(this->objectScaling, "Editor.ObjectScaling",
+ "Scaling factor that is applied to all intractable objects.");
+
+ defs->optional(this->confidenceThreshold, "Editor.ConfidenceThreshold",
+ "Only objects with a confidence greater than this value are shown.");
+
+ return defs;
+ }
+
+ std::string ObjectMemoryEditor::getDefaultName() const
+ {
+ return "InteractiveMemoryEditor";
+ }
+
+ void ObjectMemoryEditor::onInitComponent()
+ {
+ {
+ providerInfo.objectType = objpose::ObjectType::KnownObject;
+
+ for (const auto& dataset : objectFinder.getDatasets())
+ {
+ std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset(dataset);
+
+ for (const auto& obj: objects)
+ {
+ providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
+ }
+ }
+ }
+ }
+
+ void ObjectMemoryEditor::onConnectComponent()
+ {
+ setDebugObserverBatchModeEnabled(true);
+
+ objectVizTask = new SimpleRunningTask<>([this]()
+ {
+ this->run();
+ });
+ objectVizTask->start();
+ }
+
+ void ObjectMemoryEditor::onDisconnectComponent()
+ {
+ }
+
+ void ObjectMemoryEditor::onExitComponent()
+ {
+ }
+
+
+ void ObjectMemoryEditor::run()
+ {
+ objpose::ObjectPoseClient client = getClient();
+
+ Editor::Properties properties =
+ {
+ .providerName = getName(),
+ .objectScaling = objectScaling,
+ .confidenceThreshold = confidenceThreshold,
+ .availableObjects = providerInfo.supportedObjects,
+ };
+
+ Editor editor(arviz, properties,
+ [this](objpose::ProvidedObjectPoseSeq &poses)
+ {
+ objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
+ },
+ [client]() -> objpose::ObjectPoseSeq
+ {
+ return client.fetchObjectPoses();
+ });
+
+ Metronome metronome(Frequency::Hertz(20));
+ while (objectVizTask and not objectVizTask->isStopped())
+ {
+ editor.step();
+
+ metronome.waitForNextTick();
+ }
+ }
+
+ objpose::ProviderInfo ObjectMemoryEditor::getProviderInfo(const Ice::Current & /*unused*/)
+ {
+ return providerInfo;
+ }
+
+ objpose::provider::RequestObjectsOutput ObjectMemoryEditor::requestObjects(
+ const objpose::provider::RequestObjectsInput &input, const Ice::Current & /*unused*/)
+ {
+ objpose::provider::RequestObjectsOutput output;
+
+ for (const auto &id: input.objectIDs)
+ {
+ output.results[id].success = false;
+ }
+
+ return output;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
new file mode 100644
index 0000000000000000000000000000000000000000..1cc90b97906890dced6bf29bd25a3c26fa164f3f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include "InteractionObserver.h"
+
+namespace armarx
+{
+ class ObjectMemoryEditor :
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::ObjectPoseClientPluginUser,
+ virtual public armarx::ObjectPoseProviderPluginUser
+ {
+ public:
+ std::string getDefaultName() const override;
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ public:
+ objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
+ objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+
+ private:
+ void run();
+
+ armarx::ObjectFinder objectFinder;
+ objpose::ProviderInfo providerInfo;
+
+ float objectScaling = 1.01F;
+ float confidenceThreshold = 0.0F;
+
+ armarx::SimpleRunningTask<>::pointer_type objectVizTask;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 7f3b065d376bd23e7c99348bb3d94e9f6ddd3abf..6fcc0faa70a46b51250795f7641ca6c8d6a2765c 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,5 +1,6 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
-add_subdirectory(VirtualRobotWriterExample)
-add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
+add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(VirtualRobotWriterExample)
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..91c7bdc1834110f3c0be76f7d6eefb6f001338b9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
@@ -0,0 +1,40 @@
+armarx_component_set_name(RobotStatePredictionClientExample)
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarxGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPICore
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
+)
+
+set(SOURCES
+ # ToDo: Move to library.
+ RobotStatePredictionClient.cpp
+
+ Component.cpp
+ Impl.cpp
+)
+
+set(HEADERS
+ # ToDo: Move to library.
+ RobotStatePredictionClient.h
+ # ToDo: Move to simox.
+ simox_alg.hpp
+
+ Component.h
+ Impl.h
+
+ RobotStatePredictionClientExample.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# Generate the application.
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f13e111eec8e51f3fa67e5d04f03d3df1fd0b64
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -0,0 +1,97 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "RobotStatePredictionClientExample.h"
+#include "Impl.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Component::Component() :
+ pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ {
+ }
+
+
+ RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
+
+
+ std::string Component::getDefaultName() const
+ {
+ return "RobotStatePredictionClientExample";
+ }
+
+
+ armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ ARMARX_CHECK_NOT_NULL(pimpl);
+ pimpl->defineProperties(defs, "p.");
+
+ return defs;
+ }
+
+
+ void Component::onInitComponent()
+ {
+ }
+
+
+ void Component::onConnectComponent()
+ {
+ pimpl->connect(memoryNameSystem(), arviz);
+ pimpl->start();
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+
+
+ void Component::onDisconnectComponent()
+ {
+ pimpl->stop();
+ }
+
+
+ void Component::onExitComponent()
+ {
+ }
+
+
+ void Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ VBoxLayout root = {VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void Component::RemoteGui_update()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..198257f6b033cfc0433ca5b0caad18e47ee77e45
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <memory>
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
+// For some reason, the generated main requires Impl to be complete ...
+#include "Impl.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+ class Impl;
+
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ *
+ * An example for an ArMem Memory Client.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Connects to the example memory, and commits and queries example data.
+ */
+ class Component :
+ virtual public armarx::Component
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ , virtual public armarx::armem::ClientPluginUser
+ {
+ public:
+ using Impl = robot_state_prediction_client_example::Impl;
+
+ Component();
+ virtual ~Component();
+
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+
+ std::unique_ptr<Impl> pimpl = nullptr;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ };
+ RemoteGuiTab tab;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c433669630c9631e471666fa3b3cd02acd25c2f9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -0,0 +1,261 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Impl.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ {
+ client.remote.robotReader.emplace(memoryNameSystem);
+ }
+
+
+ Impl::~Impl() = default;
+
+
+ void
+ Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
+ const std::string& prefix)
+ {
+ defs->optional(properties.robotName, prefix + "robotName", "Name of the robot.");
+
+ defs->optional(properties.predictAheadSeconds,
+ prefix + "predictAheadSeconds",
+ "How far into the future to predict [s].");
+
+ client.remote.robotReader->registerPropertyDefinitions(defs);
+ }
+
+
+ void
+ Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
+ {
+ try
+ {
+ client.remote.reader =
+ mns.useReader(armem::MemoryID(armem::robot_state::constants::memoryName));
+ ARMARX_IMPORTANT << "got reader";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_WARNING << e.what();
+ }
+
+ client.remote.robotReader->connect();
+
+ this->remote.arviz = arviz;
+ }
+
+
+ void
+ Impl::start()
+ {
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
+ task->start();
+ }
+
+
+ void
+ Impl::stop()
+ {
+ task->stop();
+ }
+
+
+ void
+ Impl::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequencyHz));
+
+ while (task and not task->isStopped())
+ {
+ metronome.waitForNextTick();
+
+ runOnce();
+ }
+ }
+
+
+ void
+ Impl::runOnce()
+ {
+ ARMARX_CHECK(client.remote.reader);
+
+ const DateTime now = Clock::Now();
+ const DateTime predictedTime =
+ now + Duration::SecondsDouble(properties.predictAheadSeconds);
+
+ if (not robotViz)
+ {
+ auto desc = client.remote.robotReader->queryDescription(properties.robotName, now);
+ if (desc.has_value())
+ {
+ PackagePath& pp = desc->xml;
+ robotViz = viz::Robot(properties.robotName)
+ .file(pp.serialize().package, pp.serialize().path)
+ .overrideColor(simox::Color::cyan(255, 64));
+ }
+ else
+ {
+ ARMARX_INFO << "Failed to get robot description.";
+ }
+ }
+ if (not robotViz)
+ {
+ return;
+ }
+
+ // Which entities to predict?
+
+ const std::vector<armem::MemoryID> locEntityIDs = this->localizationEntityIDs.has_value()
+ ? this->localizationEntityIDs.value()
+ : client.queryLocalizationEntityIDs();
+ const std::vector<armem::MemoryID> propEntityIDs =
+ this->propioceptionEntityIDs.has_value() ? this->propioceptionEntityIDs.value()
+ : client.queryProprioceptionEntityIDs();
+
+ // Predict.
+
+ auto prediction = client.predictWholeBody(
+ locEntityIDs, propEntityIDs, predictedTime, properties.robotName, "Linear");
+
+ // Gather results.
+
+ if (prediction.globalPose.has_value())
+ {
+ // ARMARX_INFO << "Predicted global pose: \n" << globalPose->matrix();
+ robotViz->pose(prediction.globalPose->matrix());
+
+ if (not localizationEntityIDs)
+ {
+ // Store entity IDs for successful lookup.
+ this->localizationEntityIDs = locEntityIDs;
+ }
+ }
+ if (prediction.jointPositions.has_value())
+ {
+ robotViz->joints(prediction.jointPositions.value());
+
+ if (not propioceptionEntityIDs)
+ {
+ this->propioceptionEntityIDs = propEntityIDs;
+ }
+ }
+
+ // Visualize.
+ {
+ viz::Layer layer = remote.arviz.layer(properties.robotName + " Prediction");
+ layer.add(robotViz.value());
+ remote.arviz.commit(layer);
+ }
+ }
+
+
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..272f9e2e62d6ac16b49a2674a7952d96e8618975
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -0,0 +1,87 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <vector>
+
+#include <ArmarXCore/util/tasks.h>
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "RobotStatePredictionClient.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ class Impl
+ {
+ public:
+
+ Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ ~Impl();
+
+
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
+
+ void start();
+ void stop();
+
+ void run();
+ void runOnce();
+
+
+ public:
+
+ struct Properties
+ {
+ float updateFrequencyHz = 10;
+
+ std::string robotName = "Armar6";
+ float predictAheadSeconds = 1.0;
+ };
+ Properties properties;
+
+ struct Remote
+ {
+ viz::Client arviz;
+ };
+ Remote remote;
+
+ armarx::SimpleRunningTask<>::pointer_type task;
+
+
+ armem::robot_state::RobotStatePredictionClient client;
+
+ std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
+ std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
+ std::optional<viz::Robot> robotViz;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d98d0f2717747a91f7a56c3e0c547cbb0088fbd
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -0,0 +1,280 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStatePredictionClient.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "simox_alg.hpp"
+
+
+namespace armarx::armem::robot_state
+{
+
+ RobotStatePredictionClient::RobotStatePredictionClient()
+ {
+ }
+
+
+ std::vector<armem::MemoryID>
+ RobotStatePredictionClient::queryLocalizationEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::localizationCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::queryProprioceptionEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::proprioceptionCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::_queryEntityIDs(const std::string& coreSegmentName)
+ {
+ armem::client::QueryBuilder qb;
+ {
+ namespace qf = armarx::armem::client::query_fns;
+ qb.coreSegments(qf::withName(coreSegmentName))
+ .providerSegments(qf::all())
+ .entities(qf::all())
+ .snapshots(qf::latest());
+ }
+ armem::client::QueryResult result = remote.reader.query(qb);
+ if (result.success)
+ {
+ return armem::getEntityIDs(result.memory);
+ }
+ else
+ {
+ // ToDo: Use exceptions to escalate error.
+ ARMARX_VERBOSE << "Query failed: " << result.errorMessage;
+ return {};
+ }
+ }
+
+
+ std::vector<armem::PredictionRequest>
+ RobotStatePredictionClient::makePredictionRequests(
+ const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID)
+ {
+ std::vector<armem::PredictionRequest> requests;
+ requests.reserve(entityIDs.size());
+ for (const armem::MemoryID& entityID : entityIDs)
+ {
+ armem::PredictionRequest& request = requests.emplace_back();
+ request.snapshotID = entityID.withTimestamp(predictedTime);
+ request.predictionSettings.predictionEngineID = engineID;
+ }
+ ARMARX_CHECK_EQUAL(requests.size(), entityIDs.size());
+ return requests;
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionRequest> requests =
+ makePredictionRequests(entityIDs, predictedTime, engineID);
+ return predict(requests);
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<PredictionRequest>& requests)
+ {
+ const std::vector<PredictionResult> results = remote.reader.predict(requests);
+ ARMARX_CHECK_EQUAL(results.size(), requests.size());
+ return results;
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::lookupGlobalPose(
+ const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime)
+ {
+ ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
+ std::stringstream errorMessages;
+
+ namespace loc = armem::common::robot_state::localization;
+
+ std::optional<armem::wm::CoreSegment> coreSegment;
+
+ loc::TransformQuery query;
+ query.header.parentFrame = armarx::GlobalFrame;
+ query.header.frame = armem::robot_state::constants::robotRootNodeName;
+ query.header.agent = "";
+ query.header.timestamp = predictedTime;
+
+ for (size_t i = 0; i < localizationPredictionResults.size(); ++i)
+ {
+ const armem::PredictionResult& result = localizationPredictionResults[i];
+ const armem::MemoryID& entityID = localizationEntityIDs.at(i);
+
+ if (result.success)
+ {
+ if (query.header.agent.empty())
+ {
+ const arondto::Transform tf = arondto::Transform::FromAron(result.prediction);
+ query.header.agent = tf.header.agent;
+ }
+ if (not coreSegment)
+ {
+ coreSegment.emplace(entityID.getCoreSegmentID());
+ }
+ {
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ update.timeCreated = predictedTime;
+ update.instancesData = {result.prediction};
+ coreSegment->update(update);
+ }
+ }
+ else
+ {
+ errorMessages << "Prediction of '" << entityID << "'"
+ << " failed: " << result.errorMessage << "\n";
+ }
+ }
+
+ std::optional<Eigen::Affine3f> result;
+ if (coreSegment.has_value())
+ {
+ loc::TransformResult tf =
+ loc::TransformHelper::lookupTransform(coreSegment.value(), query);
+
+ if (tf)
+ {
+ result = tf.transform.transform;
+ }
+ else
+ {
+ errorMessages << "\nFailed to lookup transform: " << tf.errorMessage << "\n";
+ }
+ }
+
+ if (not errorMessages.str().empty())
+ {
+ // ToDo: Introduce an exception here?
+ ARMARX_VERBOSE << errorMessages.str();
+ }
+ return result;
+ }
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::lookupJointPositions(
+ const std::vector<PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName)
+ {
+ auto it = std::find_if(proprioceptionPredictionResults.begin(),
+ proprioceptionPredictionResults.end(),
+ [&robotName](const PredictionResult& result)
+ { return result.snapshotID.entityName == robotName; });
+ if (it != proprioceptionPredictionResults.end())
+ {
+ auto result = *it;
+ auto prop = armem::arondto::Proprioception::FromAron(result.prediction);
+ return prop.joints.position;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::predictGlobalPose(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ std::optional<Eigen::Affine3f> pose = lookupGlobalPose(results, entityIDs, predictedTime);
+ return pose;
+ }
+
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::predictJointPositions(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ return lookupJointPositions(results, robotName);
+ }
+
+
+ RobotStatePredictionClient::WholeBodyPrediction
+ RobotStatePredictionClient::predictWholeBody(const std::vector<armem::MemoryID>& locEntityIDs,
+ const std::vector<armem::MemoryID>& propEntityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ RobotStatePredictionClient::WholeBodyPrediction result;
+
+ // Predict.
+ const std::vector<armem::MemoryID> entityIDs =
+ simox::alg::concatenate(locEntityIDs, propEntityIDs);
+
+ if (entityIDs.empty())
+ {
+ return result;
+ }
+
+ auto _results = predict(entityIDs, predictedTime, "Linear");
+
+ // Gather results.
+ std::vector<armem::PredictionResult> locResults, propResults;
+ locResults = simox::alg::slice(_results, 0, locEntityIDs.size());
+ propResults = simox::alg::slice(_results, locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(locResults.size(), locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(propResults.size(), propEntityIDs.size());
+
+
+ result.globalPose = lookupGlobalPose(locResults, locEntityIDs, predictedTime);
+ result.jointPositions = lookupJointPositions(propResults, robotName);
+
+ return result;
+ }
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ef31275e1efbf7ea9f5fd43ce2fb467d54b2bd7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
@@ -0,0 +1,106 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <optional>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+
+namespace armarx::armem::robot_state
+{
+
+ class RobotStatePredictionClient
+ {
+ public:
+ RobotStatePredictionClient();
+
+
+ struct WholeBodyPrediction
+ {
+ std::optional<Eigen::Affine3f> globalPose;
+ std::optional<std::map<std::string, float>> jointPositions;
+ };
+ WholeBodyPrediction
+ predictWholeBody(const std::vector<armem::MemoryID>& localizationEntityIDs,
+ const std::vector<armem::MemoryID>& proprioceptionEntityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ predictGlobalPose(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<std::map<std::string, float>>
+ predictJointPositions(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+
+ std::vector<armem::PredictionResult> predict(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+ std::vector<armem::PredictionResult>
+ predict(const std::vector<armem::PredictionRequest>& requests);
+
+
+ std::vector<armem::MemoryID> queryLocalizationEntityIDs();
+ std::vector<armem::MemoryID> queryProprioceptionEntityIDs();
+
+ std::vector<armem::PredictionRequest>
+ makePredictionRequests(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ lookupGlobalPose(const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime);
+
+ std::optional<std::map<std::string, float>> lookupJointPositions(
+ const std::vector<armem::PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName);
+
+
+ private:
+ std::vector<armem::MemoryID> _queryEntityIDs(const std::string& coreSegmentName);
+
+ public:
+ struct Remote
+ {
+ armem::client::Reader reader;
+ std::optional<armem::robot_state::VirtualRobotReader> robotReader;
+ };
+ Remote remote;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..3023fef630da40fc660dc828bd66c14c5060982d
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
@@ -0,0 +1,31 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "Component.h"
+
+
+namespace armarx
+{
+ using RobotStatePredictionClientExample = armarx::robot_state_prediction_client_example::Component;
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a56a6ca012eb3b3180a757005f9a5d5633b1f82
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
@@ -0,0 +1,96 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
index 7045df683d1fb8d5c7fafa83a049fc7f1e9d2ea1..1cd517681ad03a2721c31bb1b55e188f4cfe7d09 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
@@ -21,6 +21,7 @@
#include "SimpleVirtualRobot.h"
+#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
@@ -53,6 +54,8 @@ namespace armarx::simple_virtual_robot
defs->optional(properties.robot.package, "p.robot.package", "Package of the Simox robot XML.");
defs->optional(properties.robot.path, "p.robot.path", "Local path of the Simox robot XML.");
+ defs->optional(properties.robot.jointValues, "p.robot.jointValues", "Specify a certain joint configuration.");
+
return defs;
}
@@ -106,6 +109,24 @@ namespace armarx::simple_virtual_robot
VirtualRobot::RobotPtr robot =
VirtualRobot::RobotIO::loadRobot(path.toSystemPath(), VirtualRobot::RobotIO::eStructure);
+ ARMARX_CHECK_NOT_NULL(robot) << "Failed to load robot `" << path << "`.";
+
+ if(not p.jointValues.empty())
+ {
+ ARMARX_DEBUG << "Parsing: " << p.jointValues;
+
+ const nlohmann::json j = nlohmann::json::parse(p.jointValues);
+ ARMARX_DEBUG << "JSON parsed as: " << j;
+
+ std::map<std::string, float> jointValues;
+ nlohmann::from_json(j, jointValues);
+
+ ARMARX_VERBOSE << "The following joint values are given by the user: " << jointValues;
+ robot->setJointValues(jointValues);
+ }
+
+
+
if (not p.name.empty())
{
robot->setName(p.name);
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
index d1680923b5109098622b3030c8569f1633540e0f..677f4e085d1d5a12c081b0e27623652561441904 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
@@ -60,6 +60,8 @@ namespace armarx::simple_virtual_robot
std::string name;
std::string package;
std::string path;
+
+ std::string jointValues; // json-style map<std::string, float>
};
Robot robot;
};
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6aab4a4ff38ce842d3c00f4d57bceec3cd5f5672..53e51cd0bd760e8112dba418b606efcdb6293613 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -2,36 +2,19 @@
#include "VirtualRobotReaderExampleClient.h"
-
-#include <memory>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(this->memoryNameSystem()) {}
+ virtualRobotReader(this->memoryNameSystem())
+ {
+ }
+
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
@@ -40,20 +23,27 @@ namespace armarx::robot_state
defs->topic(debugObserver);
- defs->optional(p.robotName, "robotName");
- defs->optional(p.updateFrequency, "updateFrequency");
+ defs->optional(properties.robotName, "p.robotName",
+ "The name of the robot to use.");
+ defs->optional(properties.updateFrequency, "p.updateFrequency [Hz]",
+ "The frequency of the running loop.");
virtualRobotReader.registerPropertyDefinitions(defs);
return defs;
}
+
std::string VirtualRobotReaderExampleClient::getDefaultName() const
{
return "VirtualRobotReaderExampleClient";
}
- void VirtualRobotReaderExampleClient::onInitComponent() {}
+
+ void VirtualRobotReaderExampleClient::onInitComponent()
+ {
+ }
+
void VirtualRobotReaderExampleClient::onConnectComponent()
{
@@ -61,42 +51,71 @@ namespace armarx::robot_state
ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
- task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task = new SimpleRunningTask<>([this]()
+ {
+ this->run();
+ });
task->start();
}
+
void VirtualRobotReaderExampleClient::onDisconnectComponent()
{
task->stop();
}
- void VirtualRobotReaderExampleClient::onExitComponent() {}
- void VirtualRobotReaderExampleClient::run()
+ void VirtualRobotReaderExampleClient::onExitComponent()
{
- const armem::Time now = armem::Time::Now();
+ }
- // initialize if needed
- if (virtualRobot == nullptr)
- {
- TIMING_START(getRobot);
- virtualRobot = virtualRobotReader.getRobot(p.robotName, now);
+ void VirtualRobotReaderExampleClient::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequency));
+ while (task and not task->isStopped())
+ {
+ const armem::Time now = armem::Time::Now();
- if (virtualRobot == nullptr)
+ // Initialize the robot if needed.
+ if (robot == nullptr)
{
- ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
- return;
+ // The TIMING_* macros are optional, you do not need them.
+ TIMING_START(getRobot);
+
+ robot = virtualRobotReader.getRobot(properties.robotName, now);
+ if (robot)
+ {
+ // Only print timing once the robot is loadable & loaded.
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "Could not create virtual robot.";
+ }
}
- // only print timing once the robot is loadable & loaded
- TIMING_END_STREAM(getRobot, ARMARX_INFO);
- }
+ if (robot)
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Synchronizing robot.";
- ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+ TIMING_START(synchronizeRobot);
+ ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*robot, now));
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
- TIMING_START(synchronizeRobot);
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*virtualRobot, now));
- TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+ // Do something with the robot (your code follows here, there are just a examples) ...
+
+ Eigen::Matrix4f globalPose = robot->getGlobalPose();
+ (void) globalPose;
+
+ std::vector<std::string> nodeNames = robot->getRobotNodeNames();
+ (void) nodeNames;
+
+ // End.
+ }
+
+ metronome.waitForNextTick();
+ }
}
} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 322362fe4ea476c0701bfe137ab4cc1eaea08ec5..c5b194cb2c68bbd30b58c362e3f6d348a54032b2 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -22,21 +22,15 @@
#pragma once
-
-// ArmarX
-#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXCore/util/tasks.h>
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
namespace armarx::robot_state
{
@@ -53,6 +47,7 @@ namespace armarx::robot_state
*/
class VirtualRobotReaderExampleClient :
virtual public armarx::Component,
+ // Use the memory client plugin.
virtual public armarx::armem::client::ComponentPluginUser
{
public:
@@ -70,6 +65,9 @@ namespace armarx::robot_state
void onDisconnectComponent() override;
void onExitComponent() override;
+
+ private:
+
void run();
@@ -79,17 +77,20 @@ namespace armarx::robot_state
{
std::string robotName{"Armar6"};
float updateFrequency{10.F};
- } p;
-
- armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
-
- armarx::DebugObserverInterfacePrx debugObserver;
+ };
+ Properties properties;
- // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ // The running task which is started in onConnectComponent().
+ armarx::SimpleRunningTask<>::pointer_type task;
+ // The reader used to get the robot state.
armem::robot_state::VirtualRobotReader virtualRobotReader;
- std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+ // The Simox robot model.
+ VirtualRobot::RobotPtr robot = nullptr;
+
+ // For publishing timing information.
+ armarx::DebugObserverInterfacePrx debugObserver;
};
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index e2059d968293dce364030082be4455186400ce0b..77135658d4a93d47e0b4474002b589cf569de67f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -73,7 +73,10 @@ namespace armarx
workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
// We support the "Latest" prediction engine for the entire memory.
- workingMemory().addPredictionEngine({"Latest"});
+ workingMemory().addPredictor(
+ armem::PredictionEngine{.engineID = "Latest"},
+ [this](const armem::PredictionRequest& request)
+ { return this->predictLatest(request); });
// For illustration purposes, we add more segments (without types).
bool trim = true;
@@ -216,66 +219,44 @@ namespace armarx
}
// PREDICTING
-
- armem::prediction::data::PredictionResultSeq
- ExampleMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
- {
- armem::prediction::data::PredictionResultSeq result;
- for (const auto& request : requests)
- {
- result.push_back(predictSingle(request));
- }
- return result;
- }
-
- armem::prediction::data::PredictionResult
- ExampleMemory::predictSingle(const armem::prediction::data::PredictionRequest& request)
+ armem::PredictionResult
+ ExampleMemory::predictLatest(const armem::PredictionRequest& request)
{
armem::PredictionResult result;
-
- std::string engine = request.settings.predictionEngineID;
- if (engine.empty() || engine == "Latest")
+ auto memID = request.snapshotID;
+ result.snapshotID = memID;
+
+ armem::client::QueryBuilder builder;
+ builder.latestEntitySnapshot(memID);
+ auto queryResult =
+ query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
+ if (queryResult.success)
{
- auto boID = fromIce<armem::MemoryID>(request.snapshotID);
- armem::client::QueryBuilder builder;
- builder.latestEntitySnapshot(boID);
- auto queryResult =
- query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
- if (queryResult.success)
+ auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
+ auto* latest = readMemory.findLatestSnapshot(memID);
+ if (latest != nullptr)
{
- auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
- auto* latest = readMemory.findLatestSnapshot(boID);
- if (latest != nullptr)
- {
- auto instance = boID.hasInstanceIndex()
- ? latest->getInstance(boID)
- : latest->getInstance(latest->getInstanceIndices().at(0));
- result.success = true;
- result.snapshotID = boID;
- result.prediction = instance.data();
- }
- else
- {
- result.success = false;
- result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
- }
+ auto instance = memID.hasInstanceIndex()
+ ? latest->getInstance(memID)
+ : latest->getInstance(latest->getInstanceIndices().at(0));
+ result.success = true;
+ result.prediction = instance.data();
}
else
{
result.success = false;
result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
}
else
{
result.success = false;
result.errorMessage =
- "This memory does not support the prediction engine '" + engine + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
- return result.toIce();
+ return result;
}
// REMOTE GUI
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 76e437bcb6f6db0e31c023eed4386831267c5fc2..b41f1262755f8ddad59eb9dc3c03309766183116 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -28,6 +28,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
@@ -71,11 +72,6 @@ namespace armarx
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
- // PredictingMemoryInterface interface
- public:
- armem::prediction::data::PredictionResultSeq
- predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
-
protected:
@@ -89,8 +85,7 @@ namespace armarx
private:
- armem::prediction::data::PredictionResult
- predictSingle(const armem::prediction::data::PredictionRequest& request);
+ armem::PredictionResult predictLatest(const armem::PredictionRequest& request);
armarx::DebugObserverInterfacePrx debugObserver;
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index fd3e2cdd938ca611c53ed13cf783744f399a8080..07f3089ca3acaf5370ca5ff38f3d2f2e3187294e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -288,9 +288,7 @@ namespace armarx::armem::server::obj
{
result.success = false;
result.errorMessage << "No predictions are supported for MemoryID "
- << boRequest.snapshotID
- << ". Have you given an instance index if requesting"
- << " an object pose prediction?";
+ << boRequest.snapshotID;
}
results.push_back(result.toIce());
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index fc71209b76d3b0c356df7a9895e5af95ba43faaf..8233df3d685339da7f43c134e588a21324a8ec79 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -25,17 +25,16 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <IceUtil/Time.h>
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
@@ -204,7 +203,6 @@ namespace armarx::armem::server::robot_state
return { new Pose(poseMap[robotName].matrix()) };
}
-
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index d402496819431eded99b506b69fbab48e6804086..fb0329dc65699624bfd53236783846cdb3592876 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -132,7 +132,7 @@ namespace armarx::armem::server::robot_state
std::queue<proprioception::RobotUnitData> dataQueue;
std::mutex dataMutex;
- bool waitForRobotUnit = false;
+ bool waitForRobotUnit = false;
};
RobotUnit robotUnit;
};
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index d740b5f2385f3c95b2004ded5011586728bab65c..75882cf47a238bca9e67a1131829f75c20869ea2 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -138,10 +138,17 @@ namespace armarx
SkillManagerComponentPluginUser::removeProvider(skillProviderName, current);
}
- void SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t)
+ skills::provider::dto::SkillStatusUpdate SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t)
{
- skillExecutionRequestCoreSegment.addSkillExecutionRequest(info);
- SkillManagerComponentPluginUser::executeSkill(info, current);
+ skills::manager::dto::SkillExecutionRequest requestCopy = info;
+ if (requestCopy.skillId.providerName == "*")
+ {
+ // sanitize the provider name if set to 'any'
+ requestCopy.skillId.providerName = getFirstProviderNameThatHasSkill(requestCopy.skillId.skillName);
+ }
+
+ skillExecutionRequestCoreSegment.addSkillExecutionRequest(requestCopy);
+ return SkillManagerComponentPluginUser::executeSkill(requestCopy, current);
}
void SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t)
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index a71fcb4988315e3b1c1345a4362bd634d18eba4d..91506c16fb87793083328d173d973f19ad268e75 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -76,7 +76,7 @@ namespace armarx
// Override SkillManager to add memory functions
void addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current ¤t) override;
void removeProvider(const std::string&, const Ice::Current ¤t) override;
- void executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
+ skills::provider::dto::SkillStatusUpdate executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current ¤t) override;
// WritingInterface interface
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index a5e0c8a639f90ae7572e87855a2a42ab8427c2f5..bc50155edc0d42a775c63ade67e35f38e34fd8c5 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -10,8 +10,11 @@
namespace armarx::skills::provider
{
+ HelloWorldSkill::HelloWorldSkill() :
+ Skill(GetSkillDescription())
+ {}
- SkillDescription CreateHelloWorldSkillDescription()
+ SkillDescription HelloWorldSkill::GetSkillDescription()
{
armarx::skills::Example::HelloWorldAcceptedType default_params;
default_params.some_float = 5;
@@ -22,34 +25,37 @@ namespace armarx::skills::provider
"HelloWorld",
"This skill logs a message on ARMARX_IMPORTANT",
{},
- 1000,
+ armarx::core::time::Duration::MilliSeconds(1000),
armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
default_params.toAron()
};
}
- HelloWorldSkill::HelloWorldSkill() :
- Skill(CreateHelloWorldSkillDescription())
- {}
- Skill::Status HelloWorldSkill::main(const aron::data::DictPtr& d, const CallbackT&)
+ Skill::MainResult HelloWorldSkill::main(const MainInput& in)
{
ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" <<
"I received the following data: \n" <<
- aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(d).dump(2) << "\n" <<
+ aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in.params).dump(2) << "\n" <<
"(executed at: " << IceUtil::Time::now() << ")";
- return Skill::Status::Succeeded;
+ return {TerminatedSkillStatus::Succeeded, nullptr};
}
ChainingSkill::ChainingSkill() :
- Skill(SkillDescription{
+ Skill(GetSkillDescription())
+ {}
+
+ SkillDescription ChainingSkill::GetSkillDescription()
+ {
+ return SkillDescription{
"ChainingSkill",
"This skill calls the HelloWorld skill three times.",
{},
- 3000,
+ armarx::core::time::Duration::MilliSeconds(3000),
nullptr
- })
- {}
- Skill::Status ChainingSkill::main(const aron::data::DictPtr& d, const CallbackT&)
+ };
+ }
+
+ Skill::MainResult ChainingSkill::main(const MainInput& in)
{
armarx::skills::Example::HelloWorldAcceptedType exec1;
armarx::skills::Example::HelloWorldAcceptedType exec2;
@@ -59,74 +65,69 @@ namespace armarx::skills::provider
exec2.some_text = "Hello from the ChainingSkill 2";
exec3.some_text = "Hello from the ChainingSkill 3";
- manager::dto::SkillExecutionRequest exec;
- exec.providerName = "SkillProviderExample";
- exec.skillName = "HelloWorld";
-
- exec.params = exec1.toAron()->toAronDictDTO();
- manager->executeSkill(exec);
+ SkillProxy skillExecPrx(manager, {"SkillProviderExample", "HelloWorld"});
- exec.params = exec2.toAron()->toAronDictDTO();
- manager->executeSkill(exec);
+ skillExecPrx.executeFullSkill(getSkillId().toString(), exec1.toAron());
+ skillExecPrx.executeFullSkill(getSkillId().toString(), exec2.toAron());
+ skillExecPrx.executeFullSkill(getSkillId().toString(), exec3.toAron());
- exec.params = exec3.toAron()->toAronDictDTO();
- manager->executeSkill(exec);
-
- return Skill::Status::Succeeded;
+ return {TerminatedSkillStatus::Succeeded, nullptr};
}
TimeoutSkill::TimeoutSkill() :
- Skill(SkillDescription{
+ PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
+ {}
+
+ SkillDescription TimeoutSkill::GetSkillDescription()
+ {
+ return SkillDescription{
"Timeout",
"This fails with timeout reached",
{},
- 1000,
+ armarx::core::time::Duration::MilliSeconds(1000),
nullptr
- })
- {}
- Skill::Status TimeoutSkill::main(const aron::data::DictPtr& d, const CallbackT&)
+ };
+ }
+
+ PeriodicSkill::StepResult TimeoutSkill::step(const MainInput& in)
{
- int i = 0;
- while (!timeoutReached)
- {
- // do heavy work
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
-
- // check if work is done
- if(i++ > 15)
- {
- ARMARX_IMPORTANT << "Somehow the timeout check succeceded....";
- return Skill::Status::Succeeded;
- }
- }
- return Skill::Status::TimeoutReached;
+ // do heavy work
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+ return {ActiveOrTerminatedSkillStatus::Running, nullptr};
}
CallbackSkill::CallbackSkill() :
- Skill(SkillDescription{
+ Skill(GetSkillDescription())
+ {}
+
+ SkillDescription CallbackSkill::GetSkillDescription()
+ {
+ return SkillDescription{
"ShowMeCallbacks",
"This skill does shows callbacks",
{},
- 1000,
+ armarx::core::time::Duration::MilliSeconds(1000),
nullptr
- })
- {}
- Skill::Status CallbackSkill::main(const aron::data::DictPtr& d, const CallbackT& callback)
+ };
+ }
+
+ Skill::MainResult CallbackSkill::main(const MainInput& in)
{
ARMARX_IMPORTANT << "Logging three updates via the callback";
auto up1 = std::make_shared<aron::data::Dict>();
up1->addElement("updateInfo", std::make_shared<aron::data::String>("Update 1"));
- callback(up1);
+ in.callback(up1);
auto up2 = std::make_shared<aron::data::Dict>();
up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
- callback(up2);
+ in.callback(up2);
auto up3 = std::make_shared<aron::data::Dict>();
up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
- callback(up3);
+ in.callback(up3);
- return Skill::Status::Succeeded;
+ return {TerminatedSkillStatus::Succeeded, nullptr};
}
@@ -156,8 +157,11 @@ namespace armarx::skills::provider
fooDesc.acceptedType = nullptr; // accept everything
fooDesc.description = "This skill does exactly nothing.";
fooDesc.skillName = "Foo";
- fooDesc.timeoutMs = 1000;
- addSkill([](const aron::data::DictPtr&){ std::cout << "Hello from Foo" << std::endl; return true; }, fooDesc);
+ fooDesc.timeout = armarx::core::time::Duration::MilliSeconds(1000);
+ addSkill([](const std::string& clientId, const aron::data::DictPtr&){
+ std::cout << "Hello from Foo. The skill was called from " << clientId << "." << std::endl;
+ return TerminatedSkillStatus::Succeeded;
+ }, fooDesc);
}
// Add another example skill
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 8ed9ffc9e2326724df01d83292e6561f6b54d959..cbd795396941e1d004fd5f7fdf0fa946d70cb71e 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -27,6 +27,7 @@
#include <ArmarXCore/core/Component.h>
// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
namespace armarx::skills::provider
@@ -36,22 +37,34 @@ namespace armarx::skills::provider
{
public:
HelloWorldSkill();
- Status main(const aron::data::DictPtr&, const CallbackT&) final;
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput& in) final;
};
class ChainingSkill : public Skill
{
public:
ChainingSkill();
- Status main(const aron::data::DictPtr&, const CallbackT&) final;
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput& in) final;
};
- class TimeoutSkill : public Skill
+ class TimeoutSkill : public PeriodicSkill
{
public:
TimeoutSkill();
- Status main(const aron::data::DictPtr&, const CallbackT&) final;
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ PeriodicSkill::StepResult step(const MainInput& in) final;
};
@@ -59,7 +72,11 @@ namespace armarx::skills::provider
{
public:
CallbackSkill();
- Status main(const aron::data::DictPtr&, const CallbackT&) final;
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput& in) final;
};
/**
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 40250b2b1a7d0dc472806715fe6c4daf9b532f17..5cabc89ddbb4843acc00a24c42a35b03d87f99b8 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -685,7 +685,7 @@ namespace armarx
frame.getRawData() +
frame.size()));
// recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
- recvQueue.push(dataGramWidthTimeStamp);
+ recvQueue.commit(dataGramWidthTimeStamp);
}
void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 00d50db0a789bec5a044839e31189ba429414b07..3aa3984c65aad167af2b49e94ab34130c880c998 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -14,6 +14,31 @@
<string>SkillManagerMonitorWidget</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QGroupBox" name="groupBoxActiveSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Active Skills</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <widget class="QListWidget" name="listWidgetActiveSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
<item>
<widget class="QSplitter" name="splitter">
<property name="orientation">
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 4c3e3a983d6acd45abbadd4626ab374acdd10541..27a954593ad99d252410ffec11d086689c1d9e07 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -28,6 +28,8 @@
#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
+#include <RobotAPI/libraries/skills/provider/Skill.h>
+
// modals
#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
@@ -158,6 +160,7 @@ namespace armarx
if (!connected)
return;
+ /* CHECK OWN SKILLS LIST */
// remove non-existing ones
auto managerSkills = manager->getSkillDescriptions();
std::vector<std::string> removedProviders;
@@ -187,6 +190,7 @@ namespace armarx
}
}
+ /* CHECK TREE VIEW */
// remove providers from tree
int i = 0;
while (i < widget.treeWidgetSkills->topLevelItemCount())
@@ -218,7 +222,8 @@ namespace armarx
}
}
- // update status
+ // update status and active skills window
+ std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
auto managerStatuses = manager->getSkillExecutionStatuses();
for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
@@ -232,20 +237,50 @@ namespace armarx
for (int j = 0; j < item->childCount(); ++j)
{
QTreeWidgetItem* skillItem = item->child(j);
- auto skillName = skillItem->text(0).toStdString();
-
- auto statusForSkill = allStatusesForProvider.at(skillName);
+ skills::SkillID currentSkillId(providerName, skillItem->text(0).toStdString());
+ auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusForSkill.header.status)));
+
+ if (not statusForSkill.header.executorName.empty()) // it means that the skill was called by someone
+ {
+ activeSkillsAndPrefixes.insert({currentSkillId, statusForSkill.header.executorName});
+ }
}
}
catch (const std::exception& e)
{
// Perhaps the skill provider died after the check at the beginning of this method
- // Continue
continue;
}
}
+
+ // finally update the view of active skills
+ widget.listWidgetActiveSkills->clear();
+ for (const auto& [id, prefix] : activeSkillsAndPrefixes)
+ {
+ auto prefixedStr = id.toString(prefix);
+ bool longest = true;
+ for (const auto& [id2, prefix2] : activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ {
+ auto prefixedStr2 = id.toString(prefix2);
+ if (prefixedStr == prefixedStr2)
+ {
+ continue;
+ }
+
+ if (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ {
+ longest = false;
+ break;
+ }
+ }
+
+ if (longest)
+ {
+ widget.listWidgetActiveSkills->addItem(QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
+ }
+ }
}
void SkillManagerMonitorWidgetController::executeSkill()
@@ -263,9 +298,13 @@ namespace armarx
auto data = getConfigAsAron();
+ char hostname[HOST_NAME_MAX];
+
+ gethostname(hostname, HOST_NAME_MAX);
+
skills::manager::dto::SkillExecutionRequest exInfo;
- exInfo.providerName = selectedSkill.providerName;
- exInfo.skillName = selectedSkill.skillName;
+ exInfo.executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")";
+ exInfo.skillId = {selectedSkill.providerName, selectedSkill.skillName};
exInfo.params = aron::data::Dict::ToAronDictDTO(data);
ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName << ". The data was: " << data;
diff --git a/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
index 1da45e9db1429cb4319b0c477c8ef16ec6fb3024..bd831fe2294dd8476c6fb3b3a73fb776c2acf2d2 100644
--- a/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
@@ -65,7 +65,8 @@ module armarx
struct StartRecordInput {
armarx::core::time::dto::DateTime executionTime;
- string recordingID; //recordingID is formatted: timestamp_recordingName
+ string recordingID;
+ armarx::core::time::dto::DateTime startTime;
Configuration configuration;
};
}
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index 28712a953e389ff54a7b7bab2f2a5cf48e24526e..088d28561684e1a604595f63af4aad6903e4caaf 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -22,7 +22,6 @@
#pragma once
-//#include <RobotAPI/interface/skills/SkillProviderCallbackInterface.ice>
#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
module armarx
@@ -36,9 +35,9 @@ module armarx
// Inputs
struct SkillExecutionRequest
{
- string providerName;
- string skillName;
- aron::data::dto::Dict params;
+ string executorName; // Who called the request
+ provider::dto::SkillID skillId; // The id of a skill to request. Note that the skill or provider name can be regexes.
+ aron::data::dto::Dict params; // the parameters for the skill. Can be nullptr if the skill does not require parameters
};
struct ProviderInfo
{
@@ -56,7 +55,7 @@ module armarx
{
interface SkillManagerInterface extends callback::dti::SkillProviderCallbackInterface
{
- // There is by design no method to access the providerPrx. You should only communicate with the manager
+ // There is by design no method to get the ProviderInfo. You should only communicate with the manager
void addProvider(dto::ProviderInfo providerInfo);
void removeProvider(string providerName);
@@ -64,7 +63,7 @@ module armarx
dto::SkillDescriptionMapMap getSkillDescriptions();
dto::SkillStatusUpdateMapMap getSkillExecutionStatuses();
- void executeSkill(dto::SkillExecutionRequest skillExecutionInfo);
+ provider::dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest skillExecutionInfo);
void abortSkill(string providerName, string skillName);
};
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index 33fbf744fb2ec84fee801b980106ca80cc020fe9..ed40611c800bfccba6f44310ca5d88639d83c02f 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -51,25 +51,32 @@ module armarx
{
sequence<string> StringList;
- // Description of a skill. Needs to be sent to a skill manager or memory
+ // A skill ID. Must be unique within one SkillManager
+ struct SkillID
+ {
+ string providerName;
+ string skillName;
+ };
+
+ // Description of a skill, independant of a provider
// A skill is nothing but a executable thing, which can be executed on one or more 'robots' (empty means all)
struct SkillDescription
{
string skillName; // the name of the skill
- string description; // a human readable description of what the skill does
- StringList robots; // the names of the robotis that are able to eecute that skill
- long timeoutMs; // in milliseconds
- aron::type::dto::AronObject acceptedType; // the name of the object is irrelevant and only used in GUI
+ string description; // a human readable description of what the skill does. Used in GUI
+ StringList robots; // the names of the robots that are able to execute that skill
+ long timeoutMs; // in milliseconds, can be set to -1 for infinite
+ aron::type::dto::AronObject acceptedType; // the name of the object is irrelevant and only used in GUI. nullptr if not set
aron::data::dto::Dict defaultParams; // the default parameterization used in GUI. nullptr if not set
};
dictionary<string, SkillDescription> SkillDescriptionMap;
- // Inputs
+ // Input to a provider to execute a skill
struct SkillExecutionRequest
{
- string skillName;
- string requesterName;
- aron::data::dto::Dict params;
+ string skillName; // the id of the skill
+ string executorName; // the name of the component/lib/skill that called the execution of the skill
+ aron::data::dto::Dict params; // the used parameterization
callback::dti::SkillProviderCallbackInterface* callbackInterface; // use nullptr if you do not want to have callbacks
};
@@ -92,17 +99,17 @@ module armarx
// Status updates of a skill
struct SkillStatusUpdateHeader
{
- string providerName; // the name of the provider the skill is currently running on
- string skillName;
- aron::data::dto::Dict usedParams;
- callback::dti::SkillProviderCallbackInterface* usedCallbackInterface;
- Execution::Status status;
+ SkillID skillId; // the id of the skill
+ string executorName; // the name of the component/lib/skill that called the execution of the skill
+ aron::data::dto::Dict usedParams; // the used parameterization
+ callback::dti::SkillProviderCallbackInterface* usedCallbackInterface; // the used callback interface. Probably a prx to the manager
+ Execution::Status status; // the current status of the skill
};
struct SkillStatusUpdate
{
SkillStatusUpdateHeader header;
- aron::data::dto::Dict data; // can be anything
+ aron::data::dto::Dict data; // data, attached to the status update. If send via a callback, this data may be used by the callback interface
};
dictionary<string, SkillStatusUpdate> SkillStatusUpdateMap;
@@ -110,7 +117,6 @@ module armarx
module dti
{
-
interface SkillProviderInterface
{
dto::SkillDescription getSkillDescription(string name);
@@ -120,7 +126,8 @@ module armarx
// execute skill will ALWAYS fully execute the skill and wait, until the skill is finished.
// Use the _begin() method, if you want to call a skill async
- void executeSkill(dto::SkillExecutionRequest executionInfo);
+ // This method returns a status update where the status is ALWAYS one of the terminating values
+ dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest executionInfo);
// try to kill a skill as soon as possible. When the skill is stopped depends on the implementation.
void abortSkill(string skill);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index ec6658fa85afba280fef0613fbc60596da57ee8a..3e503f9a7f04ff1fdcdb4bb1988ee04f0b18666d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -3,6 +3,9 @@ set(LIB_NAME ${PROJECT_NAME}ArmarXObjects)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(manif_FOUND "manif not available")
+
set(LIBS
# ArmarXGui
RemoteGui
@@ -10,6 +13,9 @@ set(LIBS
RobotAPI::Core
RobotAPIInterfaces
aroncommon
+
+ Eigen3::Eigen
+ ${manif_LIBRARIES}
)
set(LIB_FILES
@@ -64,6 +70,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
+
add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 74f3233ee04475ba4cd8cca04cdc8b35ede5ba18..82dc134ee4b958981cf1ebe5d74047f49eb62a2a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -12,6 +12,7 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include "ice_conversions.h"
@@ -171,6 +172,31 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+ // ToDo: We can provide ...Robot() and ...Original() versions as well.
+ objpose::ProvidedObjectPose ObjectPose::toProvidedObjectPoseGlobal() const
+ {
+ objpose::ProvidedObjectPose providedObjectPose;
+
+ providedObjectPose.objectID = this->objectID;
+ providedObjectPose.providerName = this->providerName;
+ providedObjectPose.objectType = this->objectType;
+
+ providedObjectPose.isStatic = this->isStatic;
+
+ providedObjectPose.objectPoseFrame = GlobalFrame;
+ providedObjectPose.objectPose = this->objectPoseGlobal;
+ providedObjectPose.objectPoseGaussian = this->objectPoseGlobalGaussian;
+
+ providedObjectPose.objectJointValues = this->objectJointValues;
+
+ providedObjectPose.confidence = this->confidence;
+ providedObjectPose.timestamp = this->timestamp;
+
+ providedObjectPose.localOOBB = this->localOOBB;
+
+ return providedObjectPose;
+ }
+
void ObjectPose::setObjectPoseRobot(
const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 106f52a842e1c3a5790068a30a96182326c24629..6ee27fa6e039a862217d48601ad8d2d02881a615 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -46,6 +46,7 @@ namespace armarx::objpose
void toIce(data::ObjectPose& ice) const;
void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot = nullptr);
+ objpose::ProvidedObjectPose toProvidedObjectPoseGlobal() const;
void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
index fc9dfe5081f69215398cd5c6eefaf7c8c1fbd705..e74da09bfa4a0c26adf4f1579ead89947f282fba 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -22,6 +22,8 @@
#include "predictions.h"
+#include <manif/SE3.h>
+
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
@@ -41,44 +43,45 @@ namespace armarx::objpose
const DateTime timeOrigin = DateTime::Now();
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
std::vector<double> timestampsSec;
- std::vector<Eigen::Vector3d> positions;
+ std::vector<Vector6d> tangentPoses;
// ToDo: How to handle attached objects?
for (const auto& [timestamp, pose] : poses)
{
timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
- positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+ manif::SE3f se3(simox::math::position(pose.objectPoseGlobal),
+ Eigen::Quaternionf(simox::math::orientation(pose.objectPoseGlobal)));
+ tangentPoses.emplace_back(se3.cast<double>().log().coeffs());
}
- ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+ ARMARX_CHECK_EQUAL(timestampsSec.size(), tangentPoses.size());
- Eigen::Vector3d prediction;
+ Eigen::Matrix4f prediction;
// Static objects don't move. Objects that haven't moved for a while probably won't either.
if (timestampsSec.size() <= 1 || latestPose.isStatic)
{
- prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+ prediction = latestPose.objectPoseGlobal;
}
else
{
- using simox::math::LinearRegression3d;
+ using simox::math::LinearRegression;
const bool inputOffset = false;
- const LinearRegression3d model =
- LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+ const LinearRegression<tangentDims> model =
+ LinearRegression<tangentDims>::Fit(timestampsSec, tangentPoses, inputOffset);
const auto predictionTime = armarx::fromIce<DateTime>(time);
- prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ Vector6d linearPred = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
}
// Used as a template for the returned pose prediction.
ObjectPose latestCopy = latestPose;
- // Reuse the rotation from the latest pose.
- // This is a pretty generous assumption, but the linear model doesn't cover rotations,
- // so it's our best guess.
- Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
- simox::math::position(latest) = prediction.cast<float>();
- latestCopy.setObjectPoseGlobal(latest);
+ latestCopy.setObjectPoseGlobal(prediction);
result.success = true;
result.prediction = latestCopy.toIce();
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 099ad8b4b981aeed1cc9e274c25672a1014ee120..764c797c11c2bc7e48ad64ca0ee0173197613e92 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -16,6 +16,7 @@ add_subdirectory(natik)
add_subdirectory(ukfm)
add_subdirectory(aron)
+add_subdirectory(aron_component_config)
add_subdirectory(armem)
add_subdirectory(armem_grasping)
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
index 628824cb3bfdbd750939721c1cba92f21967dc68..b01b2c6e976d2f3d2f3e882668236cea36943c69 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
@@ -302,8 +302,8 @@ namespace armarx
namespace armarx
{
- box_to_grasp_candidates::box_to_grasp_candidates(const armarx::RobotNameHelper& rnh, const VirtualRobot::RobotPtr& robot)
- : _rnh{std::make_shared<armarx::RobotNameHelper>(rnh)}, _robot{robot}
+ box_to_grasp_candidates::box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh, const VirtualRobot::RobotPtr& robot)
+ : _rnh{std::move(rnh)}, _robot{robot}
{
ARMARX_CHECK_NOT_NULL(_robot);
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
index 737df4d2cbfc3a485c98619685e42c360ba5968f..dfd7e1030ba71ebd68b18cba6aa476c0bab51ebc 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
@@ -102,7 +102,7 @@ namespace armarx
) const;
};
public:
- box_to_grasp_candidates(const armarx::RobotNameHelper& rnh,
+ box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh,
const VirtualRobot::RobotPtr& robot);
void add_armar6_defaults();
diff --git a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
index b8a69f3c0e7b5850c7ed8d5faffd2fb8df6dfd0b..9888dccad9687c74234e5a0e6ee2b59a4511e141 100644
--- a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
@@ -2,14 +2,16 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <utility>
+
#include "grasp_candidate_drawer.h"
namespace armarx
{
grasp_candidate_drawer::grasp_candidate_drawer(
- const armarx::RobotNameHelper& rnh,
+ armarx::RobotNameHelperPtr rnh,
const VirtualRobot::RobotPtr& robot)
- : rnh{std::make_shared<armarx::RobotNameHelper>(rnh)}, robot{robot}
+ : rnh(std::move(rnh)), robot{robot}
{}
void grasp_candidate_drawer::reset_colors()
diff --git a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
index 3a241476976d0c1f1072645906bf08f0f3c7cb91..1673d86a0957db9ffdb4216241a97ee85618608c 100644
--- a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
+++ b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
@@ -22,7 +22,7 @@ namespace armarx
Eigen::Matrix4f tcp_2_hroot;
};
public:
- grasp_candidate_drawer(const armarx::RobotNameHelper& rnh,
+ grasp_candidate_drawer(armarx::RobotNameHelperPtr rnh,
const VirtualRobot::RobotPtr& robot);
void reset_colors();
public:
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index b9ad4f67a8e71d645fc5c868912a0ea9e271c1c9..8072390200cb2696c0a51bde1f7ca6807a76659d 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -30,10 +30,10 @@
namespace armarx::plugins
{
- const RobotNameHelper& RobotStateComponentPlugin::getRobotNameHelper() const
+ RobotNameHelperPtr RobotStateComponentPlugin::getRobotNameHelper() const
{
ARMARX_CHECK_NOT_NULL(_nameHelper);
- return *_nameHelper;
+ return _nameHelper;
}
void RobotStateComponentPlugin::setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc)
{
@@ -318,7 +318,7 @@ namespace armarx
{
return getRobotStateComponentPlugin().getRobotStateComponent();
}
- const RobotNameHelper& RobotStateComponentPluginUser::getRobotNameHelper() const
+ RobotNameHelperPtr RobotStateComponentPluginUser::getRobotNameHelper() const
{
return getRobotStateComponentPlugin().getRobotNameHelper();
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index d3a1ba42bd9a21279268f24780572957b1d80aa4..af85178be6aaa8e98da1e19e8c76dc0019bc237f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -93,7 +93,7 @@ namespace armarx::plugins
//querry
public:
const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
- const RobotNameHelper& getRobotNameHelper() const;
+ RobotNameHelperPtr getRobotNameHelper() const;
Eigen::Matrix4f transformFromTo(const std::string& from,
const std::string& to,
const VirtualRobot::RobotPtr& rob);
@@ -178,7 +178,7 @@ namespace armarx
const RobotStateComponentInterfacePrx& getRobotStateComponent() const;
- const RobotNameHelper& getRobotNameHelper() const;
+ RobotNameHelperPtr getRobotNameHelper() const;
//get / add
public:
bool hasRobot(const std::string& id) const;
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c17efd18644d5b515170b714d263f4b37a194f25..5d6ed01da11647da7bd64177aabb439d276efa96 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -164,6 +164,7 @@ set(LIB_HEADERS
mns/plugins/Plugin.h
mns/plugins/PluginUser.h
+ util/prediction_helpers.h
util/util.h
)
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index deab5d748d4606afd2bd6670585193ac0c011822..f3991bc59641ac81b1efe74f781edeebb6f6f8fd 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -429,6 +429,7 @@ namespace armarx::armem::client
{
server::dto::StartRecordInput i;
i.executionTime = armarx::core::time::dto::DateTime();
+ i.startTime = armarx::core::time::dto::DateTime();
i.recordingID = "";
i.configuration.clear();
diff --git a/source/RobotAPI/libraries/armem/core/Prediction.h b/source/RobotAPI/libraries/armem/core/Prediction.h
index a8ec9ee52436b5b5d96c5c8013d058f8d6abc6e5..259ea375e546c64bfee9993939312fb3476ae60a 100644
--- a/source/RobotAPI/libraries/armem/core/Prediction.h
+++ b/source/RobotAPI/libraries/armem/core/Prediction.h
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
namespace armarx::armem
{
@@ -66,6 +67,7 @@ namespace armarx::armem
armem::prediction::data::PredictionResult toIce() const;
};
+
void toIce(armem::prediction::data::PredictionEngine& ice,
const PredictionEngine& engine);
void fromIce(const armem::prediction::data::PredictionEngine& ice,
diff --git a/source/RobotAPI/libraries/armem/core/container_maps.h b/source/RobotAPI/libraries/armem/core/container_maps.h
index 625c92bb02548c66b96c18e349bbe23acc0385ef..6b836da0b833ea76ce24a1920e0247255e9b6e61 100644
--- a/source/RobotAPI/libraries/armem/core/container_maps.h
+++ b/source/RobotAPI/libraries/armem/core/container_maps.h
@@ -32,14 +32,6 @@ namespace armarx::armem
namespace detail
{
- template <class KeyT, class ValueT>
- struct MapRef
- {
- KeyT* key;
- ValueT* value;
- };
-
-
/**
* @brief Get the entry in the map for which the returned key is the longest prefix
@@ -81,6 +73,50 @@ namespace armarx::armem
return result;
}
+ /**
+ * @brief Get the entry in the map for which the returned key is the longest prefix
+ * of the given key among the keys in the map that satisfy the predicate.
+ *
+ * `prefixFunc` is used to successively calculate the prefixes of the given key.
+ * It must be pure and return an empty optional when there is no shorter
+ * prefix of the given key (for strings, this would be the case when passed the empty string).
+ * `predicate` is used to filter for entries that satisfy the desired condition.
+ * It must be pure.
+ *
+ * @param keyValMap the map that contains the key-value-pairs to search
+ * @param prefixFunc the function that returns the longest non-identical prefix of the key
+ * @param predicate the predicate to filter entries on
+ * @param key the key to calculate the prefixes of
+ *
+ * @return The iterator pointing to the found entry, or `keyValMap.end()`.
+ */
+ template <typename KeyT, typename ValueT>
+ typename std::map<KeyT, ValueT>::const_iterator
+ findEntryWithLongestPrefixAnd(
+ const std::map<KeyT, ValueT>& keyValMap,
+ const std::function<std::optional<KeyT>(KeyT&)>& prefixFunc,
+ const KeyT& key,
+ const std::function<bool(const KeyT&, const ValueT&)>& predicate)
+ {
+ std::optional<KeyT> curKey = key;
+
+ typename std::map<KeyT, ValueT>::const_iterator result = keyValMap.end();
+ do
+ {
+ auto iterator = keyValMap.find(curKey.value());
+ if (iterator != keyValMap.end() and predicate(iterator->first, iterator->second))
+ {
+ result = iterator;
+ }
+ else
+ {
+ curKey = prefixFunc(curKey.value());
+ }
+ }
+ while (result == keyValMap.end() and curKey.has_value());
+
+ return result;
+ }
/**
* @brief Accumulate all the values in a map for which the keys are prefixes of the given key.
@@ -202,6 +238,23 @@ namespace armarx::armem
}
+ /**
+ * @brief Find the entry with the most specific key that contains the given ID
+ * and satisfies the predicate, or `idMap.end()` if no key contains the ID and satisfies
+ * the predicate.
+ *
+ * @see `detail::findEntryWithLongestPrefixAnd()`
+ */
+ template <typename ValueT>
+ typename std::map<MemoryID, ValueT>::const_iterator
+ findMostSpecificEntryContainingIDAnd(const std::map<MemoryID, ValueT>& idMap, const MemoryID& id,
+ const std::function<bool(const MemoryID&, const ValueT&)>& predicate)
+ {
+ return detail::findEntryWithLongestPrefixAnd<MemoryID, ValueT>(
+ idMap, &getMemoryIDParent, id, predicate);
+ }
+
+
/**
* @brief Return all values of keys containing the given ID.
*
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
index f1300da6a0d20f6685bc358494d9997c7e4bdb17..6f7ae4d48ffe65665affe83a9356858c57e29044 100644
--- a/source/RobotAPI/libraries/armem/core/operations.h
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -50,6 +50,20 @@ namespace armarx::armem
}
+ template <class ContainerT>
+ std::vector<MemoryID> getEntityIDs(const ContainerT& container)
+ {
+ std::vector<armem::MemoryID> entityIDs;
+
+ container.forEachEntity([&entityIDs](const auto& entity)
+ {
+ entityIDs.push_back(entity.id());
+ });
+
+ return entityIDs;
+ }
+
+
std::string print(const wm::Memory& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::CoreSegment& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::ProviderSegment& data, int maxDepth = -1, int depth = 0);
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 6d44b25f7e2a416df134df83a0efd170c5a99333..f2967422eaf548e5f040bd5b1685f7555e42bdb5 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -176,6 +176,7 @@ set(LIB_HEADERS
wm/memory_definitions.h
wm/ice_conversions.h
wm/detail/MaxHistorySize.h
+ wm/detail/Prediction.h
plugins.h
plugins/Plugin.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 8f37824ff7ff96a8280813247bb17290ae48582b..63b9bba20d1575cc3377109b856d57a09bc4aae8 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -428,13 +428,23 @@ namespace armarx::armem::server
}
// PREDICTION
+ prediction::data::PredictionResultSeq
+ MemoryToIceAdapter::predict(prediction::data::PredictionRequestSeq requests)
+ {
+ auto res = workingMemory->dispatchPredictions(
+ armarx::fromIce<std::vector<PredictionRequest>>(requests));
+ return armarx::toIce<prediction::data::PredictionResultSeq>(res);
+ }
+
prediction::data::EngineSupportMap MemoryToIceAdapter::getAvailableEngines()
{
prediction::data::EngineSupportMap result;
armarx::toIce(result, workingMemory->getAllPredictionEngines());
- prediction::data::EngineSupportMap ltmMap;
- armarx::toIce(ltmMap, workingMemory->getAllPredictionEngines());
+ // Uncomment once LTM also supports prediction engines.
+
+ /*prediction::data::EngineSupportMap ltmMap;
+ armarx::toIce(ltmMap, longtermMemory->getAllPredictionEngines());
for (const auto& [memoryID, engines] : ltmMap)
{
auto entryIter = result.find(memoryID);
@@ -450,7 +460,7 @@ namespace armarx::armem::server
engineSet.insert(engines.begin(), engines.end());
entryIter->second.assign(engineSet.begin(), engineSet.end());
}
- }
+ }*/
return result;
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 6f19796395d85a1ff043e46c2172cba0940e5808..41b0312e6518ebcf78471f42efe23d5bf0eb9634 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -57,6 +57,9 @@ namespace armarx::armem::server
dto::RecordStatusResult getRecordStatus();
// PREDICTION
+ prediction::data::PredictionResultSeq
+ predict(prediction::data::PredictionRequestSeq requests);
+
prediction::data::EngineSupportMap getAvailableEngines();
public:
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 8d0b986443914d60c976da57ae6736c775bc1f54..299e8242ee540df8c5397d86a03a7cae2d1a0eb8 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -146,16 +146,7 @@ namespace armarx::armem::server::plugins
armem::prediction::data::PredictionResultSeq
ReadWritePluginUser::predict(const armem::prediction::data::PredictionRequestSeq& requests)
{
- armem::prediction::data::PredictionResultSeq result;
- for (auto request : requests)
- {
- armem::PredictionResult singleResult;
- singleResult.success = false;
- singleResult.errorMessage = "This memory does not implement predictions.";
- singleResult.prediction = nullptr;
- result.push_back(singleResult.toIce());
- }
- return result;
+ return iceAdapter().predict(requests);
}
armem::prediction::data::EngineSupportMap
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index d90756691871b6ad444a67fa76a733d0086b5b29..2e94c2cdb47a1fb883a6e790240be4c76a21b5e6 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -1,7 +1,7 @@
#include "MaxHistorySize.h"
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
void MaxHistorySize::setMaxHistorySize(long maxSize)
{
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
index 962179a68aeb601dc261eb6d496389295597a2ea..64eb07a63d4d8087378856623ea47c2edcee3b2f 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -1,6 +1,6 @@
#pragma once
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
// TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
new file mode 100644
index 0000000000000000000000000000000000000000..a438226d6f865764810a18cba5617f4d1288e696
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
@@ -0,0 +1,295 @@
+/*
+ * 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 flied 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::core::base::detail
+ * @author phesch ( phesch at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+#include <RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h>
+#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
+
+namespace armarx::armem::server::wm::detail
+{
+
+ using Predictor = std::function<PredictionResult(const PredictionRequest&)>;
+
+ /**
+ * Can do predictions, but has no children it could delegate predictions to.
+ *
+ * This class is integrated with `armem::base::detail::Predictive`:
+ * If `DerivedT` is also a `Predictive`, the setters of this class also update the
+ * `Predictive` part.
+ */
+ template <class DerivedT>
+ class Prediction
+ {
+ using Predictive = armem::base::detail::Predictive<DerivedT>;
+
+ Predictive* _asPredictive()
+ {
+ return dynamic_cast<Predictive*>(&base::detail::derived<DerivedT>(this));
+ }
+
+ public:
+ explicit Prediction(const std::map<PredictionEngine, Predictor>& predictors = {})
+ {
+ this->setPredictors(predictors);
+ }
+
+ void
+ addPredictor(const PredictionEngine& engine, Predictor&& predictor)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->addPredictionEngine(engine);
+ }
+ }
+
+ void
+ setPredictors(const std::map<PredictionEngine, Predictor>& predictors)
+ {
+ this->_predictors.clear();
+ for (const auto& [engine, predictor] : predictors)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+ }
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->setPredictionEngines(simox::alg::get_keys(predictors));
+ }
+ }
+
+ /**
+ * Resolves mapping of requests to predictors and dispatches them.
+ *
+ * In this case, the resolution is basically no-op because there are no children.
+ */
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ std::vector<PredictionResult> results;
+ for (const auto& request : requests)
+ {
+ results.push_back(dispatchTargetedPrediction(request, ownID));
+ }
+ return results;
+ }
+
+ /**
+ * Dispatches a single prediction request (assuming resolution was done by the caller).
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ if (ownID == target)
+ {
+ auto it = _predictors.find(request.predictionSettings.predictionEngineID);
+ if (it != _predictors.end())
+ {
+ const Predictor& predictor = it->second;
+ result = predictor(request);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " with engine '" << request.predictionSettings.predictionEngineID
+ << "' in " << ownID << ": Engine not registered.";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID;
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+ std::map<std::string, Predictor> _predictors; // NOLINT
+ };
+
+
+ /**
+ * Can do predictions itself and has children it could delegate predictions to.
+ */
+ template <class DerivedT>
+ class PredictionContainer : public Prediction<DerivedT>
+ {
+ public:
+ using Prediction<DerivedT>::Prediction;
+
+ explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {})
+ : Prediction<DerivedT>(predictors)
+ {
+ }
+
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ const std::map<MemoryID, std::vector<PredictionEngine>> engines =
+ derivedThis.getAllPredictionEngines();
+
+ std::vector<PredictionResult> results;
+ for (const PredictionRequest& request : requests)
+ {
+ PredictionResult& result = results.emplace_back();
+ result.snapshotID = request.snapshotID;
+
+ auto iter =
+ armem::findMostSpecificEntryContainingIDAnd<std::vector<PredictionEngine>>(
+ engines, request.snapshotID,
+ [&request](const MemoryID& /*unused*/,
+ const std::vector<PredictionEngine>& supported) -> bool
+ {
+ for (const PredictionEngine& engine : supported)
+ {
+ if (engine.engineID == request.predictionSettings.predictionEngineID)
+ {
+ return true;
+ }
+ }
+ return false;
+ });
+
+ if (iter != engines.end())
+ {
+ const MemoryID& responsibleID = iter->first;
+
+ result = dispatchTargetedPrediction(request, responsibleID);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find segment offering prediction engine '"
+ << request.predictionSettings.predictionEngineID << "' for memory ID "
+ << request.snapshotID << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ return results;
+ }
+
+
+ /**
+ * Semantics: This container or one of its children (target) is responsible
+ * for performing the prediction.
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ MemoryID ownID = derivedThis.id();
+ if (ownID == target)
+ {
+ // Delegate to base class.
+ result = Prediction<DerivedT>::dispatchTargetedPrediction(request, target);
+ }
+ // Check if of this' children is really responsible for the request.
+ else if (contains(ownID, target))
+ {
+ std::string childName = _getChildName(ownID, target);
+
+ // TODO(phesch): Looping over all the children just to find the one
+ // with the right name isn't nice, but it's the interface we've got.
+ // TODO(RainerKartmann): Try to add findChild() to loopup mixins.
+ typename DerivedT::ChildT* child = nullptr;
+ derivedThis.forEachChild(
+ [&child, &childName](auto& otherChild)
+ {
+ if (otherChild.name() == childName)
+ {
+ child = &otherChild;
+ }
+ });
+ if (child)
+ {
+ result = child->dispatchTargetedPrediction(request, target);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find memory item with ID " << target << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID << ".";
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+
+ std::string _getChildName(const MemoryID& parent, const MemoryID& child)
+ {
+ ARMARX_CHECK(armem::contains(parent, child));
+ ARMARX_CHECK(parent != child);
+
+ size_t parentLength = parent.getItems().size();
+
+ // Get iterator to first entry of child ID (memory).
+ std::vector<std::string> childItems = child.getItems();
+
+ size_t index = parentLength;
+ ARMARX_CHECK_FITS_SIZE(index, childItems.size());
+
+ return childItems[index];
+ }
+
+
+ };
+
+} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 35da4691d69dcb4a0290d84f19098608b7d3aab6..973e3ca5c21f1913f5abb84718789b4d4d27b2d2 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -3,6 +3,8 @@
#include "error.h"
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
@@ -107,5 +109,4 @@ namespace armarx::armem::server::wm
result.memoryUpdateType = UpdateType::UpdatedExisting;
return result;
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index e6889b7f8e0a6b40f16d28ad2bcc05f68250b1a5..35015353a2653ca957bb19fe29c7e8430790885d 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -1,6 +1,7 @@
#pragma once
#include "detail/MaxHistorySize.h"
+#include "detail/Prediction.h"
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
@@ -60,6 +61,7 @@ namespace armarx::armem::server::wm
public base::ProviderSegmentBase<Entity, ProviderSegment>
, public detail::MaxHistorySizeParent<ProviderSegment>
, public armem::wm::detail::FindInstanceDataMixin<ProviderSegment>
+ , public armem::server::wm::detail::Prediction<ProviderSegment>
{
public:
@@ -82,9 +84,10 @@ namespace armarx::armem::server::wm
/// @brief base::CoreSegmentBase
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- public detail::MaxHistorySizeParent<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ , public detail::MaxHistorySizeParent<CoreSegment>
, public armem::wm::detail::FindInstanceDataMixin<CoreSegment>
+ , public armem::server::wm::detail::PredictionContainer<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
@@ -125,6 +128,7 @@ namespace armarx::armem::server::wm
class Memory :
public base::MemoryBase<CoreSegment, Memory>
, public armem::wm::detail::FindInstanceDataMixin<Memory>
+ , public armem::server::wm::detail::PredictionContainer<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
diff --git a/source/RobotAPI/libraries/armem/util/prediction_helpers.h b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..1734dca8f20e488e789e0c39af380b24376a5e9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
@@ -0,0 +1,135 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <ArmarXCore/core/time.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem
+{
+ /**
+ * Holds info on snapshot data extracted from a time range.
+ * Used for performing predictions.
+ */
+ template <typename DataType, typename LatestType>
+ struct SnapshotRangeInfo
+ {
+ bool success = false;
+ std::string errorMessage = "";
+
+ std::vector<double> timestampsSec = {};
+ std::vector<DataType> values = {};
+ LatestType latestValue;
+ };
+
+ /*!
+ * @brief Get data points for the snapshots of an entity in a given time range.
+ *
+ * The resulting `SnapshotRangeInfo` is useful for performing predictions.
+ * Data from the individual snapshots is given to the conversion functions
+ * before being stored in the result.
+ * Timestamps in the `timestampsSec` vector are relative to the endTime parameter
+ * to this function.
+ * Data from a snapshot is always retrieved from the instance with index 0.
+ * If some data cannot be retrieved, `success` is set to `false`
+ * and the `errorMessage` in the return value is set accordingly.
+ *
+ * @param segment the segment to get the snapshot data from
+ * @param entityID the entity to get the data for
+ * @param startTime the beginning of the time range
+ * @param endTime the end of the time range. Timestamps are relative to this value
+ * @param dictToData the conversion function from `DictPtr` to `DataType`
+ * @param dictToLatest the conversion function from `DictPtr` to `LatestType`
+ * @return the corresponding `LatestSnapshotInfo`
+ */
+ template <typename SegmentType, typename DataType, typename LatestType>
+ SnapshotRangeInfo<DataType, LatestType>
+ getSnapshotsInRange(const SegmentType* segment,
+ const MemoryID& entityID,
+ const DateTime& startTime,
+ const DateTime& endTime,
+ std::function<DataType(const aron::data::DictPtr&)> dictToData,
+ std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
+ {
+ SnapshotRangeInfo<DataType, LatestType> result;
+ result.success = false;
+
+ const server::wm::Entity* entity = segment->findEntity(entityID);
+ if (entity == nullptr)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find entity with ID " << entityID << ".";
+ result.errorMessage = sstream.str();
+ return result;
+ }
+
+ const int instanceIndex = 0;
+ bool querySuccess = true;
+ entity->forEachSnapshotInTimeRange(
+ startTime,
+ endTime,
+ [&](const wm::EntitySnapshot& snapshot)
+ {
+ const auto* instance = snapshot.findInstance(instanceIndex);
+ if (instance)
+ {
+ result.timestampsSec.push_back(
+ (instance->id().timestamp - endTime).toSecondsDouble());
+ result.values.emplace_back(dictToData(instance->data()));
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex
+ << " in snapshot " << snapshot.id() << ".";
+ result.errorMessage = sstream.str();
+ querySuccess = false;
+ }
+ });
+
+ if (querySuccess)
+ {
+ aron::data::DictPtr latest = entity->findLatestInstanceData(instanceIndex);
+ if (latest)
+ {
+ result.success = true;
+ result.latestValue = dictToLatest(latest);
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex << " for entity "
+ << entity->id() << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+
+ return result;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 17dfd176e77f561e744be85802df356edbf4d804..76112d64cfa0788fadfbcc5e86f923b8bf1d978b 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -82,20 +82,33 @@ namespace armarx::armem::grasping::known_grasps
qb
.coreSegments().withName(properties.coreSegmentName)
.providerSegments().all()
- .entities().withName(entityName)
+ .entities().withName(entityName) // first, we search for the input entity, which may be dataset/class/index
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ ARMARX_INFO << "Lookup result in reader: " << qResult;
if (not qResult.success) /* c++20 [[unlikely]] */
{
return std::nullopt;
}
- return queryKnownGraspInfo(qResult.memory, timestamp);
+ auto ret = queryKnownGraspInfo(qResult.memory, timestamp);
+
+ if (not ret)
+ {
+ // No grasp info was found. Try if we find a grasp without the object entity index
+ auto split = simox::alg::split(entityName, "/");
+ if (split.size() > 2) // there is more than just dataset/class
+ {
+ ARMARX_INFO << "No grasp found for object entity " << entityName << ". Search for grasp without the index";
+ return queryKnownGraspInfoByEntityName(split[0] + "/" + split[1], timestamp);
+ }
+ }
+
+ return ret;
}
} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
index 945670d82d8b2b130da2e04b3aad0ace5977efee..e421d4ee185b092ece8f81f3af45116e04c3d723 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
@@ -58,7 +58,7 @@ namespace armarx::armem::grasping::known_grasps
armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index f1ad93a2c87396e2f178d2ed522d39dcd7de95b9..708144660674c9e874cc82d83ffa9e5779cd9d83 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -66,9 +66,10 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
retGrasp.creator = graspNode.attribute_value("Creation");
- ARMARX_CHECK_EQUAL(graspNode.nodes().size(), 1);
+ ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
RapidXmlReaderNode transformNode = graspNode.nodes()[0];
+ ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index baf2fc6b4d3b8a7f72b2f63a286b6db06d6fefea..8b2b4f1d70cc65a416c8aea58c3bd165b1bcdb3f 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -43,6 +43,25 @@ namespace armarx::armem::gui::instance
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ const aron::type::AnyObject& type,
+ const aron::data::Dict& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->updateDispatch(item, key, nullptr, childData);
+ }
+ return true;
+ });
+
+ builder.updateTreeWithContainer(parent, data.getAllKeys());
+ }
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
@@ -236,8 +255,10 @@ namespace armarx::armem::gui::instance
{
_updateTree(item, *t, d ? *d : emptyList);
}
- // else???
- // phesch: else we stop here, since there's no container to recurse into.
+ else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::AnyObject::DynamicCast(type))
+ {
+ _updateTree(item, *t, d ? *d : emptyDict);
+ }
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
index b8aacc686df4dbf2119d963f5fc163aabe0c52bf..750eae39bc7020962e2a0cb275e38bc29faae2eb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -32,6 +32,9 @@ namespace armarx::armem::gui::instance
void updateTree(QTreeWidgetItem* parent,
const aron::type::Dict& type,
const aron::data::Dict& data);
+ void updateTree(QTreeWidgetItem* parent,
+ const aron::type::AnyObject& type,
+ const aron::data::Dict& data);
void updateTree(QTreeWidgetItem* parent,
const aron::type::Object& type,
const aron::data::Dict& data);
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
index 0856ff1d24329de8c4718c569072e45bbce2116a..a32ced5e4ef945ff4426c75b074cc24434b889c3 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
@@ -41,7 +41,7 @@ namespace armarx::armem::gui
timestampLayout(new QHBoxLayout()),
instanceSpinner(new QSpinBox()),
predictionEngineSelector(new QComboBox()),
- predictButton(new QPushButton("Make prediction")),
+ predictButton(new QPushButton("Predict")),
entityInfoRetriever(entityInfoRetriever)
{
auto* vlayout = new QVBoxLayout();
@@ -51,13 +51,13 @@ namespace armarx::armem::gui
vlayout->addLayout(hlayout);
timestampInputSelector->addItems({"Absolute", "Relative to now"});
- timestampLayout->addWidget(new QLabel("Prediction time"));
+ timestampLayout->addWidget(new QLabel("Time"));
timestampLayout->addWidget(timestampInputSelector);
vlayout->addLayout(timestampLayout);
hlayout = new QHBoxLayout();
predictionEngineSelector->setSizeAdjustPolicy(QComboBox::AdjustToContents);
- hlayout->addWidget(new QLabel("Prediction engine"));
+ hlayout->addWidget(new QLabel("Engine"));
hlayout->addWidget(predictionEngineSelector);
hlayout->addStretch();
vlayout->addLayout(hlayout);
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
index 04b28cde489ae3f1efb4804ef574cb1a40516fb9..51aade002b3e6c607cf951463801e3aa281667b5 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
@@ -43,7 +43,7 @@ namespace armarx::armem::gui
microseconds->setSuffix(" µs");
auto* hlayout = new QHBoxLayout();
- hlayout->addWidget(new QLabel("Time"));
+ // hlayout->addWidget(new QLabel("Time"));
hlayout->addWidget(dateTime);
hlayout->addWidget(new QLabel("."));
hlayout->addWidget(microseconds);
@@ -66,7 +66,7 @@ namespace armarx::armem::gui
seconds->setValue(0);
auto* hlayout = new QHBoxLayout();
- hlayout->addWidget(new QLabel("Seconds"));
+ // hlayout->addWidget(new QLabel("Seconds"));
hlayout->addWidget(seconds);
setLayout(hlayout);
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 6cbb3ecdcfee0334df79a9d6728080c5bce55f5f..54049d8bbeb899b350dfab4906a125157ea527b9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -28,7 +28,7 @@ namespace armarx::armem::articulated_object
{"PriorKnowledgeData"}, obj.getFilename()),
obj.getFilename())
},
- .instance = "", // TODO(fabian.reister):
+ .instance = obj.getName(),
.config = {
.timestamp = timestamp,
.globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 861ff14b97addd9b87fee4a834bd605f20b2b681..a515fe725b51ff8302a129053996e97d6738872c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -21,8 +21,8 @@
namespace armarx::armem::obj::instance
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider) :
+ memoryNameSystem(memoryNameSystem), objPoseProvider(objPoseProvider)
{}
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -55,6 +55,16 @@ namespace armarx::armem::obj::instance
}
}
+ bool Reader::requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until)
+ {
+ auto entityNameParts = GetEntityNameParts(entityName);
+
+ armarx::data::ObjectID requestObject({std::get<0>(entityNameParts), std::get<1>(entityNameParts), std::get<2>(entityNameParts)});
+ armarx::objpose::provider::RequestObjectsOutput requestResult = objPoseProvider->requestObjects({{requestObject}, until.toMilliSeconds()});
+
+ return requestResult.results.at(requestObject).success;
+ }
+
/// get the latest object from an memory and cast it to an ObjectInstance
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 825687d5b6bf17acad4515841e9852c9f020dbed..4900004e06517e824053b4ed080de02c42aeb3a2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -32,23 +32,29 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+// The object pose provider. As long as the provider is not connected to armem we need the second proxy
+#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
namespace armarx::armem::obj::instance
{
class Reader
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
+ // localization stuff
+ bool requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until);
+
+ // query existing instances from the memory
std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&);
std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
- // return the class name, e.g. Kitchen/greencup in Kitchen/greencup/0
+ // return the dataset/class, e.g. Kitchen/greencup in Kitchen/greencup/0
static std::string GetObjectId(const std::string& s)
{
auto split = simox::alg::split(s, "/");
@@ -69,7 +75,40 @@ namespace armarx::armem::obj::instance
return "";
}
- // return the class name, e.g. greencup in Kitchen/greencup/0
+ // return all parts of the entity name
+ static std::tuple<std::string, std::string, std::string> GetEntityNameParts(const std::string& s)
+ {
+ std::string dataset = "";
+ std::string clazz = "";
+ std::string instance = "";
+
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ for (auto& e : split)
+ {
+ e = simox::alg::replace_all(e, "/", "");
+ }
+
+ if (split.size() > 0)
+ {
+ dataset = split[0];
+ }
+ if (split.size() > 1)
+ {
+ clazz = split[1];
+ }
+ if (split.size() > 2)
+ {
+ instance = split[2];
+ }
+ return {dataset, clazz, instance};
+ }
+
+ // return the class, e.g. greencup in Kitchen/greencup/0
static std::string GetObjectClassName(const std::string& s)
{
auto split = simox::alg::split(s, "/");
@@ -133,8 +172,10 @@ namespace armarx::armem::obj::instance
const std::string propertyPrefix = "mem.obj.instance.";
armem::client::MemoryNameSystem& memoryNameSystem;
+ objpose::ObjectPoseProviderPrx objPoseProvider;
+
armem::client::Reader memoryReader;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
index 1c9f82e01dd6e48f4264b5de597413fb0b2eb57c..972227993aa5c6369ff656a594fa2e2005b0f9dd 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
@@ -60,7 +60,7 @@ namespace armarx::armem::obj::instance
armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index 9f977a7beaa67789b181cbabb18752cbf4242fef..74e29b855363472523ae328b2698059a8f9d108f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -36,6 +36,8 @@ armarx_add_library(
instance/Visu.h
instance/ArticulatedObjectVisu.h
+ instance/visu/LinearPredictionsVisu.h
+
attachments/Segment.h
SOURCES
@@ -49,6 +51,8 @@ armarx_add_library(
instance/Visu.cpp
instance/ArticulatedObjectVisu.cpp
+ instance/visu/LinearPredictionsVisu.cpp
+
attachments/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 1bac179ed97c29d431f3f47107ad6e367677b3da..9076cedadf778bb46f64f2c90388ff8d197436e7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -53,8 +53,7 @@ namespace armarx::armem::server::obj::instance
SpecializedCoreSegment(memoryToIceAdapter,
"Instance",
arondto::ObjectInstance::ToAronType(),
- 64,
- predictionEngines)
+ 64)
{
oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
{
@@ -1122,7 +1121,8 @@ namespace armarx::armem::server::obj::instance
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ if (infiniteHistory.hasValueChanged()
+ || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
segment.doLocked([this, &segment]()
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 00456e20382f6d6ad4d573bd7fc6e1820d656433..a77c3e8516a081a57d4c94327150062857f5f233 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -572,7 +572,7 @@ namespace armarx::armem::server::obj::instance
poseHistories[id] = instance::Segment::getObjectPosesInRange(
*entity,
Time::Now() - Duration::SecondsDouble(
- visu.linearPredictionTimeWindowSeconds),
+ visu.linearPredictions.timeWindowSeconds),
Time::Invalid());
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index eb7d90cd267566d10487ce4d2b390ccc63eee586..3a13f6722e5cd2c0f824c630a83367b47af1de72 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -35,27 +35,28 @@ namespace armarx::armem::server::obj::instance
defs->optional(objectFramesScale, prefix + "objectFramesScale",
"Scaling of object frames.");
- defs->optional(gaussiansPosition, prefix + "gaussians.position",
+ gaussians.defineProperties(defs, prefix + "gaussians.");
+
+ defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+ "Prefer articulated object models if available.");
+
+ linearPredictions.defineProperties(defs, prefix + "predictions.linear.");
+ }
+
+
+ void Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(position, prefix + "position",
"Enable showing pose gaussians (position part).");
- defs->optional(gaussiansPositionScale, prefix + "gaussians.positionScale",
+ defs->optional(positionScale, prefix + "positionScale",
"Scaling of pose gaussians (position part).");
- defs->optional(gaussiansOrientation, prefix + "gaussians.position",
+ defs->optional(orientation, prefix + "position",
"Enable showing pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationScale, prefix + "gaussians.positionScale",
+ defs->optional(orientationScale, prefix + "positionScale",
"Scaling of pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationDisplaced, prefix + "gaussians.positionDisplaced",
+ defs->optional(orientationDisplaced, prefix + "positionDisplaced",
"Displace center orientation (co)variance circle arrows along their rotation axis.");
-
- defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
- "Prefer articulated object models if available.");
-
- defs->optional(showLinearPredictions, prefix + "predictions.linear.show",
- "Show arrows linearly predicting object positions.");
- defs->optional(linearPredictionTimeOffsetSeconds, prefix + "predictions.linear.timeOffset",
- "The offset (in seconds) to the current time to make predictions for.");
- defs->optional(linearPredictionTimeWindowSeconds, prefix + "predictions.linear.timeWindow",
- "The time window (in seconds) into the past to perform the regression on.");
}
@@ -168,11 +169,26 @@ namespace armarx::armem::server::obj::instance
const std::string key = id.str();
const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+
+ auto makeObject = [&objectInfo, &id](const std::string& key)
{
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+ viz::Object object(key);
+ if (objectInfo)
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
+ return object;
+ };
+ bool hasObject = false;
+ {
bool done = false;
- if (useArticulatedModels && objectInfo)
+ if (useArticulatedModels and objectInfo)
{
if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
{
@@ -185,18 +201,9 @@ namespace armarx::armem::server::obj::instance
done = true;
}
}
- if (!done)
+ if (not done)
{
- viz::Object object(key);
- object.pose(pose);
- if (objectInfo)
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
+ viz::Object object = makeObject(key).pose(pose);
if (alphaByConfidence && objectPose.confidence < 1.0f)
{
object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
@@ -206,10 +213,11 @@ namespace armarx::armem::server::obj::instance
object.overrideColor(simox::Color::white().with_alpha(alpha));
}
layer.add(object);
+ hasObject = true;
}
}
- if (oobbs && objectPose.localOOBB)
+ if (oobbs and objectPose.localOOBB)
{
const simox::OrientedBoxf oobb = inGlobalFrame
? objectPose.oobbGlobal().value()
@@ -220,83 +228,85 @@ namespace armarx::armem::server::obj::instance
{
layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
}
- if (objectPose.objectPoseGlobalGaussian.has_value() and (gaussiansPosition or gaussiansOrientation))
+ if (gaussians.showAny()
+ and (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value() : objectPose.objectPoseRobotGaussian.has_value()))
{
- const objpose::PoseManifoldGaussian& gaussian = objectPose.objectPoseGlobalGaussian.value();
- objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+ gaussians.draw(layer, objectPose, inGlobalFrame);
+ }
+ if (linearPredictions.showAny() and hasObject)
+ {
+ linearPredictions.draw(layer, makeObject, objectPose, poseHistory, inGlobalFrame);
+ }
+ }
- if (gaussiansPosition)
- {
- layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
- .position(ellipsoid.center)
- .orientation(ellipsoid.orientation)
- .axisLengths(gaussiansPositionScale * ellipsoid.size)
- .color(viz::Color::azure(255, 32))
- );
+ bool Visu::Gaussians::showAny() const
+ {
+ return position or orientation;
+ }
- if (false) // Arrows can be visualized for debugging.
- {
- for (int i = 0; i < 3; ++i)
- {
- Eigen::Vector3i color = Eigen::Vector3i::Zero();
- color(i) = 255;
-
- layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
- .fromTo(ellipsoid.center,
- ellipsoid.center + gaussiansPositionScale * ellipsoid.size(i)
- * ellipsoid.orientation.col(i)
- )
- .width(5)
- .color(simox::Color(color))
- );
- }
- }
- }
- if (gaussiansOrientation)
+ void Visu::Gaussians::draw(
+ viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const
+ {
+ const std::string key = objectPose.objectID.str();
+
+ const objpose::PoseManifoldGaussian& gaussian =
+ inGlobalFrame
+ ? objectPose.objectPoseGlobalGaussian.value()
+ : objectPose.objectPoseRobotGaussian.value()
+ ;
+ objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+
+ if (position)
+ {
+ layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
+ .position(ellipsoid.center)
+ .orientation(ellipsoid.orientation)
+ .axisLengths(positionScale * ellipsoid.size)
+ .color(viz::Color::azure(255, 32))
+ );
+
+ if (false) // Arrows can be visualized for debugging.
{
- const float pi = static_cast<float>(M_PI);
for (int i = 0; i < 3; ++i)
{
- const bool global = true;
- Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
-
- Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ Eigen::Vector3i color = Eigen::Vector3i::Zero();
color(i) = 255;
- color(3) = 64;
-
- layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
- .position(gaussiansOrientationDisplaced
- ? ellipsoid.center + gaussiansOrientationScale * rot.axis()
- : ellipsoid.center)
- .normal(rot.axis())
- .radius(gaussiansOrientationScale)
- .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
- .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+
+ layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
+ .fromTo(ellipsoid.center,
+ ellipsoid.center + positionScale * ellipsoid.size(i)
+ * ellipsoid.orientation.col(i)
+ )
+ .width(5)
.color(simox::Color(color))
);
}
}
}
- if (showLinearPredictions)
+ if (orientation)
{
- auto predictionResult = objpose::predictObjectPoseLinear(
- poseHistory,
- Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds),
- objectPose);
- if (predictionResult.success)
+ const float pi = static_cast<float>(M_PI);
+ for (int i = 0; i < 3; ++i)
{
- auto predictedPose =
- armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
- Eigen::Vector3f predictedPosition = simox::math::position(
- inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot);
- layer.add(viz::Arrow(key + " Linear Prediction")
- .fromTo(simox::math::position(pose), predictedPosition)
- .color(viz::Color::blue()));
- }
- else
- {
- ARMARX_INFO << "Linear prediction for visualization failed: "
- << predictionResult.errorMessage;
+ const bool global = true;
+ Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
+
+ Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ color(i) = 255;
+ color(3) = 64;
+
+ layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
+ .position(orientationDisplaced
+ ? ellipsoid.center + orientationScale * rot.axis()
+ : ellipsoid.center)
+ .normal(rot.axis())
+ .radius(orientationScale)
+ .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
+ .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+ .color(simox::Color(color))
+ );
}
}
}
@@ -324,12 +334,12 @@ namespace armarx::armem::server::obj::instance
objectFrames.setValue(visu.objectFrames);
initScale(objectFramesScale, visu.objectFramesScale, 10);
- gaussians.position.setValue(visu.gaussiansPosition);
- initScale(gaussians.positionScale, visu.gaussiansPositionScale, 10);
+ gaussians.position.setValue(visu.gaussians.position);
+ initScale(gaussians.positionScale, visu.gaussians.positionScale, 10);
- gaussians.orientation.setValue(visu.gaussiansOrientation);
- initScale(gaussians.orientationScale, visu.gaussiansOrientationScale, 0.5);
- gaussians.orientationDisplaced.setValue(visu.gaussiansOrientationDisplaced);
+ gaussians.orientation.setValue(visu.gaussians.orientation);
+ initScale(gaussians.orientationScale, visu.gaussians.orientationScale, 0.5);
+ gaussians.orientationDisplaced.setValue(visu.gaussians.orientationDisplaced);
useArticulatedModels.setValue(visu.useArticulatedModels);
@@ -358,7 +368,7 @@ namespace armarx::armem::server::obj::instance
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
- linearPredictions.setup(visu);
+ linearPredictions.setup(visu.linearPredictions);
grid.add(linearPredictions.group, {row, 0}, {1, 4});
row++;
@@ -390,6 +400,17 @@ namespace armarx::armem::server::obj::instance
}
+ void Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
+ {
+ data.position = position.getValue();
+ data.positionScale = positionScale.getValue();
+
+ data.orientation = orientation.getValue();
+ data.orientationScale = orientationScale.getValue();
+ data.orientationDisplaced = orientationDisplaced.getValue();
+ }
+
+
void Visu::RemoteGui::update(Visu& visu)
{
visu.enabled = enabled.getValue();
@@ -401,56 +422,11 @@ namespace armarx::armem::server::obj::instance
visu.objectFrames = objectFrames.getValue();
visu.objectFramesScale = objectFramesScale.getValue();
- visu.gaussiansPosition = gaussians.position.getValue();
- visu.gaussiansPositionScale = gaussians.positionScale.getValue();
-
- visu.gaussiansOrientation = gaussians.orientation.getValue();
- visu.gaussiansOrientationScale = gaussians.orientationScale.getValue();
- visu.gaussiansOrientationDisplaced = gaussians.orientationDisplaced.getValue();
+ gaussians.update(visu.gaussians);
visu.useArticulatedModels = useArticulatedModels.getValue();
- linearPredictions.update(visu);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::setup(const Visu& visu)
- {
- using namespace armarx::RemoteGui::Client;
-
- show.setValue(visu.showLinearPredictions);
- timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
- timeOffsetSeconds.setRange(-1e6, 1e6);
- timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
-
- timeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
- timeWindowSeconds.setRange(0, 1e6);
- timeWindowSeconds.setSteps(2 * 1000 * 1000);
-
-
- GridLayout grid;
- int row = 0;
-
- grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
- row++;
- grid.add(Label("Time (seconds from now):"), {row, 0})
- .add(timeOffsetSeconds, {row, 1});
- row++;
- grid.add(Label("Time Window (seconds):"), {row, 0})
- .add(timeWindowSeconds, {row, 1});
- row++;
-
- group = GroupBox();
- group.setLabel("Linear Predictions");
- group.addChild(grid);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
- {
- visu.showLinearPredictions = show.getValue();
- visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
- visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
+ linearPredictions.update(visu.linearPredictions);
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 0341f514421e10a9158a446c1cb2c4a7eb3625e6..13a27228aa1198d91d1c0db4c1f809ce93b6f5f1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -9,6 +9,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h>
namespace armarx
@@ -83,19 +84,28 @@ namespace armarx::armem::server::obj::instance
bool objectFrames = false;
float objectFramesScale = 1.0;
- bool gaussiansPosition = true;
- float gaussiansPositionScale = 1.0;
- bool gaussiansOrientation = false;
- float gaussiansOrientationScale = 100;
- bool gaussiansOrientationDisplaced = false;
+ struct Gaussians
+ {
+ bool position = true;
+ float positionScale = 1.0;
+ bool orientation = false;
+ float orientationScale = 100;
+ bool orientationDisplaced = false;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const;
+ };
+ Gaussians gaussians;
/// Prefer articulated models if available.
bool useArticulatedModels = true;
- /// Linear prediction arrows for object positions.
- bool showLinearPredictions = false;
- float linearPredictionTimeOffsetSeconds = 1;
- float linearPredictionTimeWindowSeconds = 2;
+ /// Linear predictions for objects.
+ visu::LinearPredictions linearPredictions;
SimpleRunningTask<>::pointer_type updateTask;
@@ -125,25 +135,13 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::GroupBox group;
void setup(const Visu& data);
+ void update(Visu::Gaussians& data);
};
Gaussians gaussians;
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
-
- struct LinearPredictions
- {
- armarx::RemoteGui::Client::CheckBox show;
-
- armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
- armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
-
- armarx::RemoteGui::Client::GroupBox group;
-
- void setup(const Visu& data);
- void update(Visu& data);
- };
- LinearPredictions linearPredictions;
+ visu::LinearPredictions::RemoteGui linearPredictions;
void setup(const Visu& visu);
void update(Visu& visu);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..789b597d51ab1845ca75375bb13a7ec105dadc3d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
@@ -0,0 +1,146 @@
+#include "LinearPredictionsVisu.h"
+
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/ice_conversions.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ void LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(showGhost, prefix + "showGhost",
+ "Show ghosts at linearly predicted object poses.");
+ defs->optional(ghostAlpha, prefix + "ghostAlpha",
+ "Alpha of linear prediction ghosts.");
+ defs->optional(showFrame, prefix + "showFrame",
+ "Show frames at linearly predicted object poses.");
+ defs->optional(showArrow, prefix + "showArrow",
+ "Show arrows from current object poses to the linearly predicted ones.");
+ defs->optional(timeOffsetSeconds, prefix + "timeOffset",
+ "The offset (in seconds) to the current time to make predictions for.");
+ defs->optional(timeWindowSeconds, prefix + "timeWindow",
+ "The time window (in seconds) into the past to perform the regression on.");
+
+ }
+
+
+ bool LinearPredictions::showAny() const
+ {
+ return showGhost or showFrame or showArrow;
+ }
+
+ void LinearPredictions::draw(
+ viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame
+ ) const
+ {
+ const std::string key = objectPose.objectID.str();
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+
+ auto predictionResult = objpose::predictObjectPoseLinear(
+ poseHistory,
+ DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds),
+ objectPose);
+ if (predictionResult.success)
+ {
+ auto predictedObjectPose =
+ armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
+ const Eigen::Matrix4f& predictedPose =
+ inGlobalFrame ? predictedObjectPose.objectPoseGlobal : predictedObjectPose.objectPoseRobot;
+
+ if (showGhost)
+ {
+ layer.add(makeObjectFn(key + " Linear Prediction Ghost")
+ .pose(predictedPose)
+ .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
+ }
+ if (showFrame)
+ {
+ layer.add(viz::Pose(key + " Linear Prediction Pose")
+ .pose(predictedPose));
+ }
+ if (showArrow)
+ {
+ layer.add(viz::Arrow(key + " Linear Prediction Arrw")
+ .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
+ .width(10)
+ .color(viz::Color::azure()));
+ }
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Linear prediction for visualization failed: "
+ << predictionResult.errorMessage;
+ }
+ }
+
+
+ void LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showGhost.setName("Show Ghost");
+ showGhost.setValue(data.showGhost);
+ showFrame.setValue(data.showFrame);
+ showArrow.setValue(data.showArrow);
+
+ ghostAlpha.setRange(0, 1);
+ ghostAlpha.setValue(0.5);
+
+ timeOffsetSeconds.setValue(data.timeOffsetSeconds);
+ timeOffsetSeconds.setRange(-1e6, 1e6);
+ timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
+
+ timeWindowSeconds.setValue(data.timeWindowSeconds);
+ timeWindowSeconds.setRange(0, 1e6);
+ timeWindowSeconds.setSteps(2 * 1000 * 1000);
+
+ GridLayout grid;
+ int row = 0;
+
+ HBoxLayout showBoxes(
+ {Label("Ghost"), showGhost,
+ Label(" Frame"), showFrame,
+ Label(" Arrow"), showArrow,
+ Label(" Ghost Alpha"), ghostAlpha
+ });
+ grid.add(showBoxes, {row, 0}, {1, 2});
+ row++;
+
+ grid.add(Label("Prediction time (sec from now):"), {row, 0})
+ .add(timeOffsetSeconds, {row, 1});
+ row++;
+
+ grid.add(Label("Model time window (sec before now):"), {row, 0})
+ .add(timeWindowSeconds, {row, 1});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Linear Predictions");
+ group.addChild(grid);
+ }
+
+
+ void LinearPredictions::RemoteGui::update(LinearPredictions& data)
+ {
+ data.showGhost = showGhost.getValue();
+ data.ghostAlpha = ghostAlpha.getValue();
+ data.showFrame = showFrame.getValue();
+ data.showArrow = showArrow.getValue();
+ data.timeOffsetSeconds = timeOffsetSeconds.getValue();
+ data.timeWindowSeconds = timeWindowSeconds.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ede135125c6d44ac62bdd5ecef2fc6eeb2f353c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
@@ -0,0 +1,59 @@
+#pragma once
+
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ /// Visualization control for linear predictions for objects.
+ struct LinearPredictions : public armarx::Logging
+ {
+ bool showGhost = false;
+ float ghostAlpha = 0.7;
+
+ bool showFrame = false;
+ bool showArrow = false;
+
+ float timeOffsetSeconds = 1;
+ float timeWindowSeconds = 2;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame) const;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::CheckBox showGhost;
+ armarx::RemoteGui::Client::FloatSpinBox ghostAlpha;
+
+ armarx::RemoteGui::Client::CheckBox showFrame;
+ armarx::RemoteGui::Client::CheckBox showArrow;
+
+ armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
+ armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
+
+ armarx::RemoteGui::Client::GroupBox group;
+
+ void setup(const LinearPredictions& data);
+ void update(LinearPredictions& data);
+ };
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index fda00f26806d70a922b9487a008099117150d9e4..e9f7993d12a4979b496ee24b73da003e74eda1e3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -1,4 +1,5 @@
#include "TransformHelper.h"
+#include <optional>
#include <string>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -48,12 +49,23 @@ namespace armarx::armem::common::robot_state::localization
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
- const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ const std::optional<armem::Time> sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+
+ if(not sanitizedTimestamp.has_value())
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::Error,
+ .errorMessage = "Error: Issue with timestamp"};
+ }
+
+
auto header = query.header;
+ ARMARX_CHECK(sanitizedTimestamp.has_value());
+
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
- header.timestamp = sanitizedTimestamp;
+ header.timestamp = sanitizedTimestamp.value();
if (transforms.empty())
{
@@ -178,27 +190,43 @@ namespace armarx::armem::common::robot_state::localization
}
template <class ...Args>
- armarx::core::time::DateTime
+ std::optional<armarx::core::time::DateTime>
TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
{
// first we check which the newest timestamp is
- int64_t timeSinceEpochUs = 0;
+ std::optional<int64_t> timeSinceEpochUs = std::nullopt;
localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+
+ if(snapshot == nullptr)
+ {
+ return;
+ }
+
+ if(not snapshot->hasInstance(0))
+ {
+ return;
+ }
+
const armem::wm::EntityInstance& item = snapshot->getInstance(0);
const auto tf = _convertEntityToTransform(item);
const auto& dataTs = tf.header.timestamp;
- timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+ timeSinceEpochUs = std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
});
+ if(not timeSinceEpochUs.has_value())
+ {
+ return std::nullopt;
+ }
+
// then we ensure that the timestamp is not more recent than the query timestamp
- timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+ timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+ return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 6e2611f7a178ba64ab3e25082b5e74a6b968dc23..ae1e0bc5d1d6a355483cbd60626990f2065ffd9b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -113,7 +113,7 @@ namespace armarx::armem::common::robot_state::localization
template <class ...Args>
static
- armarx::core::time::DateTime
+ std::optional<armarx::core::time::DateTime>
_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
static
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 848efe75e0b615f367f5602c9cdfdfb3cdbbd7ea..169d59f1a86d6a1bea1b9e54033409d23fb3cd7f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -7,7 +7,7 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
+armarx_build_if(manif_FOUND "manif not available")
armarx_add_library(
LIBS
@@ -27,6 +27,7 @@ armarx_add_library(
# System / External
Eigen3::Eigen
+ ${manif_LIBRARIES}
HEADERS
forward_declarations.h
@@ -69,5 +70,8 @@ armarx_add_library(
description/Segment.cpp
)
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
add_library(RobotAPI::armem_robot_state_server ALIAS armem_robot_state_server)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index e68119affae5664f232dba25c679a8cce9fbdd10..1607132e51d9370e3ea746193d835f265bf529ff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -42,8 +42,8 @@ namespace armarx::armem::server::robot_state
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(
- p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.framesEnabled, prefix + "famesEnabled", "Enable or disable visualization of frames.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
@@ -199,10 +199,15 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
+ std::vector layers{layer};
ARMARX_DEBUG << "Visualize frames ...";
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
+ if(p.framesEnabled)
+ {
+ viz::Layer layerFrames = arviz.layer("Frames");
+ visualizeFrames(layerFrames, frames);
+ layers.push_back(layerFrames);
+ }
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
@@ -211,7 +216,7 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Commit visualization ...";
TIMING_START(tVisuCommit);
- arviz.commit({layer, layerFrames});
+ arviz.commit(layers);
TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index ce69772c1b1c55553c62e39d3db5e8e4503d2592..66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -84,6 +84,7 @@ namespace armarx::armem::server::robot_state
struct Properties
{
bool enabled = true;
+ bool framesEnabled = false;
float frequencyHz = 25;
} p;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index dc92366e7ab6eb45468bde0a046f32b888e66a6e..c41908c9ac35ca17519e4c7400eb04903791699b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -3,6 +3,11 @@
// STL
#include <iterator>
+#include <manif/SE3.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -12,6 +17,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -36,6 +42,24 @@ namespace armarx::armem::server::robot_state::localization
{
}
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request){ return this->predictLinear(request); });
+ }
+
void Segment::onConnect()
{
@@ -171,4 +195,78 @@ namespace armarx::armem::server::robot_state::localization
return update;
}
+
+ PredictionResult Segment::predictLinear(const PredictionRequest& request)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow =
+ Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Vector6d, arondto::Transform> info;
+
+ doLocked(
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment, Vector6d, arondto::Transform>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data)
+ {
+ Eigen::Matrix4d mat =
+ arondto::Transform::FromAron(data).transform.cast<double>();
+ manif::SE3d se3(simox::math::position(mat),
+ Eigen::Quaterniond(simox::math::orientation(mat)));
+ return se3.log().coeffs();
+ },
+ [](const aron::data::DictPtr& data)
+ { return arondto::Transform::FromAron(data); });
+ });
+
+ if (info.success)
+ {
+ Eigen::Matrix4f prediction;
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = info.latestValue.transform;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression<tangentDims> model = LinearRegression<tangentDims>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ Vector6d linearPred =
+ model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
+ }
+
+ info.latestValue.transform = prediction;
+ result.success = true;
+ result.prediction = info.latestValue.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index ba7138fd0c8a4bbe6467ebc00c68e3e743643f29..0f1fe71345bf1d2767f5cfc7b0c268e1f41e7eba 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -49,6 +49,9 @@ namespace armarx::armem::server::robot_state::localization
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
void onConnect();
@@ -67,6 +70,13 @@ namespace armarx::armem::server::robot_state::localization
EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
+ PredictionResult predictLinear(const PredictionRequest& request);
+
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index ca9e43ed9d4a7022dd4c6a4eecba530e81bf7b21..3351772f3344698e006c06a92b86dcaff0868801 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,5 +1,7 @@
#include "Segment.h"
+#include <SimoxUtility/math/regression/linear.hpp>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -9,6 +11,7 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -16,12 +19,34 @@ namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::ToAronType(), 1024)
+ Base(memoryToIceAdapter,
+ "Proprioception",
+ arondto::Proprioception::ToAronType(),
+ 1024)
{
}
Segment::~Segment() = default;
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(
+ armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request) { return this->predictLinear(request); });
+ }
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
@@ -129,6 +154,127 @@ namespace armarx::armem::server::robot_state::proprioception
return robotUnitProviderID;
}
+ Eigen::VectorXd readJointData(const wm::EntityInstanceData& data)
+ {
+ namespace adn = aron::data;
+
+ std::vector<double> values;
+
+ auto addData =
+ [&](adn::DictPtr dict) // NOLINT
+ {
+ for (const auto& [name, value] : dict->getElements())
+ {
+ values.push_back(
+ static_cast<double>(adn::Float::DynamicCastAndCheck(value)->getValue()));
+ }
+ };
+
+ if (adn::DictPtr joints = getDictElement(data, "joints"))
+ {
+ if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
+ {
+ addData(jointsPosition);
+ }
+ if (adn::DictPtr jointsVelocity = getDictElement(*joints, "velocity"))
+ {
+ addData(jointsVelocity);
+ }
+ if (adn::DictPtr jointsTorque = getDictElement(*joints, "torque"))
+ {
+ addData(jointsTorque);
+ }
+ }
+ Eigen::VectorXd vec =
+ Eigen::Map<Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+ return vec;
+ }
+
+ void
+ emplaceJointData(const Eigen::VectorXd& jointData,
+ arondto::Proprioception& dataTemplate)
+ {
+ Eigen::Index row = 0;
+ for (auto& [joint, value] : dataTemplate.joints.position)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.velocity)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.torque)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ }
+
+ armem::PredictionResult
+ Segment::predictLinear(const armem::PredictionRequest& request) const
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow = Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Eigen::VectorXd, aron::data::DictPtr> info;
+
+ doLocked(
+ // Default capture because the number of variables was getting out of hand
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment,
+ Eigen::VectorXd,
+ aron::data::DictPtr>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data) { return readJointData(*data); },
+ [](const aron::data::DictPtr& data) { return data; });
+ });
+
+ if (info.success)
+ {
+ Eigen::VectorXd latestJoints = readJointData(*info.latestValue);
+ Eigen::VectorXd prediction(latestJoints.size());
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = latestJoints;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression model = LinearRegression<Eigen::Dynamic>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ }
+
+ arondto::Proprioception templateData =
+ arondto::Proprioception::FromAron(info.latestValue);
+ emplaceJointData(prediction, templateData);
+ result.success = true;
+ result.prediction = templateData.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
std::map<std::string, float>
Segment::readJointPositions(const wm::EntityInstanceData& data)
@@ -151,4 +297,4 @@ namespace armarx::armem::server::robot_state::proprioception
return jointPositions;
}
-} // namespace armarx::armem::server::robot_state::proprioception
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 808fbd7905c1ec6e44e51296ea959e7e3af755d5..8123aea55ea50a11d02b8bf1d50aee0fbe5b09dc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -32,6 +32,7 @@
// RobotAPI
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
@@ -52,6 +53,10 @@ namespace armarx::armem::server::robot_state::proprioception
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
+
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
@@ -62,6 +67,8 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::MemoryID& getRobotUnitProviderID() const;
+ armem::PredictionResult predictLinear(const armem::PredictionRequest& request) const;
+
private:
@@ -75,6 +82,12 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitInterfacePrx robotUnit;
armem::MemoryID robotUnitProviderID;
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
+
// Debug Observer prefix
const std::string dp = "Proprioception::getRobotJointPositions() | ";
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
index b84f79b64e878a53828d95ff70ec6c586da0113b..84b9840794dbd1a618b51e5219732924949c1b8e 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -49,7 +49,7 @@ The memory should look like the following:
<String />
</ObjectChild>
- <ObjectChild key='clientId'>
+ <ObjectChild key='executorName'>
<String />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 0181e2729c0ac1ff1f70ac433d73c298da5272c8..51c72a4caa79ca48823dc06670d90989cf2c2d07 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -39,8 +39,8 @@ namespace armarx::skills::segment
// create commit about new update
armarx::skills::arondto::SkillExecutionEvent event;
- event.providerName = update.header.providerName;
- event.skillName = update.header.skillName;
+ event.providerName = update.header.skillId.providerName;
+ event.skillName = update.header.skillId.skillName;
event.status = ExecutionStatus2String.at(update.header.status);
event.params = aron::data::Dict::FromAronDictDTO(update.header.usedParams);
event.data = aron::data::Dict::FromAronDictDTO(update.data);
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 8a60cb6689e79231e2b0ceb046bc2832899f29a6..f8333282e1c09b206ef8829cbda44c64bde470ec 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -37,8 +37,7 @@ namespace armarx::skills::segment
request.fromAron(commitDataAron);
skills::manager::dto::SkillExecutionRequest info;
- info.providerName = request.providerName;
- info.skillName = request.skillName;
+ info.skillId = {request.providerName, request.skillName};
info.params = request.params->toAronDictDTO();
return info;
}
@@ -48,24 +47,41 @@ namespace armarx::skills::segment
{
// override directly execution to add a request to the memory
armem::Commit comm;
- auto& entityUpdate = comm.add();
skills::arondto::SkillExecutionRequest request;
- request.clientId = "";
- request.providerName = info.providerName;
- request.skillName = info.skillName;
+ request.executorName = info.executorName;
+ request.providerName = info.skillId.providerName;
+ request.skillName = info.skillId.skillName;
request.params = aron::data::Dict::FromAronDictDTO(info.params);
auto aron = request.toAron();
- armem::MemoryID skillExecutionMemID = id();
- skillExecutionMemID.providerSegmentName = request.providerName;
- skillExecutionMemID.entityName = request.skillName;
+ {
+ auto& entityUpdate = comm.add();
+
+ armem::MemoryID skillExecutionMemID = id();
+ skillExecutionMemID.providerSegmentName = request.providerName;
+ skillExecutionMemID.entityName = request.skillName;
+
+ entityUpdate.entityID = skillExecutionMemID;
+ entityUpdate.instancesData = { aron };
+ entityUpdate.confidence = 1.0;
+ entityUpdate.timeCreated = armem::Time::Now();
+ }
+
+ {
+ auto& entityUpdate = comm.add();
+
+ armem::MemoryID skillExecutionMemID = id();
+ skillExecutionMemID.providerSegmentName = "All Skill Execution Requests";
+ skillExecutionMemID.entityName = "All Skill Execution Requests";
+
+ entityUpdate.entityID = skillExecutionMemID;
+ entityUpdate.instancesData = { aron };
+ entityUpdate.confidence = 1.0;
+ entityUpdate.timeCreated = armem::Time::Now();
+ }
- entityUpdate.entityID = skillExecutionMemID;
- entityUpdate.instancesData = { aron };
- entityUpdate.confidence = 1.0;
- entityUpdate.timeCreated = armem::Time::Now();
iceMemory.commit(comm);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
index 8cdd98da4fd8d151225f671bc0ed4c5e3c2ef0cf..add1b800c01ad8186008d55574559e0705527a4b 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -209,7 +209,7 @@ namespace armarx::aron::codegenerator::cpp
doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::NONE, const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path())", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = ::armarx::aron::type::Maybe::NONE, const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = ::armarx::aron::Path())", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
@@ -226,12 +226,16 @@ namespace armarx::aron::codegenerator::cpp
doc << "@param w - The writer implementation\n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
+ b->addLine("try");
std::string dummy;
- b->appendBlock(this->getWriteBlock("", Path(), dummy));
+ b->addBlock(this->getWriteBlock("", Path(), dummy));
+ b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the write method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
+
m->setBlock(b);
return m;
}
@@ -250,8 +254,12 @@ namespace armarx::aron::codegenerator::cpp
b->addLine("this->resetSoft();");
b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))");
- b->addLineAsBlock("throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\");");
- b->appendBlock(this->getReadBlock("", "input"));
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\");");
+
+ b->addLine("try");
+ b->addBlock(this->getReadBlock("", "input"));
+ b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the read method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
return m;
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
index b81341ae2bf19f38cbb9928eafdbf9d3f0f9ffe5..e3c2913dd52b5924ea7d02f9c5a402626788f7e2 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
@@ -59,7 +59,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
variantAccessor = Generator::ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = armarx::aron::data::readAndWrite<armarx::aron::data::FromVariantConverter<WriterT>>(" + cppAccessor + "); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = ::armarx::aron::data::readAndWrite<::armarx::aron::data::FromVariantConverter<WriterT>>(" + cppAccessor + "); // of " + cppAccessor);
return block_if_data;
}
@@ -69,7 +69,7 @@ namespace armarx::aron::codegenerator::cpp::generator
auto block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
- block_if_data->addLine(cppAccessor + " = armarx::aron::data::Dict::DynamicCastAndCheck(armarx::aron::data::readAndWrite<aron::data::ToVariantConverter<ReaderT>>(" + variantAccessor + ")); // of " + cppAccessor);
+ block_if_data->addLine(cppAccessor + " = ::armarx::aron::data::Dict::DynamicCastAndCheck(::armarx::aron::data::readAndWrite<::armarx::aron::data::ToVariantConverter<ReaderT>>(" + variantAccessor + ")); // of " + cppAccessor);
return block_if_data;
}
@@ -81,7 +81,7 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeAnyObject(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ "::armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt b/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f9e545e5c4caceed94f27109f824468e111c742e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt
@@ -0,0 +1,58 @@
+#armarx_add_library(AronComponentConfig
+# DEPENDENCIES
+# ArmarXCoreInterfaces ArmarXCore ArmarXGuiComponentPlugins
+# RobotAPIInterfaces RobotAPICore RobotAPIComponentPlugins
+# SOURCES
+# PropertyDefinitionVisitors.cpp
+# RemoteGuiVisitors.cpp
+# HEADERS
+# PropertyDefinitionVisitors.h
+# RemoteGui.h
+# RemoteGuiVisitors.h
+# Util.h
+# ComponentPlugin.h
+# )
+
+set(LIB_NAME aron_component_config)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+
+set(LIBS
+ ArmarXCoreInterfaces ArmarXCore ArmarXGuiComponentPlugins
+ RobotAPIInterfaces RobotAPICore RobotAPIComponentPlugins
+)
+
+set(LIB_FILES
+ PropertyDefinitionVisitors.cpp
+ RemoteGuiVisitors.cpp
+)
+
+set(LIB_HEADERS
+ PropertyDefinitionVisitors.h
+ RemoteGui.h
+ RemoteGuiVisitors.h
+ Util.h
+ ComponentPlugin.h
+)
+
+
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
+)
+
+add_library(
+ RobotAPI::aron_component_config
+ ALIAS
+ aron_component_config
+)
diff --git a/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb3c794e6c3b77d5bc0c71459f7038f7d13c5153
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
@@ -0,0 +1,157 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
+#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h>
+#include "PropertyDefinitionVisitors.h"
+#include "RemoteGui.h"
+
+
+namespace armarx::plugins
+{
+ template <typename T> concept isAronGenerated = std::is_base_of<armarx::aron::cpp::AronGeneratedClass, T>::value;
+
+ template <typename AronStructT>
+ requires isAronGenerated<AronStructT>
+ class AronComponentConfigPlugin : public ComponentPlugin
+ {
+ protected:
+ void preOnInitComponent() override
+ {
+ ManagedIceObjectPlugin::preOnInitComponent();
+ }
+
+ void postOnInitComponent() override
+ {
+ ManagedIceObjectPlugin::postOnInitComponent();
+ }
+
+ void preOnConnectComponent() override
+ {
+ armarx::aron::component_config::PropertyDefinitionGetterVisitor vis(parent<armarx::PropertyUser>());
+ auto data = std::static_pointer_cast<armarx::aron::data::Variant>(config_.getUpToDateReadBuffer().toAron());
+ auto type = config_.getUpToDateReadBuffer().ToAronType();
+ armarx::aron::data::visitRecursive(vis, data, type);
+ config_.getWriteBuffer().fromAron(std::static_pointer_cast<armarx::aron::data::Dict>(data));
+ config_.commitWrite();
+
+ remote_gui_plugin_
+ ->createOrUpdateTab(RemoteGui::makeConfigGui("Config", config_.getUpToDateReadBuffer()),
+ [this](armarx::RemoteGui::TabProxy& prx)
+ {
+ ARMARX_TRACE;
+ prx.receiveUpdates();
+ armarx::aron::component_config::GetValueFromMapVisitor vis(&prx);
+ auto& cfg = config_.getWriteBuffer();
+ auto data = std::static_pointer_cast<armarx::aron::data::Variant>(cfg.toAron());
+ auto type = cfg.ToAronType();
+ armarx::aron::data::visitRecursive(vis, data, type);
+ cfg.fromAron(std::static_pointer_cast<armarx::aron::data::Dict>(data));
+ config_.commitWrite();
+ if (vis.tabRebuildRequired())
+ {
+ remote_gui_plugin_->createOrUpdateTab("", RemoteGui::makeConfigGui("Config", config_.getUpToDateReadBuffer()));
+ }
+ });
+ ManagedIceObjectPlugin::preOnConnectComponent();
+ }
+
+ void postOnConnectComponent() override
+ {
+ ManagedIceObjectPlugin::postOnConnectComponent();
+ }
+
+ void postOnDisconnectComponent() override
+ {
+ ManagedIceObjectPlugin::postOnDisconnectComponent();
+ }
+
+ void postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override
+ {
+ armarx::aron::component_config::PropertyDefinitionSetterVisitor vis(properties);
+ const auto& config = config_.getUpToDateReadBuffer();
+ armarx::aron::data::visitRecursive(vis, config.toAron(), config.ToAronType());
+ ComponentPlugin::postCreatePropertyDefinitions(properties);
+ }
+
+ public:
+ AronComponentConfigPlugin(ManagedIceObject& parent, const std::string& prefix) : ComponentPlugin(parent, prefix)
+ {
+ addPluginAndDependency(remote_gui_plugin_);
+ }
+
+ armarx::WriteBufferedTripleBuffer<AronStructT> config_;
+ private:
+ template <typename PluginT, typename... ParamsT>
+ void addPluginAndDependency(std::experimental::observer_ptr<PluginT>& plugin, ParamsT&& ...params)
+ {
+ PluginT* tmp{nullptr};
+ addPlugin(tmp, std::forward<ParamsT>(params)...);
+ addPluginDependency(tmp);
+ plugin.reset(tmp);
+ }
+
+
+ std::experimental::observer_ptr<RemoteGuiComponentPlugin> remote_gui_plugin_{nullptr};
+ };
+}
+
+namespace armarx
+{
+ template <typename AronStructT>
+ requires plugins::isAronGenerated<AronStructT>
+ class AronComponentConfigPluginUser : virtual public ManagedIceObject
+ {
+ public:
+
+ AronComponentConfigPluginUser()
+ {
+ addPlugin(aron_component_config_plugin_, "");
+ }
+
+ AronStructT& getConfigWriteBuffer()
+ {
+ return aron_component_config_plugin_->config_.getWriteBuffer();
+ }
+
+ void commitConfigWriteBuffer()
+ {
+ aron_component_config_plugin_->config_.commitWrite();
+ }
+
+ const AronStructT& getUpToDateConfigReadBuffer() const
+ {
+ return aron_component_config_plugin_->config_.getUpToDateReadBuffer();
+ }
+
+ private:
+ std::experimental::observer_ptr<plugins::AronComponentConfigPlugin<AronStructT>> aron_component_config_plugin_{
+ nullptr};
+
+ };
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d40feb193882995c57b0cde5896de52203e69403
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
@@ -0,0 +1,272 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PropertyDefinitionVisitors.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
+#include <ArmarXCore/core/application/properties/PropertyUser.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+#include "Util.h"
+
+#define INPUT_GUARD(i) \
+ARMARX_CHECK_NOT_NULL(i); \
+if (in_list_) return;
+
+namespace armarx::aron::component_config
+{
+
+ void PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& elementType)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ auto name = pathToName(i);
+ property_user_->getProperty(value->getValue(), name);
+ i = value;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& elementType)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ auto name = pathToName(f);
+ property_user_->getProperty(value->getValue(), name);
+ f = value;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& elementType)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ auto name = pathToName(d);
+ property_user_->getProperty(value->getValue(), name);
+ d = value;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& elementType)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ auto name = pathToName(b);
+ property_user_->getProperty(value->getValue(), name);
+ b = value;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& elementType)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ auto name = pathToName(string);
+ property_user_->getProperty(value->getValue(), name);
+ string = value;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ const auto name = pathToName(o);
+ auto type = type::List::DynamicCast(t);
+ auto old_list = data::List::DynamicCast(o);
+ old_list->clear();
+ auto get_list = [this, & name]() -> std::vector<std::string>
+ {
+ std::string list;
+ property_user_->getProperty(list, name);
+ return simox::alg::split(list, ",");
+ };
+ int i = 0;
+ switch (type->getAcceptedType()->getDescriptor())
+ {
+ case type::Descriptor::INT:
+ {
+ std::vector<std::string> vector = get_list();
+ std::for_each(vector.begin(), vector.end(), [&old_list, &i](const auto& el){
+ old_list->addElement(make_int(std::stoi(el), old_list->getPath().withIndex(i)));
+ i++;
+ });
+ break;
+ }
+ case type::Descriptor::STRING:
+ {
+ std::vector<std::string> vector = get_list();
+ std::for_each(vector.begin(), vector.end(), [&old_list, &i](const auto& el){
+ old_list->addElement(make_string(el, old_list->getPath().withIndex(i)));
+ i++;
+ });
+ break;
+ }
+ default:
+ return;
+ }
+ in_list_ = true;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& o, TypeInput& t)
+ {
+ in_list_ = false;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& elementType)
+ {
+ auto data = data::Int::DynamicCastAndCheck(enumData);
+ auto name = pathToName(enumData);
+ property_user_->getProperty(data->getValue(), name);
+ enumData = data;
+ }
+
+ void PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& elementType)
+ {
+ ARMARX_WARNING << "Unknown data encountered: " << (unknown ? unknown->getFullName() : "nullptr");
+ }
+
+ PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs) :
+ property_user_(std::experimental::make_observer(&defs)), in_list_(false)
+ {
+
+ }
+
+ PropertyDefinitionGetterVisitor::MapElements
+ PropertyDefinitionGetterVisitor::getObjectElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getObjectElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::MapElements
+ PropertyDefinitionGetterVisitor::getDictElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getDictElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::ListElements
+ PropertyDefinitionGetterVisitor::getListElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getListElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::PairElements
+ PropertyDefinitionGetterVisitor::getPairElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getPairElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::TupleElements
+ PropertyDefinitionGetterVisitor::getTupleElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getTupleElements(o, t);
+ }
+
+ type::Descriptor PropertyDefinitionGetterVisitor::getDescriptor(DataInput& o, TypeInput& t)
+ {
+ return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
+ }
+
+ PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(const PropertyDefinitionsPtr& defs) :
+ property_definitions_(std::experimental::make_observer(defs.get()))
+ {
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i, const type::IntPtr&)
+ {
+ INPUT_GUARD(i);
+ auto name = pathToName(i);
+ property_definitions_->defineOptionalProperty<int>(name, i->getValue());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::FloatPtr& f, const type::FloatPtr&)
+ {
+ INPUT_GUARD(f);
+ auto name = pathToName(f);
+ property_definitions_->defineOptionalProperty<float>(name, f->getValue());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::DoublePtr& d, const type::DoublePtr&)
+ {
+ INPUT_GUARD(d);
+ auto name = pathToName(d);
+ property_definitions_->defineOptionalProperty<double>(name, d->getValue());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b, const type::BoolPtr&)
+ {
+ INPUT_GUARD(b);
+ auto name = pathToName(b);
+ property_definitions_->defineOptionalProperty<bool>(name, b->getValue());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::StringPtr& string, const type::StringPtr&)
+ {
+ INPUT_GUARD(string);
+ auto name = pathToName(string);
+ property_definitions_->defineOptionalProperty<std::string>(name, string->getValue());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i, const type::IntEnumPtr& t)
+ {
+ INPUT_GUARD(i);
+ auto name = pathToName(i);
+ property_definitions_->defineOptionalProperty<int>(name, i->getValue()).map(t->getAcceptedValueMap());
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::ListPtr& l, const type::ListPtr& t)
+ {
+ ARMARX_CHECK_NOT_NULL(l);
+ in_list_ = true;
+ auto name = pathToName(l);
+ auto t_desc = t->getAcceptedType()->getDescriptor();
+ auto input = l->getElements();
+ std::string str;
+ switch (t_desc)
+ {
+ case type::Descriptor::INT:
+ {
+ std::vector<int> vector;
+ std::transform(input.begin(), input.end(), std::back_inserter(vector), [](const auto& el)
+ {
+ return data::Int::DynamicCast(el)->getValue();
+ });
+ str = simox::alg::to_string(vector, ", ");
+ break;
+ }
+ case type::Descriptor::STRING:
+ {
+ std::vector<std::string> vector;
+ std::transform(input.begin(), input.end(), std::back_inserter(vector), [](const auto& el)
+ {
+ return data::String::DynamicCast(el)->getValue();
+ });
+ str = simox::alg::to_string(vector, ", ");
+ break;
+ }
+ // TODO: other basic types
+ default:
+ return;
+ }
+ property_definitions_->defineOptionalProperty<std::string>(name, str);
+ }
+
+ void PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&)
+ {
+ in_list_ = false;
+ }
+}
+
+#undef INPUT_GUARD
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
new file mode 100644
index 0000000000000000000000000000000000000000..34386f5791844ae28ec8e6bf3b6cefac5fca7737
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
@@ -0,0 +1,99 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+namespace armarx::aron::component_config
+{
+
+ class PropertyDefinitionSetterVisitor : public aron::data::RecursiveConstTypedVariantVisitor
+ {
+ public:
+ explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs);
+
+ void visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) override;
+
+ void visitAronVariant(const data::IntPtr&, const type::IntPtr&) override;
+
+ void visitAronVariant(const data::FloatPtr&, const type::FloatPtr&) override;
+
+ void visitAronVariant(const data::DoublePtr&, const type::DoublePtr&) override;
+
+ void visitAronVariant(const data::BoolPtr&, const type::BoolPtr&) override;
+
+ void visitAronVariant(const data::StringPtr&, const type::StringPtr&) override;
+
+ void visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) override;
+
+ void visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) override;
+
+ private:
+ std::experimental::observer_ptr<PropertyDefinitionContainer> property_definitions_;
+ std::atomic<bool> in_list_{false};
+ };
+
+ class PropertyDefinitionGetterVisitor : public aron::data::RecursiveTypedVisitor<data::VariantPtr,
+ const type::VariantPtr>
+ {
+ public:
+ explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs);
+
+ type::Descriptor getDescriptor(DataInput& o, TypeInput& t) override;
+
+ MapElements getObjectElements(DataInput& o, TypeInput& t) override;
+
+ MapElements getDictElements(DataInput& o, TypeInput& t) override;
+
+ ListElements getListElements(DataInput& o, TypeInput& t) override;
+
+ PairElements getPairElements(DataInput& o, TypeInput& t) override;
+
+ TupleElements getTupleElements(DataInput& o, TypeInput& t) override;
+
+ void visitInt(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitFloat(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitDouble(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitBool(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitString(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitListOnEnter(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitListOnExit(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitIntEnum(DataInput& elementData, TypeInput& elementType) override;
+
+ private:
+ std::experimental::observer_ptr<const armarx::PropertyUser> property_user_;
+ std::atomic<bool> in_list_;
+ };
+
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGui.h b/source/RobotAPI/libraries/aron_component_config/RemoteGui.h
new file mode 100644
index 0000000000000000000000000000000000000000..e790688585294d69d7ff68e39ec54ad763b72046
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGui.h
@@ -0,0 +1,58 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include "RemoteGuiVisitors.h"
+
+namespace armarx::RemoteGui
+{
+ template <typename T> concept isAronGenerated = std::is_base_of<armarx::aron::cpp::AronGeneratedClass, T>::value;
+
+ template <typename AronStructT>
+ requires isAronGenerated<AronStructT>
+ detail::GroupBoxBuilder makeConfigGui(
+ const std::string& name,
+ const AronStructT& val)
+ {
+ aron::component_config::MakeConfigGuiVisitor vis(name);
+ armarx::aron::data::visitRecursive(vis, val.toAron(), val.ToAronType());
+ return vis.getGroupBoxBuilder();
+ }
+
+// template <typename AronStructT>
+// requires isAronGenerated<AronStructT>
+// void getValueFromMap(AronStructT& cfg,
+// RemoteGui::ValueMap const& values, std::string const& name)
+// {
+// armarx::aron::component_config::GetValueFromMapVisitor vis(&values);
+// auto data = std::static_pointer_cast<armarx::aron::data::Variant>(cfg.toAron());
+// auto type = cfg.ToAronType();
+// armarx::aron::data::visitRecursive(vis, data, type);
+// cfg.fromAron(std::static_pointer_cast<armarx::aron::data::Dict>(data));
+// }
+
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..555401357cab15f0a3079f8d2fa84e1fc2c6790c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
@@ -0,0 +1,313 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RemoteGuiVisitors.h"
+
+#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+#include "Util.h"
+
+#define INPUT_GUARD(i) \
+ARMARX_TRACE; \
+ARMARX_CHECK_NOT_NULL(i); \
+if (in_list_) return;
+
+namespace armarx::aron::component_config
+{
+
+ void MakeConfigGuiVisitor::visitObjectOnEnter(DataInput& dict, TypeInput&)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(dict);
+
+ if (dict->getPath().hasElement())
+ {
+ std::string name = pathToName(dict);
+ group_hierarchy_
+ .emplace_back(std::make_shared<RemoteGui::detail::GroupBoxBuilder>(RemoteGui::makeGroupBox(name)));
+ } else
+ {
+ group_hierarchy_.push_back(builder_);
+ }
+
+ }
+
+ void MakeConfigGuiVisitor::visitObjectOnExit(DataInput& dict, TypeInput&)
+ {
+ ARMARX_TRACE;
+ auto builder = *group_hierarchy_.back();
+ group_hierarchy_.pop_back();
+ if (!group_hierarchy_.empty())
+ {
+ group_hierarchy_.back()->addChild(builder);
+ }
+ }
+
+ void MakeConfigGuiVisitor::visitInt(DataInput& i, TypeInput&)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeIntSpinBox(pathToName(i)).value(value->getValue()).min(-1000).max(1000));
+ }
+
+ void MakeConfigGuiVisitor::visitFloat(DataInput& f, TypeInput&)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeFloatSpinBox(pathToName(f)).value(value->getValue()).min(-1000).max(1000));
+ }
+
+ void MakeConfigGuiVisitor::visitDouble(DataInput& d, TypeInput&)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeFloatSpinBox(pathToName(d)).value(value->getValue()).min(-1000).max(1000));
+ }
+
+ void MakeConfigGuiVisitor::visitBool(DataInput& b, TypeInput&)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeCheckBox(pathToName(b)).value(value->getValue()).label(b->getPath().getLastElement()));
+ }
+
+ void MakeConfigGuiVisitor::visitString(DataInput& string, TypeInput&)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ group_hierarchy_.back()->addChild(RemoteGui::makeLineEdit(pathToName(string)).value(value->getValue()));
+ }
+
+ MakeConfigGuiVisitor::MakeConfigGuiVisitor(const std::string& name) :
+ builder_(std::make_unique<RemoteGui::detail::GroupBoxBuilder>(name))
+ {
+ }
+
+ RemoteGui::detail::GroupBoxBuilder MakeConfigGuiVisitor::getGroupBoxBuilder() const
+ {
+ return *builder_;
+ }
+
+ void MakeConfigGuiVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ auto group = RemoteGui::makeSimpleGridLayout(pathToName(o) + "_grid").cols(20);
+ auto data = data::List::DynamicCastAndCheck(o);
+ auto type = type::List::DynamicCast(t);
+ for (const auto& el: data->getElements())
+ {
+ switch (type->getAcceptedType()->getDescriptor())
+ {
+ case type::Descriptor::STRING:
+ group.addChild(
+ RemoteGui::makeLineEdit(pathToName(el)).value(data::String::DynamicCast(el)->getValue()),
+ 10);
+ break;
+ case type::Descriptor::INT:
+ group.addChild(RemoteGui::makeLineEdit(pathToName(el))
+ .value(std::to_string(data::Int::DynamicCast(el)->getValue())), 10);
+ break;
+ default:
+ throw armarx::NotImplementedYetException();
+ }
+ group.addHSpacer(8);
+ group.addChild(RemoteGui::makeButton(pathToName(el) + "_button").label("-").toolTip("Remove List Element"),
+ 2);
+ }
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeGroupBox(pathToName(o) + "_grp").label(o->getPath().getLastElement()).collapsed(true)
+ .addChild(group).addChild(
+ RemoteGui::makeButton(pathToName(o) + "_add")
+ .label("+")
+ .toolTip("Add new list entry.")));
+ in_list_ = true;
+ }
+
+ void MakeConfigGuiVisitor::visitListOnExit(DataInput&, TypeInput&)
+ {
+ in_list_ = false;
+ }
+
+ void MakeConfigGuiVisitor::visitIntEnum(DataInput& o, TypeInput& t)
+ {
+ INPUT_GUARD(o);
+ auto data = data::Int::DynamicCastAndCheck(o);
+ auto type = type::IntEnum::DynamicCast(t);
+ group_hierarchy_.back()->addChild(RemoteGui::makeComboBox(pathToName(o)).options(type->getAcceptedValueNames())
+ .value(type->getValueName(
+ data->getValue())));
+ }
+
+
+ GetValueFromMapVisitor::GetValueFromMapVisitor(armarx::RemoteGui::TabProxy* proxy) :
+ proxy_(std::experimental::make_observer(proxy))
+ {
+ }
+
+ void GetValueFromMapVisitor::visitInt(DataInput& i, TypeInput&)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ proxy_->getValue(value->getValue(), pathToName(i));
+ i = value;
+ }
+
+ void GetValueFromMapVisitor::visitFloat(DataInput& f, TypeInput&)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ proxy_->getValue(value->getValue(), pathToName(f));
+ f = value;
+ }
+
+ void GetValueFromMapVisitor::visitDouble(DataInput& d, TypeInput&)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ float val_f;
+ proxy_->getValue(val_f, pathToName(d));
+ value->getValue() = val_f;
+ d = value;
+ }
+
+ void GetValueFromMapVisitor::visitBool(DataInput& b, TypeInput&)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ proxy_->getValue(value->getValue(), pathToName(b));
+ b = value;
+ }
+
+ void GetValueFromMapVisitor::visitString(DataInput& string, TypeInput&)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ proxy_->getValue(value->getValue(), pathToName(string));
+ string = value;
+ }
+
+ type::Descriptor GetValueFromMapVisitor::getDescriptor(DataInput& o, TypeInput& t)
+ {
+ return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
+ }
+
+ GetValueFromMapVisitor::MapElements GetValueFromMapVisitor::getObjectElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getObjectElements(o, t);
+ }
+
+ GetValueFromMapVisitor::MapElements GetValueFromMapVisitor::getDictElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getDictElements(o, t);
+ }
+
+ GetValueFromMapVisitor::ListElements GetValueFromMapVisitor::getListElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getListElements(o, t);
+ }
+
+ GetValueFromMapVisitor::PairElements GetValueFromMapVisitor::getPairElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getPairElements(o, t);
+ }
+
+ GetValueFromMapVisitor::TupleElements GetValueFromMapVisitor::getTupleElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getTupleElements(o, t);
+ }
+
+ void GetValueFromMapVisitor::visitIntEnum(DataInput& o, TypeInput& t)
+ {
+ INPUT_GUARD(o);
+ auto data = data::Int::DynamicCastAndCheck(o);
+ auto type = type::IntEnum::DynamicCastAndCheck(t);
+ std::string str;
+ proxy_->getValue(str, pathToName(o));
+ data->getValue() = type->getValue(str);
+ o = data;
+ }
+
+ void GetValueFromMapVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ auto data = data::List::DynamicCastAndCheck(o);
+ auto type = type::List::DynamicCast(t);
+ const auto& elements = data->getElements();
+ for (const auto& [idx, el]: armarx::MakeIndexedContainer(elements))
+ {
+ if (proxy_->getButtonClicked(pathToName(el) + "_button"))
+ {
+ data->removeElement(idx);
+ tab_rebuild_required_ = true;
+ }
+
+ switch (type->getAcceptedType()->getDescriptor())
+ {
+ case type::Descriptor::STRING:
+ {
+ proxy_->getValue(data::String::DynamicCast(el)->getValue(), pathToName(el));
+ break;
+ }
+ case type::Descriptor::INT:
+ {
+ std::string str;
+ proxy_->getValue(str, pathToName(el));
+ data::Int::DynamicCast(el)->getValue() = std::stoi(str);
+ break;
+ }
+ default:
+ throw armarx::NotImplementedYetException();
+ }
+ }
+ if (proxy_->getButtonClicked(pathToName(o) + "_add"))
+ {
+ switch (type->getAcceptedType()->getDescriptor())
+ {
+ case type::Descriptor::STRING:
+ data->addElement(make_string("", o->getPath().withIndex(data->childrenSize())));
+ break;
+ case type::Descriptor::INT:
+ data->addElement(make_int(0, o->getPath().withIndex(data->childrenSize())));
+ break;
+ default:
+ throw armarx::NotImplementedYetException();
+ }
+ tab_rebuild_required_ = true;
+ }
+ in_list_ = true;
+ }
+
+ void GetValueFromMapVisitor::visitListOnExit(DataInput&, TypeInput&)
+ {
+ in_list_ = false;
+ }
+
+ bool GetValueFromMapVisitor::tabRebuildRequired() const
+ {
+ return tab_rebuild_required_;
+ }
+}
+
+#undef INPUT_GUARD
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
new file mode 100644
index 0000000000000000000000000000000000000000..26a201fbd24b07073f7a172cc2a327624da89411
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
@@ -0,0 +1,120 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+namespace armarx::RemoteGui
+{
+ namespace detail
+ {
+ class GroupBoxBuilder;
+ }
+ class TabProxy;
+
+ class ValueVariant;
+
+ using ValueMap = std::map<std::string, ValueVariant>;
+}
+namespace armarx::aron::component_config
+{
+
+ class MakeConfigGuiVisitor : public data::RecursiveConstTypedVariantVisitor
+ {
+ public:
+ explicit MakeConfigGuiVisitor(const std::string& name);
+
+ void visitObjectOnEnter(DataInput&, TypeInput&) override;
+
+ void visitObjectOnExit(DataInput&, TypeInput&) override;
+
+ void visitListOnEnter(DataInput&, TypeInput&) override;
+
+ void visitListOnExit(DataInput&, TypeInput&) override;
+
+
+ void visitInt(DataInput&, TypeInput&) override;
+
+ void visitIntEnum(DataInput&, TypeInput&) override;
+
+ void visitFloat(DataInput&, TypeInput&) override;
+
+ void visitDouble(DataInput&, TypeInput&) override;
+
+ void visitBool(DataInput&, TypeInput&) override;
+
+ void visitString(DataInput&, TypeInput&) override;
+
+ [[nodiscard]] RemoteGui::detail::GroupBoxBuilder getGroupBoxBuilder() const;
+
+ private:
+ std::shared_ptr<RemoteGui::detail::GroupBoxBuilder> builder_;
+ std::vector<std::shared_ptr<RemoteGui::detail::GroupBoxBuilder>> group_hierarchy_;
+ std::atomic_bool in_list_{false};
+ };
+
+ class GetValueFromMapVisitor : public aron::data::RecursiveTypedVisitor<data::VariantPtr, const type::VariantPtr>
+ {
+ public:
+ explicit GetValueFromMapVisitor(armarx::RemoteGui::TabProxy* proxy);
+
+ type::Descriptor getDescriptor(DataInput&, TypeInput&) override;
+
+ MapElements getObjectElements(DataInput&, TypeInput&) override;
+
+ MapElements getDictElements(DataInput&, TypeInput&) override;
+
+ ListElements getListElements(DataInput&, TypeInput&) override;
+
+ PairElements getPairElements(DataInput&, TypeInput&) override;
+
+ TupleElements getTupleElements(DataInput&, TypeInput&) override;
+
+ void visitListOnEnter(DataInput&, TypeInput&) override;
+
+ void visitListOnExit(DataInput&, TypeInput&) override;
+
+ void visitInt(DataInput&, TypeInput&) override;
+
+ void visitIntEnum(DataInput&, TypeInput&) override;
+
+ void visitFloat(DataInput&, TypeInput&) override;
+
+ void visitDouble(DataInput&, TypeInput&) override;
+
+ void visitBool(DataInput&, TypeInput&) override;
+
+ void visitString(DataInput&, TypeInput&) override;
+
+ bool tabRebuildRequired() const;
+
+
+ private:
+ std::experimental::observer_ptr<armarx::RemoteGui::TabProxy> proxy_;
+ bool in_list_{false};
+ bool tab_rebuild_required_{false};
+ };
+
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/Util.h b/source/RobotAPI/libraries/aron_component_config/Util.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6c5561b7112714812896a4eec2269c1f767b597
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/Util.h
@@ -0,0 +1,151 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+#include <numeric>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+
+
+namespace armarx::aron::component_config
+{
+
+ inline std::string pathToName(const std::shared_ptr<data::Variant>& v)
+ {
+ auto vector = v->getPath().getPath();
+ auto path = v->getPath();
+ path.setDelimeter(".");
+ path.setRootIdentifier("");
+ auto string = path.toString();
+ return string.substr(1, string.size());
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::map<std::string,
+ std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>>
+ getObjectElements(DataInputT& o, TypeInputT& t)
+ {
+ std::map<std::string, std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Object::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ for (const auto& [key, e]: x->getElements())
+ {
+ auto ct = y->getMemberType(key);
+ ret.insert({key, {e, ct}});
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::map<std::string,
+ std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>>
+ getDictElements(DataInputT& o, TypeInputT& t)
+ {
+ std::map<std::string, std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>> ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Dict::DynamicCastAndCheck(t);
+
+ auto ac = y ? y->getAcceptedType() : nullptr;
+
+ if (x)
+ {
+ for (const auto& [key, e]: x->getElements())
+ {
+ ret.insert({key, {e, ac}});
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>>
+ getListElements(DataInputT& o, TypeInputT& t)
+ {
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>> ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::List::DynamicCastAndCheck(t);
+
+ auto ac = y ? y->getAcceptedType() : nullptr;
+
+ if (x)
+ {
+ for (const auto& e: x->getElements())
+ {
+ ret.emplace_back(e, ac);
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::pair<std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>,
+ std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>>
+ getPairElements(DataInputT& o, TypeInputT& t)
+ {
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Pair::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ auto e0 = x->getElement(0);
+ auto ac0 = y->getFirstAcceptedType();
+ auto e1 = x->getElement(1);
+ auto ac1 = y->getSecondAcceptedType();
+ return {{e0, ac0},
+ {e1, ac1}};
+ }
+ return {};
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type, typename std::remove_const<TypeInputT>::type>>
+ getTupleElements(DataInputT& o, TypeInputT& t)
+ {
+ std::vector<std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Tuple::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ unsigned int i = 0;
+ for (const auto& e: x->getElements())
+ {
+ auto ac = y->getAcceptedType(i++);
+ ret.emplace_back(e, ac);
+ }
+ }
+ return ret;
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 7368c8d633d766e4adb022e0f428dfe3cdce1ce4..30241deae7dc56e5ff773c473e895457cae38ba5 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -16,27 +16,42 @@ armarx_add_library(
./manager/SkillManagerComponentPlugin.cpp
./provider/SkillProviderComponentPlugin.cpp
./provider/Skill.cpp
+ ./provider/SkillProxy.cpp
./provider/PeriodicSkill.cpp
./provider/SpecializedSkill.cpp
./provider/PeriodicSpecializedSkill.cpp
./provider/SkillDescription.cpp
./provider/SkillStatusUpdate.cpp
./provider/SkillParameterization.cpp
- ./provider/helper/LambdaSkillImplementation.cpp
+ ./provider/LambdaSkill.cpp
+ ./provider/SkillContext.cpp
+ ./provider/SkillID.cpp
./provider/detail/SkillImplementationWrapper.cpp
- HEADERS
+ HEADERS
./error/Exception.h
./manager/SkillManagerComponentPlugin.h
./provider/SkillProviderComponentPlugin.h
./provider/Skill.h
+ ./provider/SkillProxy.h
./provider/PeriodicSkill.h
./provider/SpecializedSkill.h
./provider/PeriodicSpecializedSkill.h
./provider/SkillDescription.h
./provider/SkillStatusUpdate.h
./provider/SkillParameterization.h
- ./provider/helper/LambdaSkillImplementation.h
+ ./provider/LambdaSkill.h
+ ./provider/SkillContext.h
+ ./provider/SkillID.h
./provider/detail/SkillImplementationWrapper.h
+
+ provider/mixins/All.h
+ provider/mixins/ArvizSkillMixin.h
+ provider/mixins/MNSSkillMixin.h
+ provider/mixins/MemoryReadingSkillMixin.h
+ provider/mixins/RobotReadingSkillMixin.h
+ provider/mixins/ObjectReadingSkillMixin.h
+ provider/mixins/ObjectWritingSkillMixin.h
+ provider/mixins/GraspReadingSkillMixin.h
)
add_library(RobotAPI::skills ALIAS RobotAPISkills)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index b83ad843beef64e3112fb56d7523dfcdf6c56c34..23a708aceace48317d04884a8579fd21001b92c3 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,6 +1,7 @@
#include "SkillManagerComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
+#include "../error/Exception.h"
namespace armarx::plugins
{
@@ -50,23 +51,51 @@ namespace armarx
}
}
- void SkillManagerComponentPluginUser::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current&)
+ std::string SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
{
- if (auto it = skillProviderMap.find(info.providerName); it != skillProviderMap.end())
+ for (const auto& [providerName, providerPrx] : skillProviderMap)
+ {
+ auto allSkills = providerPrx->getSkillDescriptions();
+ for (const auto& [currentSkillName, skillDesc] : allSkills)
+ {
+ if (currentSkillName == skillName)
+ {
+ return providerName;
+ }
+ }
+ }
+ return "INVALID PROVIDER NAME";
+ }
+
+ skills::provider::dto::SkillStatusUpdate SkillManagerComponentPluginUser::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current&)
+ {
+ std::string providerName = "INVALID PROVIDER NAME";
+ if (info.skillId.providerName == "*")
+ {
+ providerName = getFirstProviderNameThatHasSkill(info.skillId.skillName);
+ }
+ else if(not(info.skillId.providerName.empty()))
+ {
+ providerName = info.skillId.providerName;
+ }
+
+ if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
getProxy(myPrx, -1);
skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillName;
+ exInfo.skillName = info.skillId.skillName;
+ exInfo.executorName = info.executorName;
exInfo.callbackInterface = myPrx;
exInfo.params = info.params;
- it->second->executeSkill(exInfo);
+ return it->second->executeSkill(exInfo);
}
else
{
- throw LocalException("Could not execute a skill of provider '" + info.providerName + "' because the provider does not exist.");
+ ARMARX_ERROR << "Could not execute a skill of provider '" + providerName + "' because the provider does not exist.";
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Skill execution failed. Could not execute a skill of provider '" + providerName + "' because the provider does not exist.");
}
}
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index e9c950c740d2b253edc33adb0ccd7d08971cfee8..7b14adf04d141900166968c773f498b6060623cb 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -34,13 +34,16 @@ namespace armarx
void addProvider(const skills::manager::dto::ProviderInfo& providerInfo, const Ice::Current ¤t) override;
void removeProvider(const std::string&, const Ice::Current ¤t) override;
- void executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
+ skills::provider::dto::SkillStatusUpdate executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t) override;
void abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t) override;
skills::manager::dto::SkillDescriptionMapMap getSkillDescriptions(const Ice::Current ¤t) override;
skills::manager::dto::SkillStatusUpdateMapMap getSkillExecutionStatuses(const Ice::Current ¤t) override;
+ protected:
+ std::string getFirstProviderNameThatHasSkill(const std::string& skillName);
+
private:
armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..280cdb93e5eb62413a159f232b20c5da0b7e3bd7
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
@@ -0,0 +1,15 @@
+#include "LambdaSkill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+
+ Skill::MainResult LambdaSkill::main(const MainInput& in)
+ {
+ TerminatedSkillStatus res = fun(in.executorName, in.params);
+
+ return {.status = res, .data = nullptr};
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
similarity index 52%
rename from source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
rename to source/RobotAPI/libraries/skills/provider/LambdaSkill.h
index 0f65572ac3c1191519c1f0d697ca1c767da56acb..01e3a3897e78b444c222b352556bea118f5bc50f 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
@@ -1,15 +1,15 @@
#pragma once
-#include "../Skill.h"
+#include "Skill.h"
namespace armarx
{
- namespace skills::helper
+ namespace skills
{
class LambdaSkill : public Skill
{
public:
- using FunT = std::function<bool(const aron::data::DictPtr&)>;
+ using FunT = std::function<TerminatedSkillStatus(const std::string clientId, const aron::data::DictPtr&)>;
LambdaSkill() = delete;
LambdaSkill(const FunT& f, const SkillDescription& desc) :
@@ -17,8 +17,8 @@ namespace armarx
fun(f)
{};
- protected:
- Skill::Status main(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
+ private:
+ MainResult main(const MainInput& in) override;
private:
FunT fun;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index d459b9af70c56925eaa53e86584215ad2d291c06..70daa959f0256646125c8477768f304db5555312 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -30,61 +30,62 @@
namespace armarx::skills
{
- PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
- const armarx::Frequency& frequency) :
+ PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency) :
Skill(skillDescription), frequency(frequency)
{
}
- Skill::Status
- PeriodicSkill::main(const aron::data::DictPtr& params, const CallbackT& callback)
+ PeriodicSkill::StepResult PeriodicSkill::stepOfSkill(const MainInput& in)
{
- if(not initialize(params))
- {
- onFailed();
- return Skill::Status::Failed;
- }
+ return this->step(in);
+ }
+ Skill::MainResult PeriodicSkill::main(const MainInput& in)
+ {
core::time::Metronome metronome(frequency);
- while (not stopped and not timeoutReached)
+ while (not Skill::checkWhetherSkillShouldStopASAP())
{
- const auto status = executeOnce(params);
- switch (status)
+ const auto res = stepOfSkill(in);
+ switch (res.status)
{
- case Status::Running:
+ case ActiveOrTerminatedSkillStatus::Running:
// nothing to do here
break;
- case Status::Succeeded:
- onSucceeded();
- return Skill::Status::Succeeded;
- case Status::Failed:
- onFailed();
- return Skill::Status::Failed;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return {TerminatedSkillStatus::Aborted, res.data};
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return {TerminatedSkillStatus::Succeeded, res.data};
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return {TerminatedSkillStatus::Failed, res.data};
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
if (stopped)
{
- onStopped();
- return Skill::Status::Stopped;
+ return {TerminatedSkillStatus::Aborted, nullptr};
}
if (timeoutReached)
{
- onTimeoutReached();
- return Skill::Status::TimeoutReached;
+ ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
+ return {TerminatedSkillStatus::Failed, nullptr};
}
- throw armarx::LocalException("should not happen.");
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
+ PeriodicSkill::StepResult PeriodicSkill::step(const MainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillName << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+
} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
index 6fb259f602db49c3640384793ec9790cef100fa2..5df6e07e9b7304b8e7c4db10141d3980f31a041c 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -29,29 +29,25 @@
namespace armarx::skills
{
- class PeriodicSkill : virtual public Skill
+ class PeriodicSkill : public Skill
{
public:
- PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
-
- protected:
- typename Skill::Status main(const aron::data::DictPtr& params,
- const Skill::CallbackT& callback) final;
-
- enum class Status
+ struct StepResult
{
- Running,
- Failed,
- Succeeded
+ ActiveOrTerminatedSkillStatus status;
+ aron::data::DictPtr data;
};
- virtual bool initialize(const aron::data::DictPtr& params) = 0;
- virtual Status executeOnce(const aron::data::DictPtr& params) = 0;
+ PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
+
+ StepResult stepOfSkill(const MainInput& in);
+
+ private:
+ /// Do not use anymore
+ Skill::MainResult main(const MainInput& in) final;
- virtual void onSucceeded() = 0;
- virtual void onStopped() = 0;
- virtual void onFailed() = 0;
- virtual void onTimeoutReached() = 0;
+ /// Override this method with your own step function
+ virtual StepResult step(const MainInput& in);
private:
const armarx::Frequency frequency;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 2b6fd6f6d352fe3edf94dc05a15ba3194703dfa3..4efac5d6d3ff33206063b928cd202e45476b0b63 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/time/Metronome.h>
#include "Skill.h"
+#include "PeriodicSkill.h"
#include "SpecializedSkill.h"
namespace armarx::skills
@@ -35,79 +36,75 @@ namespace armarx::skills
public:
using Base = SpecializedSkill<AronT>;
- PeriodicSpecializedSkill() = delete;
+ using Skill::description;
+ using Skill::getSkillId;
+
+ using StepResult = PeriodicSkill::StepResult;
- PeriodicSpecializedSkill(const SkillDescription& skillDescription,
- const armarx::Frequency& frequency) :
+ PeriodicSpecializedSkill() = delete;
+ PeriodicSpecializedSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency) :
Base(skillDescription), frequency(frequency)
{
}
- protected:
- typename Skill::Status
- main(const AronT& params, const Skill::CallbackT& callback) final
+
+ StepResult stepOfSkill(const typename Base::SpecializedMainInput& in)
{
- if (not initialize(params))
- {
- onFailed();
- return Skill::Status::Failed;
- }
+ return this->step(in);
+ }
+
+ private:
+ using Skill::stopped;
+ using Skill::timeoutReached;
+ /// Do not use anymore
+ Skill::MainResult main(const typename Base::SpecializedMainInput& in) final
+ {
core::time::Metronome metronome(frequency);
- while (not Skill::stopped and not Skill::timeoutReached)
+ while (not Skill::checkWhetherSkillShouldStopASAP())
{
- const auto status = executeOnce(params);
- switch (status)
+ const auto statusUpdate = stepOfSkill(in);
+ switch (statusUpdate.status)
{
- case Status::Running:
+ case ActiveOrTerminatedSkillStatus::Running:
// nothing to do here
break;
- case Status::Succeeded:
- onSucceeded();
- return Skill::Status::Succeeded;
- case Status::Failed:
- onFailed();
- return Skill::Status::Failed;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return {TerminatedSkillStatus::Aborted, statusUpdate.data};
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return {TerminatedSkillStatus::Succeeded, statusUpdate.data};
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return {TerminatedSkillStatus::Failed, statusUpdate.data};
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (Skill::stopped)
+ if (stopped)
{
- onStopped();
- return Skill::Status::Stopped;
+ return {TerminatedSkillStatus::Aborted, nullptr};
}
- if (Skill::timeoutReached)
+ if (timeoutReached)
{
- onTimeoutReached();
- return Skill::Status::TimeoutReached;
+ ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
+ return {TerminatedSkillStatus::Failed, nullptr};
}
- throw armarx::LocalException("should not happen.");
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
- enum class Status
+ /// Override this method with your own step function
+ virtual StepResult step(const typename Base::SpecializedMainInput& in)
{
- Running,
- Failed,
- Succeeded
- };
-
- virtual bool initialize(const AronT& params) = 0;
- virtual Status executeOnce(const AronT& params) = 0;
-
- virtual void onSucceeded() = 0;
- virtual void onStopped() = 0;
- virtual void onFailed() = 0;
- virtual void onTimeoutReached() = 0;
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillName << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
private:
const armarx::Frequency frequency;
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 23588f899674802a590633f1813a1c10c258feaf..21b32da1a75488fde1efea01b3b48e42b7fa77b9 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -12,101 +12,184 @@ namespace armarx
}
// install a local condition via a lambda
- void Skill::installCondition(std::function<bool()>&& f, std::function<void()>&& cb)
+ void Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
{
std::lock_guard l(callbacksMutex);
callbacks.push_back({f, cb});
}
- void Skill::notifyTimeoutReached()
+ bool Skill::isSkillAvailable(const InitInput &in) const
{
- timeoutReached = true;
+ return this->isAvailable(in);
}
- void Skill::notifyStopped()
+ bool Skill::isAvailable(const InitInput& in) const
{
- stopped = true;
+ (void) in;
+ return true;
}
- // reset all local variables
- void Skill::reset()
+ void Skill::resetSkill()
{
- started = IceUtil::Time::milliSeconds(-1);
- exited = IceUtil::Time::milliSeconds(-1);
+ //ARMARX_IMPORTANT << "Resetting skill '" << description.skillName << "'";
+
+ // resetting log times
+ started = armarx::core::time::DateTime::Invalid();
+ exited = armarx::core::time::DateTime::Invalid();
+ // resetting master running variable
running = false;
+
+ // resetting conditional variables
stopped = false;
timeoutReached = false;
+
+ this->reset();
}
- // always called before execute (should not take longer than 100ms)
- void Skill::init(const aron::data::DictPtr& params)
+ void Skill::waitForDependenciesOfSkill()
{
- (void) params;
-
- ARMARX_IMPORTANT << "Initializing Skill '" << description.skillName << "'";
+ //ARMARX_IMPORTANT << "Waiting for dependencies of skill '" << description.skillName << "'";
+ this->waitForDependencies();
+ }
+ void Skill::_init()
+ {
+ //ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
callbacks.clear();
running = true;
- started = IceUtil::Time::now();
+ started = armarx::core::time::DateTime::Now();
// install timeout condition
- installCondition(
- [&](){ return (IceUtil::Time::now().toMilliSeconds() - started.toMilliSeconds()) >= description.timeoutMs; },
+ installConditionWithCallback(
+ [&](){ return (armarx::core::time::DateTime::Now() >= (started + description.timeout)); },
[&](){ notifyTimeoutReached(); }
);
conditionCheckingThread = std::thread([&]()
{
- while (running)
+ armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
+ while (running) // when the skill ends/aborts this variable will be set to false
{
- std::lock_guard l(callbacksMutex);
- for (auto& p : callbacks)
{
- auto& f = p.first;
- auto& cb = p.second;
- if (f()) cb();
+ std::scoped_lock l(callbacksMutex);
+ for (auto& p : callbacks)
+ {
+ auto& f = p.first;
+ auto& cb = p.second;
+ if (f())
+ {
+ cb();
+ }
+ }
+ }
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_WARNING << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " vs " << conditionCheckingThreadFrequency.toCycleDuration() << ")";
}
- std::this_thread::sleep_for(std::chrono::milliseconds(50)); // check frequency
}
});
}
+ Skill::InitResult Skill::initSkill(const InitInput& in)
+ {
+ this->_init();
+ return this->init(in);
+ }
- // always called after execute (should not take longer than 100ms)
- void Skill::exit(const aron::data::DictPtr& params)
+ void Skill::_main()
{
- (void) params;
+ // Nothing here yet...
+ }
+ Skill::MainResult Skill::mainOfSkill(const MainInput& in)
+ {
+ this->_main();
+ return this->main(in);
+ }
- ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
+ void Skill::_exit()
+ {
+ // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
+ running = false; // stop checking conditions
- running = false; // notify all conditions to stop
if (conditionCheckingThread.joinable())
{
conditionCheckingThread.join();
}
- exited = IceUtil::Time::now();
+ exited = armarx::core::time::DateTime::Now();
+ }
+ Skill::ExitResult Skill::exitSkill(const ExitInput& in)
+ {
+ auto ret = this->exit(in);
+ this->_exit();
+ return ret;
}
- Skill::Status Skill::main(const aron::data::DictPtr& params, const CallbackT& callback)
+ void Skill::notifyTimeoutReached()
{
- (void) params;
+ timeoutReached = true;
+ onTimeoutReached();
+ }
- ARMARX_IMPORTANT << "Executing Skill '" << description.skillName << "'";
- return Status::Succeeded;
+ void Skill::notifySkillToStopASAP()
+ {
+ stopped = true;
+ onStopRequested();
}
- Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ bool Skill::checkWhetherSkillShouldStopASAP() const
+ {
+ return stopped || timeoutReached;
+ }
+
+ // condition effects
+ void Skill::onTimeoutReached()
+ {
+ }
+ void Skill::onStopRequested()
{
- this->reset();
- this->init(params);
- auto ret = this->main(params);
- this->exit(params);
- return ret;
}
- bool Skill::stopRequested() const
+ // reset all local variables
+ void Skill::reset()
{
- return (stopped || timeoutReached);
+ // Default nothing to reset
+ }
+
+ // Wait if needed
+ void Skill::waitForDependencies()
+ {
+ // Default wait for nothing
+ }
+
+ // always called before execute (should not take longer than 100ms)
+ Skill::InitResult Skill::init(const InitInput&)
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ // always called after execute or if skill fails (should not take longer than 100ms)
+ Skill::ExitResult Skill::exit(const ExitInput&)
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult Skill::main(const MainInput& in)
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+
+ Skill::MainResult Skill::executeFullSkill(const MainInput& in)
+ {
+ this->resetSkill();
+ this->initSkill(InitInput{.executorName = in.executorName, .params = in.params});
+ auto ret = this->mainOfSkill(in);
+ this->exitSkill(ExitInput{.executorName = in.executorName, .params = in.params});
+ return ret;
}
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 1447fdd13ff440c9f5cfe9357a4f2889170502fb..0522191bfb57d272785380502217f23006a9e254 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -1,19 +1,25 @@
#pragma once
+// std/stl
#include <mutex>
#include <queue>
#include <thread>
#include <functional>
-#include <ArmarXCore/core/ComponentPlugin.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
+// base class
+#include <ArmarXCore/core/logging/Logging.h>
+// ArmarX
#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include "../error/Exception.h"
+
+#include "SkillID.h"
+#include "SkillStatusUpdate.h"
#include "SkillDescription.h"
namespace armarx
@@ -25,65 +31,144 @@ namespace armarx
public:
using CallbackT = std::function<void(const aron::data::DictPtr&)>;
- enum class Status
+ struct InitResult
+ {
+ TerminatedSkillStatus status;
+ };
+
+ struct InitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr params;
+ };
+
+ struct MainInput
+ {
+ std::string executorName;
+ aron::data::DictPtr params;
+ CallbackT callback;
+ };
+
+ struct MainResult
+ {
+ TerminatedSkillStatus status;
+ aron::data::DictPtr data = nullptr;
+ };
+
+ struct ExitResult
+ {
+ TerminatedSkillStatus status;
+ };
+
+ struct ExitInput
{
- Succeeded,
- TimeoutReached,
- Stopped,
- Failed
+ std::string executorName;
+ aron::data::DictPtr params;
};
Skill() = delete;
Skill(const SkillDescription&);
virtual ~Skill() = default;
- /// Override this method with the actual implementation.
- virtual void init(const aron::data::DictPtr& params);
+ /// The id of the skill (combination of provider and name must be unique).
+ SkillID getSkillId() const
+ {
+ return {providerName, description.skillName};
+ }
- /// Override this method with the actual implementation.
- virtual void exit(const aron::data::DictPtr& params);
+ // Non virtual base methods. They internally call the virtual methods
+ // Lifecycle of a skill:
+ // 1. check if it is available
+ bool isSkillAvailable(const InitInput& in) const;
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status main(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ // 2. reset skill
+ void resetSkill();
+
+ // 3. Set skill scheduled. Wait for dependencies
+ void waitForDependenciesOfSkill();
+
+ // 4. All dependencies resolved. Init skill
+ InitResult initSkill(const InitInput& in);
+
+ // 5. Execute main function of skill
+ MainResult mainOfSkill(const MainInput& in);
+
+ // 6. Exit skill. This method is called in any case once the skill is scheduled.
+ ExitResult exitSkill(const ExitInput& in);
- /// Do all methods at once.
- Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ // Condition listeners
+ // used to notify the skill from extern to stop
+ void notifySkillToStopASAP();
+
+ // returns whether the skill should terminate as soon as possible
+ bool checkWhetherSkillShouldStopASAP() const;
+
+ /// Do init, main, exit together
+ MainResult executeFullSkill(const MainInput& in);
+
+ protected:
+ // fires if the skill reaches timeout
+ void notifyTimeoutReached();
+
+ // helper methods to do all the static initialization stuff
+ void _init();
+ void _main();
+ void _exit();
+
+ private:
+ /// Override if your skill can be unavailable. It receives the same input as the init method
+ virtual bool isAvailable(const InitInput& in) const;
/// Override if you have special members that needs to be resetted. It is called before the skill ititializes
virtual void reset();
- /// Override these methods if you want to do something special when notification comes
- virtual void notifyTimeoutReached();
- virtual void notifyStopped();
+ /// Override if you have special dependencies you have to wait for
+ virtual void waitForDependencies();
+
+ /// Override this method with the actual implementation.
+ virtual InitResult init(const InitInput& in);
- virtual bool stopRequested() const;
+ /// Override this method with the actual implementation. The callback is for status updates to the calling instance
+ virtual MainResult main(const MainInput& in);
- private:
- /// install a condition is easy
- void installCondition(std::function<bool()>&& f, std::function<void()>&& cb);
+ /// Override this method with the actual implementation.
+ virtual ExitResult exit(const ExitInput& in);
+
+ /// Override these methods if you want to do something special when notification comes
+ virtual void onTimeoutReached();
+ virtual void onStopRequested();
+
+ protected:
+ /// install a condition which is frequently checked from the conditionCheckingThread
+ void installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb);
public:
+ /// The descripion of the skill, which will be available via the provider/manager
const SkillDescription description;
- /// active conditions
- std::vector<std::pair<std::function<bool()>, std::function<void()>>> callbacks;
- mutable std::mutex callbacksMutex;
-
- /// running params. TODO switch to armarx::core::time
- IceUtil::Time started = IceUtil::Time::milliSeconds(-1);
- IceUtil::Time exited = IceUtil::Time::milliSeconds(-1);
+ /// running params
+ armarx::core::time::DateTime started = armarx::core::time::DateTime::Invalid();
+ armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
- /// proxies that called the skills. Will be set from provider and is const afterwards
+ /// proxy that called the skills. Will be set from provider and is const afterwards
manager::dti::SkillManagerInterfacePrx manager = nullptr;
+ /// the provider that owns this skill. Will be set from provider and is const afterwards
+ std::string providerName = "INVALID PROVIDER NAME";
+
protected:
+ /// active conditions. First is condition (bool return func)
+ std::vector<std::pair<std::function<bool()>, std::function<void()>>> callbacks;
+ mutable std::mutex callbacksMutex;
+
/// Use conditions this way
- std::atomic_bool running = true;
+ std::atomic_bool running = false;
std::atomic_bool stopped = false;
std::atomic_bool timeoutReached = false;
private:
- std::thread conditionCheckingThread;
+ std::thread conditionCheckingThread; // A therad that checks the conditions frequently
+ armarx::Frequency conditionCheckingThreadFrequency = armarx::Frequency::Hertz(20);
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillContext.cpp b/source/RobotAPI/libraries/skills/provider/SkillContext.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3af16ef1200a4e3b8ef0865f9522cf10f081e27c
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillContext.cpp
@@ -0,0 +1,6 @@
+#include "SkillContext.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillContext.h b/source/RobotAPI/libraries/skills/provider/SkillContext.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bdbc0f0260729fb213941ef64c589f1b7595a05
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillContext.h
@@ -0,0 +1,26 @@
+#pragma once
+
+// ArmarX
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ /* A base class for skill contexts. It is not required to use a context for skills but it eases the management of dependencies and properties for providers */
+ class SkillContext
+ {
+ public:
+ SkillContext() = default;
+
+ virtual void defineProperties(const armarx::PropertyDefinitionsPtr& defs, const std::string& prefix) {};
+
+ virtual void onInit(armarx::Component& parent) {};
+ virtual void onConnected(armarx::Component& parent) {};
+ virtual void onDisconnected(armarx::Component& parent) {};
+ virtual void onStopped(armarx::Component& parent) {};
+
+ private:
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
index 86c000842499ba0cdc25914398607e9738b48b54..4e195f39e3953d47c98371023ecb90d4c95f9fab 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
@@ -11,7 +11,7 @@ namespace armarx
ret.description = description;
ret.skillName = skillName;
ret.robots = robots;
- ret.timeoutMs = timeoutMs;
+ ret.timeoutMs = timeout.toMilliSeconds();
ret.defaultParams = aron::data::Dict::ToAronDictDTO(defaultParams);
return ret;
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.h b/source/RobotAPI/libraries/skills/provider/SkillDescription.h
index 688ad41eb132b02bcf2ccddbda893774f591880b..57c3c93b18230c81b4152d9d915e4d2337beb294 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillDescription.h
@@ -3,6 +3,8 @@
#include <string>
#include <vector>
+#include <ArmarXCore/core/time/Duration.h>
+
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
@@ -13,10 +15,10 @@ namespace armarx
{
struct SkillDescription
{
- std::string skillName = "";
- std::string description = "";
+ std::string skillName = "NOT INITIALIZED YET";
+ std::string description = "NOT INITIALIZED YET";
std::vector<std::string> robots = {};
- long timeoutMs = -1;
+ armarx::core::time::Duration timeout = armarx::core::time::Duration::MilliSeconds(-1);
aron::type::ObjectPtr acceptedType = nullptr;
aron::data::DictPtr defaultParams = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.cpp b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4619f717f4eeeedc4f7e61dc8ca0698241966b00
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
@@ -0,0 +1,40 @@
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ SkillID::SkillID(const std::string& providerName, const std::string& skillName) : providerName(providerName), skillName(skillName)
+ {
+ if (simox::alg::contains(providerName, NAME_SEPARATOR) || simox::alg::contains(skillName, NAME_SEPARATOR))
+ {
+ throw error::SkillException(__PRETTY_FUNCTION__, std::string("A skill provider or a skill contains the blacklisted token '") + NAME_SEPARATOR + "'.");
+ }
+
+ if (simox::alg::contains(providerName, PREFIX_SEPARATOR) || simox::alg::contains(skillName, PREFIX_SEPARATOR))
+ {
+ throw error::SkillException(__PRETTY_FUNCTION__, std::string("A skill provider or a skill contains the blacklisted token '") + PREFIX_SEPARATOR + "'.");
+ }
+ }
+
+ bool SkillID::operator==(const SkillID& other) const
+ {
+ return toString() == other.toString();
+ }
+
+ bool SkillID::operator<(const SkillID& other) const
+ {
+ return toString() < other.toString();
+ }
+
+ provider::dto::SkillID SkillID::toIce() const
+ {
+ return {providerName, skillName};
+ }
+
+ std::string SkillID::toString(const std::string& prefix) const
+ {
+ return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) + providerName + NAME_SEPARATOR + skillName;
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.h b/source/RobotAPI/libraries/skills/provider/SkillID.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c44ba82db38ce760a333adfa8518b6e86b62159
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillID.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include "../error/Exception.h"
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillID
+ {
+ public:
+ static const constexpr char* PREFIX_SEPARATOR = "->";
+ static const constexpr char* NAME_SEPARATOR = "/";
+
+ std::string providerName;
+ std::string skillName;
+
+ SkillID() = delete;
+ SkillID(const std::string& providerName, const std::string& skillName);
+
+ bool operator==(const SkillID& other) const;
+ bool operator<(const SkillID& other) const;
+
+ provider::dto::SkillID toIce() const;
+ std::string toString(const std::string& prefix = "") const;
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
index 2dc791fe3e77495717c0b6fd9f44da19f1ae41f1..f977b2ac595fd3506856d2ba42468cdef72d2e54 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
@@ -4,6 +4,9 @@ namespace armarx
{
namespace skills
{
-
+ aron::data::dto::DictPtr SkillParameterization::toIce() const
+ {
+ return aron::data::Dict::ToAronDictDTO(usedInputParams);
+ }
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
index a6ce6ed4a95c0c090daa698f3cde8c2ebe30af39..72ec6d2eb11eec1954e753021d6e634a641027fd 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
@@ -14,6 +14,8 @@ namespace armarx
{
aron::data::DictPtr usedInputParams = nullptr;
callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface = nullptr;
+
+ aron::data::dto::DictPtr toIce() const;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index 0691a6f71d8a28c1ddce4c74faf54fec213711dd..a53e04a2a92d5062dcb8785ca94301c44fc7865c 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -20,18 +20,18 @@ namespace armarx::plugins
void SkillProviderComponentPlugin::postOnConnectComponent()
{
auto& p = parent<SkillProviderComponentPluginUser>();
- std::string providerName = p.getName();
+ const std::string providerName = p.getName();
// update skill ownership
for (auto& [skillName, impl] : p.skillImplementations)
{
impl.skill->manager = manager;
+ impl.skill->providerName = providerName;
- impl.statusUpdate.providerName = p.getName();
- impl.statusUpdate.skillName = skillName;
+ impl.statusUpdate.skillId = {providerName, skillName};
}
- // register to manager
+ // register self to manager
skills::manager::dto::ProviderInfo i;
i.provider = myPrx;
i.providerName = providerName;
@@ -72,7 +72,7 @@ namespace armarx
}
std::string skillName = skill->description.skillName;
- if (connected)
+ if (connected) // TODO: fix so that skills can be added anytime!!!
{
ARMARX_WARNING << "The SkillProvider already registered to a manager. The skill '" + skillName + "' therefore cannot be added anymore. Please only add skills in the onInit method.";
return;
@@ -86,13 +86,14 @@ namespace armarx
return;
}
- ARMARX_INFO << "Adding skill " << skillName;
- skillImplementations.emplace(skillName, std::move(skill));
+ ARMARX_INFO << "Adding skill and set owning provider name" << skillName;
+ auto s = skillImplementations.emplace(skillName, std::move(skill));
+ s.first->second.statusUpdate.skillId = skills::SkillID(getName(), skillName);
}
- void SkillProviderComponentPluginUser::addSkill(const skills::helper::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
+ void SkillProviderComponentPluginUser::addSkill(const skills::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
{
- auto lambda = std::make_unique<skills::helper::LambdaSkill>(f, desc);
+ auto lambda = std::make_unique<skills::LambdaSkill>(f, desc);
addSkill(std::move(lambda));
}
@@ -136,7 +137,7 @@ namespace armarx
}
// Please not that this method waits until the skill can be scheduled!
- void SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionRequest& info, const Ice::Current &)
+ skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionRequest& info, const Ice::Current &)
{
// The skill will be executed in a different thread
std::thread execution;
@@ -146,25 +147,28 @@ namespace armarx
usedParameterization.usedCallbackInterface = info.callbackInterface;
usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+ skills::provider::dto::SkillStatusUpdate ret;
{
std::shared_lock l(skillsMutex);
std::string skillName = info.skillName;
- ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
+ ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0) << "\nThe names are: " << VAROUT(skillName) << ", " << VAROUT(getName());
// get reference of the wrapper
auto& wrapper = skillImplementations.at(skillName);
// async start execution. But we wait for the execution to finish at the end of this method
- execution = std::thread([&wrapper, &usedParameterization](){
+ execution = std::thread([&ret, &wrapper, &info, &usedParameterization](){
// execute waits until the previous execution finishes.
- wrapper.execute(usedParameterization);
+ auto x = wrapper.setupAndExecuteSkill(info.executorName, usedParameterization);
+ ret = x.toIce();
});
- } // release lock. We don't know how long the skill needs to finish and we have to releasethe lock for being able to abort the execution
+ } // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
if (execution.joinable())
{
execution.join();
}
+ return ret;
}
void SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current &)
@@ -173,6 +177,6 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
auto& wrapper = skillImplementations.at(skillName);
- wrapper.skill->notifyStopped();
+ wrapper.skill->notifySkillToStopASAP();
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index a9696009516ad1f052058035da73a5c02b8e2dd5..c018ecd9a2610c823241f3b482d799598e37aa8b 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -13,7 +13,14 @@
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "helper/LambdaSkillImplementation.h"
+// Include all types of skills
+#include "Skill.h"
+#include "SpecializedSkill.h"
+#include "LambdaSkill.h"
+#include "PeriodicSkill.h"
+#include "PeriodicSpecializedSkill.h"
+
+// Helper wrapper for execution
#include "detail/SkillImplementationWrapper.h"
namespace armarx::plugins
@@ -52,11 +59,11 @@ namespace armarx
skills::provider::dto::SkillStatusUpdate getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t = Ice::Current()) override;
skills::provider::dto::SkillStatusUpdateMap getSkillExecutionStatuses(const Ice::Current ¤t = Ice::Current()) override;
- void executeSkill(const skills::provider::dto::SkillExecutionRequest& executionInfo, const Ice::Current ¤t = Ice::Current()) override;
+ skills::provider::dto::SkillStatusUpdate executeSkill(const skills::provider::dto::SkillExecutionRequest& executionInfo, const Ice::Current ¤t = Ice::Current()) override;
void abortSkill(const std::string& name, const Ice::Current ¤t = Ice::Current()) override;
protected:
- void addSkill(const skills::helper::LambdaSkill::FunT&, const skills::SkillDescription&);
+ void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
private:
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0ebdb3b653a1be1d59d9b945ac6b35c58962595
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
@@ -0,0 +1,47 @@
+#include "SkillProxy.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const SkillID& skillId) :
+ manager(manager),
+ skillId(skillId)
+ {
+ }
+
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const std::string& skillName) :
+ manager(manager),
+ skillId(skillProviderName, skillName)
+ {
+ }
+
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const SkillDescription& skillDesc) :
+ manager(manager),
+ skillId(skillProviderName, skillDesc.skillName)
+ {
+ }
+
+ TerminatedSkillStatusUpdate SkillProxy::executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params)
+ {
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.params = params->toAronDictDTO();
+ req.skillId = skillId.toIce();
+
+ auto terminatingUpdate = manager->executeSkill(req);
+ return TerminatedSkillStatusUpdate::FromIce(terminatingUpdate);
+ }
+
+ IceInternal::Handle<Ice::AsyncResult> SkillProxy::begin_executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params)
+ {
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.params = params->toAronDictDTO();
+ req.skillId = skillId.toIce();
+
+ auto future = manager->begin_executeSkill(req);
+ return future;
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e8ddc5c74ec5e3d36423984d67807ae12da0680
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "Skill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ /* Manages the remote execution of a skill and converts the ice types */
+ class SkillProxy : public armarx::Logging
+ {
+ public:
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const SkillID& skillId);
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const std::string& skillName);
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const SkillDescription& skillDesc);
+
+ TerminatedSkillStatusUpdate executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
+ IceInternal::Handle<Ice::AsyncResult> begin_executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
+
+ private:
+ const manager::dti::SkillManagerInterfacePrx& manager;
+
+ const SkillID skillId;
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
index fa9695b624acd0440413d7c756536ba7c9c66d86..18c4b8647bf3390a64343546b278155ab6a9ce30 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
@@ -4,15 +4,241 @@ namespace armarx
{
namespace skills
{
+ SkillStatus toSkillStatus(const ActiveOrTerminatedSkillStatus& d)
+ {
+ switch (d)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ return SkillStatus::Running;
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return SkillStatus::Failed;
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return SkillStatus::Succeeded;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return SkillStatus::Aborted;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ SkillStatus toSkillStatus(const TerminatedSkillStatus& d)
+ {
+ switch (d)
+ {
+ case TerminatedSkillStatus::Failed:
+ return SkillStatus::Failed;
+ case TerminatedSkillStatus::Succeeded:
+ return SkillStatus::Succeeded;
+ case TerminatedSkillStatus::Aborted:
+ return SkillStatus::Aborted;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void toIce(provider::dto::Execution::Status& ret, const SkillStatus& status)
+ {
+ switch(status)
+ {
+ case SkillStatus::Idle:
+ ret = provider::dto::Execution::Status::Idle;
+ return;
+ case SkillStatus::Scheduled:
+ ret = provider::dto::Execution::Status::Scheduled;
+ return;
+ case SkillStatus::Running:
+ ret = provider::dto::Execution::Status::Running;
+ return;
+ case SkillStatus::Failed:
+ ret = provider::dto::Execution::Status::Failed;
+ return;
+ case SkillStatus::Succeeded:
+ ret = provider::dto::Execution::Status::Succeeded;
+ return;
+ case SkillStatus::Aborted:
+ ret = provider::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void toIce(provider::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status)
+ {
+ switch(status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ ret = provider::dto::Execution::Status::Running;
+ return;
+ case ActiveOrTerminatedSkillStatus::Failed:
+ ret = provider::dto::Execution::Status::Failed;
+ return;
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ ret = provider::dto::Execution::Status::Succeeded;
+ return;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ ret = provider::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void toIce(provider::dto::Execution::Status& ret, const TerminatedSkillStatus& status)
+ {
+ switch(status)
+ {
+ case TerminatedSkillStatus::Failed:
+ ret = provider::dto::Execution::Status::Failed;
+ return;
+ case TerminatedSkillStatus::Succeeded:
+ ret = provider::dto::Execution::Status::Succeeded;
+ return;
+ case TerminatedSkillStatus::Aborted:
+ ret = provider::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void fromIce(const provider::dto::Execution::Status& status, TerminatedSkillStatus& ret)
+ {
+ switch(status)
+ {
+ case provider::dto::Execution::Status::Idle:
+ [[fallthrough]];
+ case provider::dto::Execution::Status::Scheduled:
+ [[fallthrough]];
+ case provider::dto::Execution::Status::Running:
+ break;
+ case provider::dto::Execution::Status::Failed:
+ ret = TerminatedSkillStatus::Failed;
+ return;
+ case provider::dto::Execution::Status::Succeeded:
+ ret = TerminatedSkillStatus::Succeeded;
+ return;
+ case provider::dto::Execution::Status::Aborted:
+ ret = TerminatedSkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "You entered an invalid execution status type to convert to a terminating status.");
+ }
+
+ void fromIce(const provider::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret)
+ {
+ switch(status)
+ {
+ case provider::dto::Execution::Status::Idle:
+ [[fallthrough]];
+ case provider::dto::Execution::Status::Scheduled:
+ break;
+ case provider::dto::Execution::Status::Running:
+ ret = ActiveOrTerminatedSkillStatus::Running;
+ return;
+ case provider::dto::Execution::Status::Failed:
+ ret = ActiveOrTerminatedSkillStatus::Failed;
+ return;
+ case provider::dto::Execution::Status::Succeeded:
+ ret = ActiveOrTerminatedSkillStatus::Succeeded;
+ return;
+ case provider::dto::Execution::Status::Aborted:
+ ret = ActiveOrTerminatedSkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "You entered an invalid execution status type to convert to a terminating status.");
+ }
+
+ void fromIce(const provider::dto::Execution::Status& status, SkillStatus& ret)
+ {
+ switch(status)
+ {
+ case provider::dto::Execution::Status::Idle:
+ ret = SkillStatus::Idle;
+ return;
+ case provider::dto::Execution::Status::Scheduled:
+ ret = SkillStatus::Scheduled;
+ return;
+ case provider::dto::Execution::Status::Running:
+ ret = SkillStatus::Running;
+ return;
+ case provider::dto::Execution::Status::Failed:
+ ret = SkillStatus::Failed;
+ return;
+ case provider::dto::Execution::Status::Succeeded:
+ ret = SkillStatus::Succeeded;
+ return;
+ case provider::dto::Execution::Status::Aborted:
+ ret = SkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+
+
+
+ provider::dto::SkillStatusUpdate TerminatedSkillStatusUpdate::toIce() const
+ {
+ provider::dto::SkillStatusUpdate ret;
+ ret.header.skillId = skillId.toIce();
+ ret.header.executorName = executorName;
+ ret.data = aron::data::Dict::ToAronDictDTO(data);
+ skills::toIce(ret.header.status, status);
+ ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
+ ret.header.usedParams = usedParameterization.toIce();
+ return ret;
+ }
+
provider::dto::SkillStatusUpdate SkillStatusUpdate::toIce() const
{
provider::dto::SkillStatusUpdate ret;
- ret.header.providerName = providerName;
- ret.header.skillName = skillName;
+ ret.header.skillId = skillId.toIce();
+ ret.header.executorName = executorName;
+ ret.data = aron::data::Dict::ToAronDictDTO(data);
+ skills::toIce(ret.header.status, status);
+ ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
+ ret.header.usedParams = usedParameterization.toIce();
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate ActiveOrTerminatedSkillStatusUpdate::toIce() const
+ {
+ provider::dto::SkillStatusUpdate ret;
+ ret.header.skillId = skillId.toIce();
+ ret.header.executorName = executorName;
ret.data = aron::data::Dict::ToAronDictDTO(data);
- ret.header.status = status;
+ skills::toIce(ret.header.status, status);
ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
- ret.header.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.usedInputParams);
+ ret.header.usedParams = usedParameterization.toIce();
+ return ret;
+ }
+
+ TerminatedSkillStatusUpdate TerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
+ {
+ TerminatedSkillStatusUpdate ret;
+ ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
+ ret.executorName = update.header.executorName;
+ skills::fromIce(update.header.status, ret.status);
+ ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
+ ret.data = aron::data::Dict::FromAronDictDTO(update.data);
+ return ret;
+ }
+
+ SkillStatusUpdate SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
+ {
+ SkillStatusUpdate ret;
+ ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
+ ret.executorName = update.header.executorName;
+ skills::fromIce(update.header.status, ret.status);
+ ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
+ ret.data = aron::data::Dict::FromAronDictDTO(update.data);
+ return ret;
+ }
+
+ ActiveOrTerminatedSkillStatusUpdate ActiveOrTerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
+ {
+ ActiveOrTerminatedSkillStatusUpdate ret;
+ ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
+ ret.executorName = update.header.executorName;
+ skills::fromIce(update.header.status, ret.status);
+ ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
+ ret.data = aron::data::Dict::FromAronDictDTO(update.data);
return ret;
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
index 41a138832bd530d89de2d393335c0e5fbedbc21b..6e1ec3c169c01d337ddb94f337414f2443d11ff8 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
@@ -6,24 +6,85 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include "SkillID.h"
#include "SkillParameterization.h"
namespace armarx
{
namespace skills
{
- struct SkillStatusUpdate
+ enum class SkillStatus
+ {
+ Idle = 0,
+ Scheduled = 1,
+ Running = 2,
+ Failed = 4,
+ Succeeded = 8,
+ Aborted = 16
+ };
+
+ enum class ActiveOrTerminatedSkillStatus
+ {
+ Running = 2,
+ Failed = 4,
+ Succeeded = 8,
+ Aborted = 16
+ };
+
+ enum class TerminatedSkillStatus
+ {
+ Failed = 4,
+ Succeeded = 8,
+ Aborted = 16
+ };
+
+ SkillStatus toSkillStatus(const ActiveOrTerminatedSkillStatus&);
+ SkillStatus toSkillStatus(const TerminatedSkillStatus&);
+
+ void toIce(provider::dto::Execution::Status& ret, const SkillStatus& status);
+ void toIce(provider::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status);
+ void toIce(provider::dto::Execution::Status& ret, const TerminatedSkillStatus& status);
+
+ void fromIce(const provider::dto::Execution::Status& status, TerminatedSkillStatus& ret);
+ void fromIce(const provider::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret);
+ void fromIce(const provider::dto::Execution::Status& status, SkillStatus& ret);
+
+ struct SkillStatusUpdateBase
{
// header
- std::string providerName = "";
- std::string skillName = "";
- provider::dto::Execution::Status status = provider::dto::Execution::Status::Idle;
+ SkillID skillId = {"NOT INITIALIZED YET", "NOT INITIALIZED YET"};
+ std::string executorName = "";
SkillParameterization usedParameterization;
// data
aron::data::DictPtr data = nullptr;
+ };
+
+ // Will be returned after the execution of a skill
+ struct TerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
+
+ provider::dto::SkillStatusUpdate toIce() const;
+ static TerminatedSkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be returned from periodic skills which can still run
+ struct ActiveOrTerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
+
+ provider::dto::SkillStatusUpdate toIce() const;
+ static ActiveOrTerminatedSkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be used as status updates from skills to the callback interface
+ struct SkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ SkillStatus status = SkillStatus::Idle;
provider::dto::SkillStatusUpdate toIce() const;
+ static SkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index b922bc341506606e337ef82bf12425a20cf1199b..9f554e11e5c878bdd77cd7c866bef6871b3c6584 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -17,6 +17,25 @@ namespace armarx
public:
using ParamType = AronT;
+ struct SpecializedInitInput
+ {
+ std::string executorName;
+ AronT params;
+ };
+
+ struct SpecializedMainInput
+ {
+ std::string executorName;
+ AronT params;
+ CallbackT callback;
+ };
+
+ struct SpecializedExitInput
+ {
+ std::string executorName;
+ AronT params;
+ };
+
using Skill::Skill;
virtual ~SpecializedSkill() = default;
@@ -26,57 +45,95 @@ namespace armarx
return AronT::ToAronType();
}
+ bool isSkillAvailable(const SpecializedInitInput& in) const
+ {
+ return this->isAvailable();
+ }
+
+ Skill::InitResult initSkill(const SpecializedInitInput& in)
+ {
+ Skill::_init();
+ return this->init(in);
+ }
+ Skill::MainResult mainOfSkill(const SpecializedMainInput& in)
+ {
+ Skill::_main();
+ return this->main(in);
+ }
+ Skill::ExitResult exitSkill(const SpecializedExitInput& in)
+ {
+ Skill::_exit();
+ return this->exit(in);
+ }
+
+ Skill::MainResult executeFullSkill(const SpecializedMainInput& in)
+ {
+ this->resetSkill();
+ this->initSkill(SpecializedInitInput({.executorName = in.executorName, .params = in.params}));
+ auto ret = this->mainOfSkill(in);
+ this->exit(SpecializedExitInput({.executorName = in.executorName, .params = in.params}));
+ return ret;
+ }
+
+ private:
+ virtual bool isAvailable(const SpecializedInitInput&) const
+ {
+ return true;
+ }
+
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status main(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ virtual Skill::InitResult init(const SpecializedInitInput&)
{
- (void) params;
- return Status::Succeeded;
+ return InitResult{.status = TerminatedSkillStatus::Succeeded};
}
- virtual void init(const AronT& params)
+ /// Override this method with the actual implementation. The callback is for status updates to the calling instance
+ virtual Skill::MainResult main(const SpecializedMainInput& in)
{
- (void) params;
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
+ return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
}
- virtual void exit(const AronT& params)
+ /// Override this method with the actual implementation. The callback is for status updates to the calling instance
+ virtual Skill::ExitResult exit(const SpecializedExitInput&)
{
- (void) params;
+ return ExitResult{.status = TerminatedSkillStatus::Succeeded};
}
- Status execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ /// Do not use anymore
+ bool isAvailable(const InitInput& in) const final
{
- return Skill::execute(params.toAron(), callback);
+ AronT p;
+ p.fromAron(in.params);
+
+ return isAvailable(SpecializedInitInput({.executorName = in.executorName, .params = p}));
}
/// Do not use anymore
- Status main(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+ Skill::InitResult init(const InitInput& in) final
{
- Skill::main(params, callback);
AronT p;
- p.fromAron(params);
+ p.fromAron(in.params);
- return main(p, callback);
+ return init(SpecializedInitInput({.executorName = in.executorName, .params = p}));
}
/// Do not use anymore
- void init(const aron::data::DictPtr& params) final
+ Skill::MainResult main(const MainInput& in) final
{
- Skill::init(params);
AronT p;
- //ARMARX_IMPORTANT << aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(params).dump(2);
- p.fromAron(params);
+ p.fromAron(in.params);
- return init(p);
+ return main(SpecializedMainInput({.executorName = in.executorName, .params = p, .callback = in.callback}));
}
/// Do not use anymore
- void exit(const aron::data::DictPtr& params) final
+ Skill::ExitResult exit(const ExitInput& in) final
{
- Skill::exit(params);
AronT p;
- p.fromAron(params);
+ p.fromAron(in.params);
- exit(p);
+ return exit(SpecializedExitInput({.executorName = in.executorName, .params = p}));
}
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 50d95cade907d70b0f697589fb533dca5ea93f27..1fb8c3ac52010943e01b044d0e99ce144739a2b6 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -10,29 +10,33 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(this->skill);
}
- void SkillImplementationWrapper::execute(const skills::SkillParameterization parameterization)
+ TerminatedSkillStatusUpdate SkillImplementationWrapper::setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization parameterization)
{
std::unique_lock l(executingMutex);
const std::string skillName = skill->description.skillName;
ARMARX_INFO_S << "Executing skill: " << skillName;
- // reset Skill
- {
+ // reset execution params. This func is also used to clean up once this method returns
+ auto resetExecParam = [&](){
std::unique_lock l2(skillStatusMutex); // skill is not updating
- statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
+ //statusUpdate.status = skills::provider::dto::Execution::Status::Idle; I decided to not update the status to idle every time the skill stops.
statusUpdate.data = nullptr;
- skill->reset();
- }
+ statusUpdate.executorName = "";
+ };
// set params and setup variables
- {
+ auto setExecParams = [&](){
std::lock_guard l(skillStatusMutex);
statusUpdate.usedParameterization = parameterization;
- }
+ statusUpdate.executorName = executorName;
+ };
+
+ resetExecParam();
+ setExecParams();
auto& aron_params = parameterization.usedInputParams;
- auto updateStatus = [&](const skills::provider::dto::Execution::Status status, const aron::data::DictPtr& data = nullptr){
+ auto updateStatus = [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr){
std::lock_guard l(skillStatusMutex);
statusUpdate.status = status;
statusUpdate.data = data;
@@ -45,52 +49,174 @@ namespace armarx
}
};
+ auto createErrorMessage = [](const std::string& message){
+ auto obj = aron::make_dict();
+ auto m = aron::make_string(message, aron::Path({"errormessage"}));
+ obj->addElement("errormessage", m);
+ return obj;
+ };
+
+ // Check params
+ if (skill->description.acceptedType && not(aron_params))
+ {
+ std::string message = "SkillError 001: The Skill '" + skillName + "' requires a type but params are NULL.";
+ ARMARX_ERROR_S << message;
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
+
+ if (skill->description.acceptedType && aron_params && not(aron_params->fullfillsType(skill->description.acceptedType)))
+ {
+ std::string message = "SkillError 002: The Skill '" + skillName + "' has a type and got parameters but the input does not match the type.";
+ ARMARX_ERROR_S << message;
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
+
+ // Check if skill is available with the given parameterization
try
{
- // Check params
- if (not(aron_params) && skill->description.acceptedType)
+ if (not skill->isSkillAvailable(Skill::InitInput{.executorName = executorName, .params = aron_params}))
{
- throw armarx::LocalException("The Skill '" + skillName + "' requires a type but no params are NULL.");
+ std::string message = "SkillError 101: The Skill '" + skillName + "' is not available.";
+ ARMARX_WARNING << message;
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
}
+ }
+ catch (const std::exception& ex)
+ {
+ std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
- // set scheduled
- updateStatus(skills::provider::dto::Execution::Status::Scheduled);
+ // set scheduled
+ updateStatus(SkillStatus::Scheduled);
- // execute
- updateStatus(skills::provider::dto::Execution::Status::Running);
+ // reset skill and perhaps wait for dependencies
+ try
+ {
+ skill->resetSkill();
+ }
+ catch (const std::exception& ex)
+ {
+ std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+
+ updateStatus(SkillStatus::Failed);
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
+
+ try
+ {
+ skill->waitForDependenciesOfSkill();
+ }
+ catch (const std::exception& ex)
+ {
+ std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+
+ updateStatus(SkillStatus::Failed);
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
- skill->init(aron_params);
- auto ret = skill->main(aron_params, [&](const aron::data::DictPtr& update)
- {
- updateStatus(statusUpdate.status, update);
- });
- skill->exit(aron_params);
+ // execute. If the skill fails for some reason, from this point it will always execute its exit function.
+ updateStatus(SkillStatus::Running);
- switch (ret)
+
+ try
+ {
+ Skill::InitResult initRet = skill->initSkill({executorName, aron_params});
+ if (initRet.status != TerminatedSkillStatus::Succeeded)
{
- case skills::Skill::Status::Failed:
- throw armarx::LocalException("The Skill '" + skillName + "' failed during execution.");
- break;
- case skills::Skill::Status::TimeoutReached:
- throw armarx::LocalException("The Skill '" + skillName + "' reached timeout during execution.");
- break;
- case skills::Skill::Status::Stopped:
- {
- updateStatus(skills::provider::dto::Execution::Status::Aborted);
- break;
- }
- case skills::Skill::Status::Succeeded:
- {
- updateStatus(skills::provider::dto::Execution::Status::Succeeded);
- break;
- }
+ std::string message = "SkillError 401: The initialization of skill '" + skillName + "' did not succeed.";
+ skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+
+ updateStatus(skills::toSkillStatus(initRet.status));
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, initRet.status});
}
}
catch (const std::exception& ex)
{
- ARMARX_WARNING_S << "Skill " << skillName << " died with exception:\n" << ex.what();
- updateStatus(skills::provider::dto::Execution::Status::Failed);
+ std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+ skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+
+ updateStatus(SkillStatus::Failed);
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
}
+ // Init succeeded!
+
+
+ Skill::MainResult mainRet;
+ try
+ {
+ mainRet = skill->mainOfSkill({executorName, aron_params, [&updateStatus](const aron::data::DictPtr& update)
+ {
+ // during execution the statusUpdate.status is always RUNNING
+ updateStatus(SkillStatus::Running, update);
+ }});
+ if (mainRet.status != TerminatedSkillStatus::Succeeded)
+ {
+ std::string message = "SkillError 501: The main method of skill '" + skillName + "' did not succeed.";
+ skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+
+ updateStatus(skills::toSkillStatus(mainRet.status));
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, mainRet.status});
+ }
+ }
+ catch (const std::exception& ex)
+ {
+ std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+ skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+
+ updateStatus(SkillStatus::Failed);
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
+ // Main succeeded!
+
+ try
+ {
+ Skill::ExitResult exitRet = skill->exitSkill({executorName, aron_params});
+ if (exitRet.status != TerminatedSkillStatus::Succeeded)
+ {
+ std::string message = "SkillError 601: The exit method of skill '" + skillName + "' did not succeed.";
+ skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+
+ updateStatus(skills::toSkillStatus(exitRet.status));
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, exitRet.status});
+ }
+ }
+ catch (const std::exception& ex)
+ {
+ std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + ex.what();
+ ARMARX_ERROR_S << message;
+
+ updateStatus(SkillStatus::Failed);
+ resetExecParam();
+ return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ }
+ // Exit succeeded!
+
+
+ // All succeeded!
+ updateStatus(SkillStatus::Succeeded);
+
+ // Tidy up
+ resetExecParam();
+
+ // return result of main method
+ return {{skill->getSkillId(), executorName, parameterization, mainRet.data}, TerminatedSkillStatus::Succeeded};
}
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index f06420a2f1eef85a3e2df36d2471de55bab930e7..982bcc366dd415ceb85ce055a35992ae99a8165f 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -29,10 +29,12 @@ namespace armarx
// Task information
mutable std::shared_mutex executingMutex;
+ // ctor
SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill);
- // execute a skill. The parameterization is copied
- void execute(const skills::SkillParameterization);
+ // execute a skill. The parameterization is copied. T
+ // the return type additionally contains the input configuration (similar to the status updates used in callbacks)
+ TerminatedSkillStatusUpdate setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization);
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
deleted file mode 100644
index 25f589aab97d6ecd2f7c680dd5b5dfdfc5072052..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "LambdaSkillImplementation.h"
-
-namespace armarx
-{
- namespace skills::helper
- {
-
- Skill::Status LambdaSkill::main(const aron::data::DictPtr& data, const CallbackT& callback)
- {
- (void) callback;
- bool res = fun(data);
- return res ? Status::Succeeded : Status::Failed;
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/All.h b/source/RobotAPI/libraries/skills/provider/mixins/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f74ab14ff3d6f438c4343996852fb3065841c68
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/All.h
@@ -0,0 +1,7 @@
+#include "ArvizSkillMixin.h"
+#include "MNSSkillMixin.h"
+#include "GraspReadingSkillMixin.h"
+#include "MemoryReadingSkillMixin.h"
+#include "ObjectReadingSkillMixin.h"
+#include "ObjectWritingSkillMixin.h"
+#include "RobotReadingSkillMixin.h"
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b77cb543cb1c853f565cc4fc51d06181cca61ab
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h
@@ -0,0 +1,26 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+namespace armarx::skills::mixin
+{
+ struct ArvizSkillMixin
+ {
+ armarx::viz::Client arviz;
+ std::string layerName;
+
+ ArvizSkillMixin(const armarx::viz::Client& a, const std::string& ln) :
+ arviz(a),
+ layerName(ln)
+ {
+ }
+
+ void clearLayer()
+ {
+ auto l = arviz.layer(layerName);
+ arviz.commit(l);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..d69f390dadcb7eb0daed57f8f7a82148c055fb52
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
@@ -0,0 +1,25 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h>
+
+namespace armarx::skills::mixin
+{
+ struct GraspReadingSkillMixin
+ {
+ armem::grasping::known_grasps::Reader graspReader;
+
+ GraspReadingSkillMixin(armem::client::MemoryNameSystem& mns) : graspReader(mns)
+ {}
+ };
+
+ struct SpecificGraspReadingSkillMixin
+ {
+ std::string objectEntityId;
+ armem::grasping::known_grasps::Reader graspReader;
+
+ SpecificGraspReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns) : objectEntityId(n), graspReader(mns)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0bde8bdf665d23f3bbe59d060d74f010fd29d3f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h
@@ -0,0 +1,16 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+namespace armarx::skills::mixin
+{
+ struct MNSSkillMixin
+ {
+ armem::client::MemoryNameSystem mns;
+
+ MNSSkillMixin(const armem::client::MemoryNameSystem& m) : mns(m)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..214fbac445e675d2bde67ae38372fa271a30b504
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/MemoryReadingSkillMixin.h
@@ -0,0 +1,16 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+namespace armarx::skills::mixin
+{
+ struct MemoryReadingSkillMixin
+ {
+ armem::client::Reader memoryReader;
+
+ //MemoryReadingSkillMixin(armem::client::MemoryNameSystem& mns) : memoryReader(mns)
+ //{}
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..de5ef34b1dc448edc4a8ba634ad74600c7d33da4
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
@@ -0,0 +1,25 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+
+namespace armarx::skills::mixin
+{
+ struct ObjectReadingSkillMixin
+ {
+ armem::obj::instance::Reader objectReader;
+
+ ObjectReadingSkillMixin(armem::client::MemoryNameSystem& mns, const objpose::ObjectPoseProviderPrx& o) : objectReader(mns, o)
+ {}
+ };
+
+ struct SpecificObjectReadingSkillMixin
+ {
+ std::string objectEntityId;
+ armem::obj::instance::Reader objectReader;
+
+ SpecificObjectReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns, const objpose::ObjectPoseProviderPrx& o) : objectEntityId(n), objectReader(mns, o)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..edf8e6a2ef7360417f2e951ed36e0b24c62f4cef
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ObjectWritingSkillMixin.h
@@ -0,0 +1,16 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h>
+
+namespace armarx::skills::mixin
+{
+ struct ObjectWritingSkillMixin
+ {
+ armem::obj::instance::Writer objectWriter;
+
+ ObjectWritingSkillMixin(armem::client::MemoryNameSystem& mns) : objectWriter(mns)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..631491ddcfb70a7fc1c6930317d30bbb3c969dca
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/RobotReadingSkillMixin.h
@@ -0,0 +1,28 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+namespace armarx::skills::mixin
+{
+ struct RobotReadingSkillMixin
+ {
+ armem::robot_state::VirtualRobotReader robotReader;
+
+ RobotReadingSkillMixin(armem::client::MemoryNameSystem& mns) :
+ robotReader(mns)
+ {}
+ };
+
+ struct SpecificRobotReadingSkillMixin
+ {
+ std::string robotName;
+ armem::robot_state::VirtualRobotReader robotReader;
+
+ SpecificRobotReadingSkillMixin(const std::string& rn, armem::client::MemoryNameSystem& mns) :
+ robotName(rn),
+ robotReader(mns)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 96259ddebcdd577a2e049a2e9dda7546b76f46e4..798d6767bec35847f7f86e3cd64a5db5083ed230 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -3,10 +3,7 @@ set(LIB_NAME ukfm)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(manif QUIET)
armarx_build_if(manif_FOUND "manif not available")
-
-find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
set(LIBS