diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index 251382f2af086afccf73924421ea31a2aad01017..274619e2d4d59a0921c674950c7dd6413b7535d1 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -145,19 +145,27 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.configuration = {}
+# ArmarX.ExampleMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.ExampleMemory.mem.ltm.enabled:
+# ArmarX.ExampleMemory.mem.ltm.enable_querying:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.enabled = false
+# ArmarX.ExampleMemory.mem.ltm.enable_querying = false
+
+
+# ArmarX.ExampleMemory.mem.ltm.mode:
+# Attributes:
+# - Default: DISABLED
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.mem.ltm.mode = DISABLED
# ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
index 9622c34e544899a5f2e81e01f5f6f7ad6744b013..84c9820b1df4a8304787c971df18f978d8096cd4 100644
--- a/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
+++ b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
@@ -3,5 +3,7 @@
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/RobotSkillsMemory/config/ArVizStorage.cfg b/scenarios/RobotSkillsMemory/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/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/RobotSkillsMemory/config/DebugObserver.cfg b/scenarios/RobotSkillsMemory/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_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.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg b/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
index 7dd22218243ca4f9e67e843da8b42916f3b8568a..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
+++ b/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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
diff --git a/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
index 4fd690cefd94559b207493cf40e346a3e47f3b12..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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
diff --git a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
index 33ff12f5046dad45169c1661741ff7216055c26d..57d1fbc563156ae096bdaad0324321323a1a0724 100644
--- a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
+++ b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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
@@ -117,88 +117,105 @@
# ArmarX.SecondsStartupDelay = 0
-# ArmarX.SkillsMemory.:
+# ArmarX.SkillMemory.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.SkillMemory.EnableProfiling = false
+
+
+# ArmarX.SkillMemory.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.SkillMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SkillMemory.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory. = ""
+# ArmarX.SkillMemory.ObjectName = ""
-# ArmarX.SkillsMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.SkillMemory.StatechartCoreSegmentName: Name of the core segment for statecharts.
# Attributes:
-# - Default: false
+# - Default: Statechart
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillsMemory.EnableProfiling = false
+# ArmarX.SkillMemory.StatechartCoreSegmentName = Statechart
-# ArmarX.SkillsMemory.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.SkillMemory.TransitionsProviderSegmentName: Name of the provider segment for statechart transitions.
# Attributes:
-# - Default: Undefined
+# - Default: Transitions
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SkillsMemory.MinimumLoggingLevel = Undefined
+# ArmarX.SkillMemory.TransitionsProviderSegmentName = Transitions
-# ArmarX.SkillsMemory.ObjectName: Name of IceGrid well-known object
+# ArmarX.SkillMemory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: ""
+# - Default: Skill
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.ObjectName = ""
+# ArmarX.SkillMemory.mem.MemoryName = Skill
-# ArmarX.SkillsMemory.mem.MemoryName: Name of this memory server.
+# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: Skills
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.mem.MemoryName = Skills
+# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.SkillsMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.SkillMemory.mem.ltm.enable_querying:
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillsMemory.mns.MemoryNameSystemEnabled = true
+# ArmarX.SkillMemory.mem.ltm.enable_querying = false
-# ArmarX.SkillsMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.SkillMemory.mem.ltm.mode:
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: DISABLED
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.SkillMemory.mem.ltm.mode = DISABLED
-# ArmarX.SkillsMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.SkillMemory.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: DebugObserver
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.pub.DebugObserver = DebugObserver
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled = true
-# ArmarX.SkillsMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.SkillMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: MemoryUpdates
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.SkillMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.SkillsMemory.tpc.sub.ProfilerListener: Name of the ProfilerListenerInterface topics to subscribe.
+# ArmarX.SkillMemory.tpc.sub.ProfilerListener: Name of the ProfilerListenerInterface topics to subscribe.
# Attributes:
# - Default: StateReportingTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.sub.ProfilerListener = StateReportingTopic
+# ArmarX.SkillMemory.tpc.sub.ProfilerListener = StateReportingTopic
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
diff --git a/scenarios/SkillProviderTest/config/SkillsMemory.cfg b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
index 8b2bf04855a21e5a8ca955da89ba3a309460241e..57d1fbc563156ae096bdaad0324321323a1a0724 100644
--- a/scenarios/SkillProviderTest/config/SkillsMemory.cfg
+++ b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
@@ -169,19 +169,27 @@
# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm.configuration = {}
+# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.SkillMemory.mem.ltm.enabled:
+# ArmarX.SkillMemory.mem.ltm.enable_querying:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.enabled = false
+# ArmarX.SkillMemory.mem.ltm.enable_querying = false
+
+
+# ArmarX.SkillMemory.mem.ltm.mode:
+# Attributes:
+# - Default: DISABLED
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SkillMemory.mem.ltm.mode = DISABLED
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 77943d887c6833b87a730b1ce45b926273795ad2..f0e3077422c3d81e044806ddb4e12391424ab67e 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -135,7 +136,7 @@ namespace armarx::articulated_object
}
auto obj =
- VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
VirtualRobot::RobotIO::eStructure);
if (not obj)
@@ -163,6 +164,11 @@ namespace armarx::articulated_object
}
}
+ const auto state = articulatedObjectReaderPlugin->get().queryState(articulatedObject->getType() + "/" + articulatedObject->getName(), Clock::Now());
+
+ ARMARX_CHECK(state.has_value());
+ articulatedObject->setGlobalPose(state->globalPose.matrix());
+
ARMARX_DEBUG << "Reporting articulated objects";
const armem::Time now = armem::Time::Now();
@@ -179,7 +185,7 @@ namespace armarx::articulated_object
jointValue = node->unscaleJointValue(k, 0, 1);
}
- articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+ // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
articulatedObject->setJointValues(jointValues);
auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 6fe11f3196b211677110b31dd697a3eb4f055d5d..e00518697c1b1c0dcc8d2d7c767df17d593e1e9e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -76,7 +76,7 @@ namespace armarx::armem::server::obj
ObjectMemory::ObjectMemory() :
instance::SegmentAdapter(iceAdapter()),
classSegment(iceAdapter()),
- attachmentSegment(iceAdapter())
+ attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
{
addPlugin(virtualRobotReaderPlugin);
}
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 9b16109a96f01e951974f84c70f7e7b159558de5..65b94a3807343d850003a53bbdbabc438a2d4bcc 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -176,6 +176,24 @@ namespace armarx::skills::provider
fooDesc);
}
+ // Add another lambda example skill
+ {
+ skills::SkillDescription fooDesc;
+ fooDesc.acceptedType = nullptr; // accept everything
+ fooDesc.description = "This skill dies hard.";
+ fooDesc.skillName = "Die";
+ fooDesc.timeout = armarx::core::time::Duration::MilliSeconds(1000);
+ addSkill(
+ [](const std::string& clientId, const aron::data::DictPtr&)
+ {
+ std::cout << "bye bye... segfaulting on purpose now!" << std::endl;
+ Skill* nullSkill = NULL;
+ nullSkill->getSkillId(); // DEAD!
+ return TerminatedSkillStatus::Succeeded;
+ },
+ fooDesc);
+ }
+
// Add another example skill
addSkill(std::make_unique<CallbackSkill>());
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index cbd795396941e1d004fd5f7fdf0fa946d70cb71e..2aad033f1f00a2523dd8dcf15b03979eab92b339 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -27,8 +27,8 @@
#include <ArmarXCore/core/Component.h>
// RobotAPI
-#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
+#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
namespace armarx::skills::provider
{
@@ -55,7 +55,6 @@ namespace armarx::skills::provider
Skill::MainResult main(const MainInput& in) final;
};
-
class TimeoutSkill : public PeriodicSkill
{
public:
@@ -67,7 +66,6 @@ namespace armarx::skills::provider
PeriodicSkill::StepResult step(const MainInput& in) final;
};
-
class CallbackSkill : public Skill
{
public:
@@ -91,8 +89,8 @@ namespace armarx::skills::provider
* Detailed description of class ExampleClient.
*/
class SkillProviderExample :
- virtual public armarx::Component,
- virtual public SkillProviderComponentPluginUser
+ virtual public armarx::Component,
+ virtual public SkillProviderComponentPluginUser
{
public:
SkillProviderExample();
@@ -101,7 +99,6 @@ namespace armarx::skills::provider
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -111,4 +108,4 @@ namespace armarx::skills::provider
private:
};
-}
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 1e80202efb18b2aca77e8ba45b87827e362ae233..7c25a1b848daab3347a7247199aa9a54eefb99bc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -78,14 +78,17 @@
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="2" column="0">
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
+ <item row="3" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Update Frequency:</string>
</property>
</widget>
</item>
- <item row="3" column="0" colspan="3">
+ <item row="4" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
<property name="text">
@@ -104,8 +107,19 @@
</column>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="autoUpdateCheckBox">
+ <property name="text">
+ <string>Auto Update</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QPushButton" name="refreshNowPushButton">
+ <property name="text">
+ <string>Refresh Now</string>
+ </property>
+ </widget>
</item>
</layout>
</widget>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 0fba47d76eb851d528c306f46d24e17c24661223..f9fa41252ad6a0f1144bf7eb043f62771061b5fc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -89,7 +89,7 @@ namespace armarx
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
refreshSkillsResultTimer = new QTimer(this);
- refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
+ refreshSkillsResultTimer->setInterval(1000 / 2); // Keep this stable.
refreshSkillsResultTimer->start();
connect(widget.doubleSpinBoxUpdateFreq,
@@ -99,7 +99,7 @@ namespace armarx
connect(refreshSkillsResultTimer,
&QTimer::timeout,
this,
- &SkillManagerMonitorWidgetController::refreshSkills);
+ &SkillManagerMonitorWidgetController::refreshSkillsPeriodically);
connect(widget.pushButtonCopy,
&QPushButton::clicked,
@@ -123,19 +123,15 @@ namespace armarx
&QTreeWidget::currentItemChanged,
this,
&SkillManagerMonitorWidgetController::skillSelectionChanged);
- }
- SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
- {
+ connect(widget.refreshNowPushButton,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::refreshSkills);
}
- void
- SkillManagerMonitorWidgetController::reconnectToSkillManager()
+ SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
{
- if (connected)
- {
- getProxy(manager, observerName, 1000);
- }
}
void
@@ -152,6 +148,7 @@ namespace armarx
QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
+ getProxy(manager, observerName, 1000);
connected = true;
}
@@ -177,6 +174,15 @@ namespace armarx
refreshSkillsResultTimer->setInterval(f);
}
+ void
+ SkillManagerMonitorWidgetController::refreshSkillsPeriodically()
+ {
+ if (widget.autoUpdateCheckBox->isChecked())
+ {
+ refreshSkills();
+ }
+ }
+
void
SkillManagerMonitorWidgetController::refreshSkills()
{
@@ -191,139 +197,162 @@ namespace armarx
if (!manager)
{
- reconnectToSkillManager();
return;
}
/* CHECK OWN SKILLS LIST */
// remove non-existing ones
- auto managerSkills = manager->getSkillDescriptions();
- std::vector<std::string> removedProviders;
- for (auto it = skills.begin(); it != skills.end();)
+ try
{
- // TODO: iterate over skills, not just over providers!
- std::string providerName = it->first;
- if (managerSkills.find(providerName) == managerSkills.end())
- {
- removedProviders.push_back(providerName);
- it = skills.erase(it);
- }
- else
+ ARMARX_DEBUG << "GET REMOVED PROVIDERS AND REMOVE FROM MAP";
+ std::scoped_lock l(skillMutex);
+ auto managerSkills = manager->getSkillDescriptions();
+ std::vector<std::string> removedProviders;
+ for (auto it = skills.begin(); it != skills.end();)
{
- it++;
+ // TODO: iterate over skills, not just over providers!
+ std::string providerName = it->first;
+ if (managerSkills.find(providerName) == managerSkills.end())
+ {
+ ARMARX_DEBUG << "REMOVE " << providerName;
+ removedProviders.push_back(providerName);
+ it = skills.erase(it);
+ }
+ else
+ {
+ it++;
+ }
}
- }
- // add new ones
- std::vector<std::string> newProviders;
- for (const auto& [providerName, providerSkills] : managerSkills)
- {
- if (skills.find(providerName) == skills.end())
+ // add new ones
+ ARMARX_DEBUG << "GET NEW PROVIDERS AND ADD THEM TO MAP";
+ std::vector<std::string> newProviders;
+ for (const auto& [providerName, providerSkills] : managerSkills)
{
- skills.insert(std::make_pair(providerName, providerSkills));
- newProviders.push_back(providerName);
+ if (skills.find(providerName) == skills.end())
+ {
+ ARMARX_DEBUG << "ADD " << providerName;
+ skills.insert(std::make_pair(providerName, providerSkills));
+ newProviders.push_back(providerName);
+ }
}
- }
- /* CHECK TREE VIEW */
- // remove providers from tree
- int i = 0;
- while (i < widget.treeWidgetSkills->topLevelItemCount())
- {
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (std::find(removedProviders.begin(),
- removedProviders.end(),
- item->text(0).toStdString()) != removedProviders.end())
- {
- delete widget.treeWidgetSkills->takeTopLevelItem(i);
- }
- else
+ /* CHECK TREE VIEW */
+ // remove providers from tree
+ ARMARX_DEBUG << "REMOVE PROVIDERS FROM TREE VIEW";
+ int i = 0;
+ while (i < widget.treeWidgetSkills->topLevelItemCount())
{
- ++i;
+ QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+ if (auto it = std::find(removedProviders.begin(),
+ removedProviders.end(),
+ item->text(0).toStdString());
+ it != removedProviders.end())
+ {
+ ARMARX_DEBUG << "REMOVE PROVIDER " << *it;
+ delete widget.treeWidgetSkills->takeTopLevelItem(i);
+ }
+ else
+ {
+ ++i;
+ }
}
- }
- // add new providers
- for (const auto& [providerName, providerSkills] : skills)
- {
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
- it != newProviders.end())
+ // add new providers
+ ARMARX_DEBUG << "ADD NEW PROVIDERS TO TREE VIEW";
+ for (const auto& [providerName, providerSkills] : skills)
{
- auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
- item->setText(0, QString::fromStdString(providerName));
- for (const auto& [name, sk] : providerSkills)
+ if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
+ it != newProviders.end())
{
- auto itsk = new QTreeWidgetItem(item);
- item->addChild(itsk);
- itsk->setText(0, QString::fromStdString(name));
+ auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
+ item->setText(0, QString::fromStdString(providerName));
+ for (const auto& [name, sk] : providerSkills)
+ {
+ ARMARX_DEBUG << "ADD PROVIDER " << *it;
+ auto itsk = new QTreeWidgetItem(item);
+ item->addChild(itsk);
+ itsk->setText(0, QString::fromStdString(name));
+ }
}
}
- }
- // 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)
- {
- try
+ // update status and active skills window
+ ARMARX_DEBUG << "UPDATE STATI AND ACTIVE SKILL";
+ std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
+ auto managerStatuses = manager->getSkillExecutionStatuses();
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- auto providerName = item->text(0).toStdString();
-
- auto allStatusesForProvider = managerStatuses.at(providerName);
-
- for (int j = 0; j < item->childCount(); ++j)
+ try
{
- QTreeWidgetItem* skillItem = item->child(j);
- skills::SkillID currentSkillId(providerName, skillItem->text(0).toStdString());
+ QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = item->text(0).toStdString();
- auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
- skillItem->setText(2,
- QString::fromStdString(ExecutionStatus2String.at(
- statusForSkill.header.status)));
+ ARMARX_DEBUG << "UPDATE STATI FOR PROVIDER " << providerName;
+ auto allStatusesForProvider = managerStatuses.at(providerName);
- if (not statusForSkill.header.executorName
- .empty()) // it means that the skill was called by someone
+ for (int j = 0; j < item->childCount(); ++j)
{
- activeSkillsAndPrefixes.insert(
- {currentSkillId, statusForSkill.header.executorName});
+ QTreeWidgetItem* skillItem = item->child(j);
+ skills::SkillID currentSkillId(providerName,
+ skillItem->text(0).toStdString());
+
+ ARMARX_DEBUG << "UPDATE STATI FOR SKILL " << currentSkillId.skillName;
+ 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
+ {
+ ARMARX_DEBUG << "ADD SKILL TO ACTIVE " << currentSkillId.skillName;
+ activeSkillsAndPrefixes.insert(
+ {currentSkillId, statusForSkill.header.executorName});
+ }
}
}
+ catch (...)
+ {
+ // Perhaps the skill provider died after the check at the beginning of this method
+ continue;
+ }
}
- catch (const std::exception& e)
- {
- // Perhaps the skill provider died after the check at the beginning of this method
- 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
+ // finally update the view of active skills
+ ARMARX_DEBUG << "UPDATE ACTIVE SKILLS";
+ widget.listWidgetActiveSkills->clear();
+ for (const auto& [id, prefix] : activeSkillsAndPrefixes)
{
- auto prefixedStr2 = id.toString(prefix2);
- if (prefixedStr == prefixedStr2)
+ auto prefixedStr = id.toString(prefix);
+ bool longest = true;
+ for (
+ const auto& [id2, prefix2] :
+ activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
{
- continue;
+ auto prefixedStr2 = id.toString(prefix2);
+ if (prefixedStr == prefixedStr2)
+ {
+ continue;
+ }
+
+ if (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ {
+ longest = false;
+ break;
+ }
}
- if (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ if (longest)
{
- longest = false;
- break;
+ widget.listWidgetActiveSkills->addItem(
+ QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
}
}
-
- if (longest)
- {
- widget.listWidgetActiveSkills->addItem(
- QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
- }
+ }
+ catch (...)
+ {
+ // perhaps the manager died during the method?
}
}
@@ -335,6 +364,7 @@ namespace armarx
return;
}
+ std::scoped_lock l(skillMutex);
const auto& skillDescriptions = skills.at(selectedSkill.providerName);
if (!skillDescriptions.count(selectedSkill.skillName))
{
@@ -361,6 +391,7 @@ namespace armarx
void
SkillManagerMonitorWidgetController::stopSkill()
{
+ std::scoped_lock l(skillMutex);
if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
{
return;
@@ -381,6 +412,7 @@ namespace armarx
SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
QTreeWidgetItem*)
{
+ std::scoped_lock l(skillMutex);
widget.groupBoxSkillDetails->setEnabled(false);
if (!current)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index d9a9379bdfdea0ee56c9c5f3e601ef68a5ce75b5..f1ad1bb720ea69503b86b00f75a6e6f09f6058c5 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -43,14 +43,12 @@
namespace armarx
{
- class ARMARXCOMPONENT_IMPORT_EXPORT
- SkillManagerMonitorWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < SkillManagerMonitorWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT SkillManagerMonitorWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<SkillManagerMonitorWidgetController>
{
Q_OBJECT
public:
-
/// Controller Constructor
explicit SkillManagerMonitorWidgetController();
/// Controller destructor
@@ -65,13 +63,12 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Skills.Manager";
}
- void reconnectToSkillManager();
-
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -84,6 +81,7 @@ namespace armarx
void updateTimerFrequency();
void refreshSkills();
+ void refreshSkillsPeriodically();
void copyCurrentConfig();
void pasteCurrentConfig();
@@ -97,8 +95,8 @@ namespace armarx
/**
* Widget Form
*/
- Ui::SkillManagerMonitorWidget widget;
- QPointer<SimpleConfigDialog> dialog;
+ Ui::SkillManagerMonitorWidget widget;
+ QPointer<SimpleConfigDialog> dialog;
std::string observerName = "SkillManager";
skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
@@ -110,6 +108,7 @@ namespace armarx
};
// Data taken from observer (snapshot of it)
+ mutable std::mutex skillMutex;
skills::manager::dto::SkillDescriptionMapMap skills = {};
// User Input
@@ -128,6 +127,4 @@ namespace armarx
// connected flag
std::atomic_bool connected = false;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 80c3168c5fc5e7c893c2813082171e5cd9c6319b..20c3458aa7941cf7caa5a4a0dab9010f2267c035 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -231,6 +231,8 @@ namespace armarx::armem::articulated_object
cs.dataset = objectId.dataset();
objectInstance.pose.objectID = cs;
+ objectInstance.pose.providerName = properties.providerName;
+ objectInstance.pose.attachmentValid = false;
update.instancesData = {objectInstance.toAron()};
update.referencedTime = timestamp;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 514096731b83167b902dfe4198deae82725837ed..b9abf459ec14789b7e4fe3fccf42ba73db0f5eed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -70,6 +70,11 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
+ if(name.empty())
+ {
+ continue;
+ }
+
auto poseHistoryMap = poseHistories.find(name);
if (poseHistoryMap != poseHistories.end())
{
@@ -92,6 +97,12 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, viz::Layer> stage;
for (size_t i = 0; i < objectPoses.size(); ++i)
{
+ if(objectPoses.at(i).providerName.empty())
+ {
+ ARMARX_INFO << "Object pose provider not set!";
+ continue;
+ }
+
visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
objectPoses.at(i),
poseHistories.at(i),
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 79164ab84cea2848fd5737d301f30df6bdc0cf5d..398e5bcdbf53a3fd059eaa792e8caa16344a3b4e 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -111,7 +111,7 @@ namespace armarx::aron::data::converter
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
const auto dims = nav.getShape();
- ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ...
+ ARMARX_CHECK(dims.size() == 2 or dims.size() == 3); // for now ...
Eigen::Map<MatrixT> map(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
return map;
@@ -197,7 +197,7 @@ namespace armarx::aron::data::converter
return ConvertToDynamicArray<T>(nav);
}
- checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToArray");
auto dims = nav.getShape();
Eigen::Map<Eigen::Array<T, Rows, Cols>> ret(
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
index 03b4c0f270a1b552034022b4fed16b341085be0f..9ed6259492303586191d67d5fdcc3aebee525bf8 100644
--- a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/PropertyUser.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron_component_config/VariantHelperFactory.h>
@@ -50,7 +51,7 @@ namespace armarx::aron::component_config
{
void
- PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& /*elementType*/)
{
INPUT_GUARD(i);
auto value = data::Int::DynamicCastAndCheck(i);
@@ -59,7 +60,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& /*elementType*/)
{
INPUT_GUARD(f);
auto value = data::Float::DynamicCastAndCheck(f);
@@ -68,7 +69,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& /*elementType*/)
{
INPUT_GUARD(d);
auto value = data::Double::DynamicCastAndCheck(d);
@@ -77,7 +78,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& /*elementType*/)
{
INPUT_GUARD(b);
auto value = data::Bool::DynamicCastAndCheck(b);
@@ -86,7 +87,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& /*elementType*/)
{
INPUT_GUARD(string);
auto value = data::String::DynamicCastAndCheck(string);
@@ -135,13 +136,13 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& o, TypeInput& t)
+ PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& /*o*/, TypeInput& /*t*/)
{
in_list_ = false;
}
void
- PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& /*elementType*/)
{
ARMARX_TRACE;
auto data = data::Int::DynamicCastAndCheck(enumData);
@@ -150,21 +151,23 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& /*elementType*/)
{
ARMARX_WARNING << "Unknown data encountered: "
<< (unknown ? unknown->getFullName() : "nullptr");
}
PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(
- const armarx::PropertyUser& defs, const std::string& global_namespace):
- property_user_(std::experimental::make_observer(&defs)), global_namespace_(global_namespace), in_list_(false)
+ const armarx::PropertyUser& defs,
+ const std::string& globalNamespace) :
+ property_user_(std::experimental::make_observer(&defs)),
+ global_namespace_(globalNamespace),
+ in_list_(false)
{
}
void
- PropertyDefinitionGetterVisitor::visitDictOnEnter(std::shared_ptr<data::Variant>& o,
- const std::shared_ptr<type::Variant>& t)
+ PropertyDefinitionGetterVisitor::visitDictOnEnter(DataInput& o, TypeInput& t)
{
ARMARX_TRACE;
in_list_ = true;
@@ -198,9 +201,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitDictOnExit(
- std::shared_ptr<data::Variant>& elementData,
- const std::shared_ptr<type::Variant>& elementType)
+ PropertyDefinitionGetterVisitor::visitDictOnExit(DataInput& /*elementData*/,
+ TypeInput& /*elementType*/)
{
in_list_ = false;
}
@@ -242,8 +244,34 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitObjectOnEnter(std::shared_ptr<data::Variant>& i,
- const std::shared_ptr<type::Variant>& j)
+ PropertyDefinitionGetterVisitor::visitMatrix(DataInput& matrix, TypeInput& /*type*/)
+ {
+ INPUT_GUARD(matrix);
+ auto data = data::NDArray::DynamicCastAndCheck(matrix);
+ const auto name = global_namespace_ + pathToName(matrix);
+ const std::string property = property_user_->getProperty<std::string>(name).getValue();
+ const auto vec = simox::alg::to_eigen_vec(property, ", ");
+ ARMARX_CHECK_EQUAL(sizeof(float) * vec.size(), data->getDataAsVector().size())
+ << "Matrix recovered from properties does not match the aron type";
+
+ data->setData(data->getDataAsVector().size(),
+ reinterpret_cast<const unsigned char*>(vec.data()));
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitQuaternion(DataInput& quaternion, TypeInput& /*type*/)
+ {
+ INPUT_GUARD(quaternion);
+ auto value = data::NDArray::DynamicCastAndCheck(quaternion);
+ auto name = global_namespace_ + pathToName(quaternion);
+ const std::string property = property_user_->getProperty<std::string>(name).getValue();
+ const Eigen::Quaternion quat(simox::alg::to_eigen_vec(property, ", ").data());
+ const auto& newQuat = data::converter::AronEigenConverter::ConvertFromQuaternion(quat);
+ value->setData(newQuat->getDataAsVector().size(), newQuat->getData());
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitObjectOnEnter(DataInput& i, TypeInput& j)
{
INPUT_GUARD(i);
if (global_namespace_.empty())
@@ -254,13 +282,16 @@ namespace armarx::aron::component_config
}
PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(
- const PropertyDefinitionsPtr& defs, const std::string& global_namespace) :
- property_definitions_(std::experimental::make_observer(defs.get())), global_namespace_(global_namespace)
+ const PropertyDefinitionsPtr& defs,
+ const std::string& globalNamespace) :
+ property_definitions_(std::experimental::make_observer(defs.get())),
+ global_namespace_(globalNamespace)
{
}
void
- PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i, const type::IntPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i,
+ const type::IntPtr& /*unused*/)
{
INPUT_GUARD(i);
auto name = global_namespace_ + pathToName(i);
@@ -269,7 +300,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::FloatPtr& f,
- const type::FloatPtr&)
+ const type::FloatPtr& /*unused*/)
{
INPUT_GUARD(f);
auto name = global_namespace_ + pathToName(f);
@@ -278,7 +309,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::DoublePtr& d,
- const type::DoublePtr&)
+ const type::DoublePtr& /*unused*/)
{
INPUT_GUARD(d);
auto name = global_namespace_ + pathToName(d);
@@ -286,7 +317,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b, const type::BoolPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b,
+ const type::BoolPtr& /*unused*/)
{
INPUT_GUARD(b);
auto name = global_namespace_ + pathToName(b);
@@ -295,7 +327,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::StringPtr& string,
- const type::StringPtr& type)
+ const type::StringPtr& /*type*/)
{
INPUT_GUARD(string);
auto name = global_namespace_ + pathToName(string);
@@ -339,8 +371,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr&,
- const type::ListPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr& /*unused*/,
+ const type::ListPtr& /*unused*/)
{
in_list_ = false;
}
@@ -375,8 +407,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr&,
- const type::DictPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr& /*unused*/,
+ const type::DictPtr& /*unused*/)
{
in_list_ = false;
}
@@ -391,6 +423,48 @@ namespace armarx::aron::component_config
global_namespace_ = type->getObjectNameWithoutNamespace() + ".";
}
}
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& quaternion,
+ const type::QuaternionPtr& /*type*/)
+ {
+ INPUT_GUARD(quaternion);
+ auto name = global_namespace_ + pathToName(quaternion);
+ const auto quat =
+ data::converter::AronEigenConverter::ConvertToQuaternion<float>(quaternion);
+ property_definitions_->defineOptionalProperty<std::string>(
+ name, simox::alg::to_string(quat.coeffs(), ", "));
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& matrix,
+ const type::MatrixPtr& /*type*/)
+ {
+ INPUT_GUARD(matrix);
+ auto name = global_namespace_ + pathToName(matrix);
+ auto shape = matrix->getShape();
+ // TODO: why is shape 3 for a matrix?
+ if (shape.size() != 3 or (shape[0] != 1 and shape[1] != 1))
+ {
+ ARMARX_WARNING << "Matrix is not a vector, ignoring";
+ return;
+ }
+ const auto mat = data::converter::AronEigenConverter::ConvertToMatrix<float>(*matrix);
+ std::string value = "";
+ for (int i = 0; i < shape[0]; i++)
+ {
+ for (int j = 0; j < shape[1]; j++)
+ {
+ value += std::to_string(mat(i, j)) + ", ";
+ }
+ }
+ if (not value.empty())
+ {
+ value.pop_back();
+ value.pop_back();
+ }
+ property_definitions_->defineOptionalProperty<std::string>(name, value);
+ }
} // namespace armarx::aron::component_config
#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
index a9c1d5823bd8589acaea27b4f27928b4668ef853..e00f96e75d2ea44b44d08674fc599528e59e7a12 100644
--- a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
@@ -34,7 +34,8 @@ namespace armarx::aron::component_config
class PropertyDefinitionSetterVisitor : public aron::data::RecursiveConstTypedVariantVisitor
{
public:
- explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs, const std::string& global_namespace = "");
+ explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs,
+ const std::string& global_namespace = "");
void visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) override;
@@ -48,6 +49,10 @@ namespace armarx::aron::component_config
void visitAronVariant(const data::StringPtr&, const type::StringPtr&) override;
+ void visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&) override;
+
+ void visitAronVariant(const data::NDArrayPtr&, const type::MatrixPtr&) override;
+
void visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) override;
void visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) override;
@@ -69,7 +74,8 @@ namespace armarx::aron::component_config
public aron::data::RecursiveTypedVisitor<data::VariantPtr, const type::VariantPtr>
{
public:
- explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs, const std::string& global_namespace = "");
+ explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs,
+ const std::string& global_namespace = "");
type::Descriptor getDescriptor(DataInput& o, TypeInput& t) override;
@@ -95,6 +101,10 @@ namespace armarx::aron::component_config
void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
+ void visitQuaternion(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitMatrix(DataInput& elementData, TypeInput& elementType) override;
+
void visitListOnEnter(DataInput& elementData, TypeInput& elementType) override;
void visitListOnExit(DataInput& elementData, TypeInput& elementType) override;
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index d53a8dd6a89f0cd37f609c9a4742d97d86f4deba..0f1f8047f3f47976a7ce52a928bac858d665b053 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -62,6 +62,7 @@ namespace armarx::obstacle_avoidance
if (auto obstacle = finder.loadManipulationObject(objectPose))
{
obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ obstacle->setName(objectPose.objectID.instanceName());
sceneObjects->addSceneObject(obstacle);
}
}
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 60a38b465ddb8bd9d6cbdac5ec4952606624880c..2908d2e77a0a16627d65d2eb757d3ad8431c3271 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,20 +1,26 @@
#include "SkillManagerComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
+
#include "../error/Exception.h"
namespace armarx::plugins
{
- void SkillManagerComponentPlugin::preOnInitComponent()
- {}
-
- void SkillManagerComponentPlugin::preOnConnectComponent()
- {}
+ void
+ SkillManagerComponentPlugin::preOnInitComponent()
+ {
+ }
- void SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
- {}
-}
+ void
+ SkillManagerComponentPlugin::preOnConnectComponent()
+ {
+ }
+ void
+ SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ }
+} // namespace armarx::plugins
namespace armarx
{
@@ -23,7 +29,9 @@ namespace armarx
addPlugin(plugin);
}
- void SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current&)
+ void
+ SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current&)
{
std::lock_guard l(skillProviderMapMutex);
if (skillProviderMap.find(info.providerName) == skillProviderMap.end())
@@ -33,13 +41,16 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists. "
+ ARMARX_INFO << "Trying to add a provider with name '" << info.providerName
+ << "' but the provider already exists. "
<< "Overwriting the old provider info.";
skillProviderMap[info.providerName] = info.provider;
}
}
- void SkillManagerComponentPluginUser::removeProvider(const std::string& providerName, const Ice::Current&)
+ void
+ SkillManagerComponentPluginUser::removeProvider(const std::string& providerName,
+ const Ice::Current&)
{
std::lock_guard l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
@@ -49,11 +60,13 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't be found.";
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerName
+ << "' but it couldn't be found.";
}
}
- std::string SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
+ std::string
+ SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
{
for (const auto& [providerName, providerPrx] : skillProviderMap)
{
@@ -69,86 +82,134 @@ namespace armarx
return "INVALID PROVIDER NAME";
}
- skills::provider::dto::SkillStatusUpdate SkillManagerComponentPluginUser::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current&)
+ using SkillProviderInterfacePrxMap =
+ std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx>;
+
+ 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()))
+ else if (not(info.skillId.providerName.empty()))
{
providerName = info.skillId.providerName;
}
+ SkillProviderInterfacePrxMap skillProviderMap;
+ {
+ std::scoped_lock l(skillProviderMapMutex);
+ skillProviderMap = this->skillProviderMap;
+ }
+
bool remove = false;
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
const auto& n = it->first;
const auto& s = it->second;
- if (s)
+ try
{
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
+ if (s)
+ {
+ skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
+ getProxy(myPrx, -1);
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ skills::provider::dto::SkillExecutionRequest exInfo;
+ exInfo.skillName = info.skillId.skillName;
+ exInfo.executorName = info.executorName;
+ exInfo.callbackInterface = myPrx;
+ exInfo.params = info.params;
- return s->executeSkill(exInfo);
+ return s->executeSkill(exInfo);
+ }
+ else
+ {
+ remove = true;
+ }
}
- else
+ catch (...)
{
remove = true;
}
+
if (remove)
{
std::scoped_lock l(skillProviderMapMutex);
+ // No copy!
+ SkillProviderInterfacePrxMap& skillProviderMap = this->skillProviderMap;
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "' during execution. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << n
+ << "' during execution. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
}
else
{
- 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.");
+ 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.");
}
return {}; // Never happens
}
- void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t)
+ void
+ SkillManagerComponentPluginUser::abortSkill(const std::string& providerName,
+ const std::string& skillName,
+ const Ice::Current& current)
{
- std::scoped_lock l(skillProviderMapMutex);
+ SkillProviderInterfacePrxMap skillProviderMap;
+ {
+ std::scoped_lock l(skillProviderMapMutex);
+ skillProviderMap = this->skillProviderMap;
+ }
+
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
const auto& n = it->first;
const auto& s = it->second;
- if (s)
+ try
{
- s->abortSkill(skillName);
+ if (s)
+ {
+ s->abortSkill(skillName);
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << n << "'. Removing it from skills on next execute.";
+ }
}
- else
+ catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
+ << "'. Removing it from skills on next execute.";
}
}
}
- void SkillManagerComponentPluginUser::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current&)
+ void
+ SkillManagerComponentPluginUser::updateStatusForSkill(
+ const skills::provider::dto::SkillStatusUpdate& statusUpdate,
+ const Ice::Current&)
{
- (void) statusUpdate;
+ (void)statusUpdate;
// If you want to use the status, implement this method!
}
- skills::manager::dto::SkillDescriptionMapMap SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current ¤t)
+ skills::manager::dto::SkillDescriptionMapMap
+ SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
{
skills::manager::dto::SkillDescriptionMapMap ret;
@@ -157,22 +218,33 @@ namespace armarx
{
const auto& n = it->first;
const auto& s = it->second;
- if (s)
+ try
{
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
- ++it;
+ if (s)
+ {
+ skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
+ ret.insert({n, m});
+ ++it;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
- else
+ catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
+ << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
- skills::manager::dto::SkillStatusUpdateMapMap SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current ¤t)
+ skills::manager::dto::SkillStatusUpdateMapMap
+ SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
{
skills::manager::dto::SkillStatusUpdateMapMap ret;
@@ -181,18 +253,28 @@ namespace armarx
{
const auto& n = it->first;
const auto& s = it->second;
- if (s)
+ try
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
- it++;
+ if (s)
+ {
+ skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
+ ret.insert({n, m});
+ it++;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
- else
+ catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
+ << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 21b32da1a75488fde1efea01b3b48e42b7fa77b9..ba48923e7beab1876c6477d04845d05ce64883b9 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -131,6 +131,14 @@ namespace armarx
onTimeoutReached();
}
+ Skill::MainResult Skill::MakeAbortedResult(aron::data::DictPtr data)
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Aborted,
+ .data = data,
+ };
+ }
+
void Skill::notifySkillToStopASAP()
{
stopped = true;
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 0522191bfb57d272785380502217f23006a9e254..bfba4d4cce7c37a880ea93b919373bdd3e126402 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -107,6 +107,8 @@ namespace armarx
MainResult executeFullSkill(const MainInput& in);
protected:
+ static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
+
// fires if the skill reaches timeout
void notifyTimeoutReached();
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
index e0ebdb3b653a1be1d59d9b945ac6b35c58962595..93e18a137f3f927017426aab55ae6b2c05507830 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
@@ -43,5 +43,12 @@ namespace armarx
auto future = manager->begin_executeSkill(req);
return future;
}
+
+ void SkillProxy::abortSkill(const std::string& executorName)
+ {
+ // TODO: This will be used in the future, do not remove it!
+ (void) executorName;
+ manager->abortSkill(skillId.providerName, skillId.skillName);
+ }
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
index 6e8ddc5c74ec5e3d36423984d67807ae12da0680..6fb15914686e6d9f01d0387719b6d5729b98c495 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
@@ -17,6 +17,8 @@ namespace armarx
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);
+ void abortSkill(const std::string& executorName);
+
private:
const manager::dti::SkillManagerInterfacePrx& manager;