diff --git a/scenarios/SkillProviderTest/SkillProviderTest.scx b/scenarios/SkillProviderTest/SkillProviderTest.scx
index aa275e46094978c888fe4732db5b70eca27052ac..7e5aa06a7ce69f63fcc6fc606f55d17b704b82a1 100644
--- a/scenarios/SkillProviderTest/SkillProviderTest.scx
+++ b/scenarios/SkillProviderTest/SkillProviderTest.scx
@@ -2,5 +2,9 @@
<scenario name="SkillProviderTest" creation="2022-06-02.15:33:15" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="SkillProviderExample" instance="" package="RobotAPI" 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="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/SkillProviderTest/config/ArVizStorage.cfg b/scenarios/SkillProviderTest/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/SkillProviderTest/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/SkillProviderTest/config/DebugObserver.cfg b/scenarios/SkillProviderTest/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/SkillProviderTest/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/SkillProviderTest/config/MemoryNameSystem.cfg b/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/SkillProviderTest/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/SkillProviderTest/config/RemoteGuiProviderApp.cfg b/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/SkillProviderTest/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/SkillProviderTest/config/SkillsMemory.cfg b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
index 57d1fbc563156ae096bdaad0324321323a1a0724..fc086fbe56a016013571883b41f8f8fb721f55b6 100644
--- a/scenarios/SkillProviderTest/config/SkillsMemory.cfg
+++ b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
@@ -18,6 +18,15 @@
# ArmarX.ApplicationName = ""
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
# 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
@@ -175,21 +184,13 @@
# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.SkillMemory.mem.ltm.enable_querying:
+# ArmarX.SkillMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# 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.mem.ltm.enabled = false
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index e4e5b22aa996ecb88f8b63b3db8bba4831d699cd..605911657b2bb74dc53cdf114c4304a82a41be4a 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -5,74 +5,73 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(std::string prefix) :
+ GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(
+ std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
}
- armarx::PropertyDefinitionsPtr GraspProviderExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GraspProviderExample::createPropertyDefinitions()
{
ARMARX_IMPORTANT << "Prperty defs";
- armarx::PropertyDefinitionsPtr defs = new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
return defs;
-
}
- std::string GraspProviderExample::getDefaultName() const
+ std::string
+ GraspProviderExample::getDefaultName() const
{
return "GraspProviderExample";
}
- GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem()), reader(memoryNameSystem())
+ GraspProviderExample::GraspProviderExample()
{
-
}
-
- void GraspProviderExample::onInitComponent()
+ void
+ GraspProviderExample::onInitComponent()
{
ARMARX_IMPORTANT << "Init";
}
-
- void GraspProviderExample::onConnectComponent()
+ void
+ GraspProviderExample::onConnectComponent()
{
- writer.connect();
- reader.connect();
+ writer.connect(memoryNameSystem());
+ reader.connect(memoryNameSystem());
task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
task->start();
}
-
- void GraspProviderExample::onDisconnectComponent()
+ void
+ GraspProviderExample::onDisconnectComponent()
{
task->stop();
}
-
- void GraspProviderExample::onExitComponent()
+ void
+ GraspProviderExample::onExitComponent()
{
}
-
- void GraspProviderExample::run()
+ void
+ GraspProviderExample::run()
{
ARMARX_IMPORTANT << "Running example.";
@@ -90,9 +89,11 @@ namespace armarx
// initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
- bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+ bimanualCandidate.groupNr =
+ i; //non-necessary field, but used to commit different candidates
- writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "bimanualProvider");
+ writer.commitBimanualGraspCandidate(
+ bimanualCandidate, armem::Time::Now(), "bimanualProvider");
//test for writing Seqs, candidates from the same object appear as instances of the same snapshot
@@ -102,7 +103,8 @@ namespace armarx
candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
- writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "candidateProvider");
+ writer.commitGraspCandidateSeq(
+ candidatesToWrite, armem::Time::Now(), "candidateProvider");
// test reader and debug by logging the group number of the candidate
@@ -112,15 +114,15 @@ namespace armarx
{
candidates = reader.queryLatestGraspCandidates();
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
ARMARX_ERROR << e.makeMsg(memoryName);
}
- for (auto &[id, ca] : candidates)
+ for (auto& [id, ca] : candidates)
{
- ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr ;
+ ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr;
}
std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
@@ -129,21 +131,23 @@ namespace armarx
{
bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
ARMARX_ERROR << e.makeMsg(memoryName);
}
- for (auto &[id, ca] : bimanualCandidates)
+ for (auto& [id, ca] : bimanualCandidates)
{
- ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " << ca->groupNr ;
+ ARMARX_INFO << "bimanual candidate with ID " << id << " has group number "
+ << ca->groupNr;
}
m.waitForNextTick();
}
}
- grasping::GraspCandidate GraspProviderExample::makeDummyGraspCandidate()
+ grasping::GraspCandidate
+ GraspProviderExample::makeDummyGraspCandidate()
{
armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
@@ -168,9 +172,11 @@ namespace armarx
return candidate;
}
- grasping::BimanualGraspCandidate GraspProviderExample::makeDummyBimanualGraspCandidate()
+ grasping::BimanualGraspCandidate
+ GraspProviderExample::makeDummyBimanualGraspCandidate()
{
- armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+ armarx::grasping::BimanualGraspCandidate bimanualCandidate =
+ armarx::grasping::BimanualGraspCandidate();
bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
@@ -185,4 +191,4 @@ namespace armarx
return bimanualCandidate;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
index 9f13e111eec8e51f3fa67e5d04f03d3df1fd0b64..c1a2f394afe6b50a6d5f5da71ff7296ec5e42d56 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -21,33 +21,31 @@
*/
-#include "RobotStatePredictionClientExample.h"
-#include "Impl.h"
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "Impl.h"
+#include "RobotStatePredictionClientExample.h"
namespace armarx::robot_state_prediction_client_example
{
- Component::Component() :
- pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ Component::Component() : pimpl(std::make_unique<Impl>())
{
}
-
RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
-
- std::string Component::getDefaultName() const
+ std::string
+ Component::getDefaultName() const
{
return "RobotStatePredictionClientExample";
}
-
- armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
ARMARX_CHECK_NOT_NULL(pimpl);
pimpl->defineProperties(defs, "p.");
@@ -55,13 +53,13 @@ namespace armarx::robot_state_prediction_client_example
return defs;
}
-
- void Component::onInitComponent()
+ void
+ Component::onInitComponent()
{
}
-
- void Component::onConnectComponent()
+ void
+ Component::onConnectComponent()
{
pimpl->connect(memoryNameSystem(), arviz);
pimpl->start();
@@ -70,19 +68,19 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_startRunningTask();
}
-
- void Component::onDisconnectComponent()
+ void
+ Component::onDisconnectComponent()
{
pimpl->stop();
}
-
- void Component::onExitComponent()
+ void
+ Component::onExitComponent()
{
}
-
- void Component::createRemoteGuiTab()
+ void
+ Component::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -90,8 +88,9 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_createTab(getName(), root, &tab);
}
- void Component::RemoteGui_update()
+ void
+ Component::RemoteGui_update()
{
}
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
index c433669630c9631e471666fa3b3cd02acd25c2f9..a303f1c393b549a907bcfa49c1434a26bd873468 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -37,7 +37,6 @@
#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>
@@ -49,7 +48,6 @@ namespace simox::alg
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)
@@ -64,7 +62,6 @@ namespace simox::alg
return map;
}
-
template <class KeyT, class ValueT>
std::vector<ValueT>
multi_at(const std::map<KeyT, ValueT>& map,
@@ -111,15 +108,13 @@ namespace simox::alg
namespace armarx::robot_state_prediction_client_example
{
- Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ Impl::Impl()
{
- client.remote.robotReader.emplace(memoryNameSystem);
+ client.remote.robotReader.emplace();
}
-
Impl::~Impl() = default;
-
void
Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
const std::string& prefix)
@@ -133,7 +128,6 @@ namespace armarx::robot_state_prediction_client_example
client.remote.robotReader->registerPropertyDefinitions(defs);
}
-
void
Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
{
@@ -148,12 +142,11 @@ namespace armarx::robot_state_prediction_client_example
ARMARX_WARNING << e.what();
}
- client.remote.robotReader->connect();
+ client.remote.robotReader->connect(mns);
this->remote.arviz = arviz;
}
-
void
Impl::start()
{
@@ -161,14 +154,12 @@ namespace armarx::robot_state_prediction_client_example
task->start();
}
-
void
Impl::stop()
{
task->stop();
}
-
void
Impl::run()
{
@@ -182,7 +173,6 @@ namespace armarx::robot_state_prediction_client_example
}
}
-
void
Impl::runOnce()
{
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
index 272f9e2e62d6ac16b49a2674a7952d96e8618975..d716f5d94d7502c4c0533485c52ccd2f0a0dc732 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -27,27 +27,25 @@
#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 <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "RobotStatePredictionClient.h"
-
namespace armarx::robot_state_prediction_client_example
{
class Impl
{
public:
-
- Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ Impl();
~Impl();
- void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs,
+ const std::string& prefix);
void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
void start();
@@ -58,7 +56,6 @@ namespace armarx::robot_state_prediction_client_example
public:
-
struct Properties
{
float updateFrequencyHz = 10;
@@ -66,12 +63,14 @@ namespace armarx::robot_state_prediction_client_example
std::string robotName = "Armar6";
float predictAheadSeconds = 1.0;
};
+
Properties properties;
struct Remote
{
viz::Client arviz;
};
+
Remote remote;
armarx::SimpleRunningTask<>::pointer_type task;
@@ -82,6 +81,5 @@ namespace armarx::robot_state_prediction_client_example
std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
std::optional<viz::Robot> robotViz;
-
};
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 75882cf47a238bca9e67a1131829f75c20869ea2..d77d737156239241ed8a62952e788da11c99bb55 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -27,19 +27,19 @@
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
namespace armarx
{
- SkillsMemory::SkillsMemory():
+ SkillsMemory::SkillsMemory() :
ReadWritePluginUser(),
StatechartListenerComponentPluginUser(),
SkillManagerComponentPluginUser(),
@@ -50,10 +50,11 @@ namespace armarx
{
}
-
- armarx::PropertyDefinitionsPtr SkillsMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SkillsMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
const std::string prefix = "mem.";
statechartListenerProviderSegment.defineProperties(defs, prefix + "statechartlistener.");
@@ -64,14 +65,14 @@ namespace armarx
return defs;
}
-
- std::string SkillsMemory::getDefaultName() const
+ std::string
+ SkillsMemory::getDefaultName() const
{
return "SkillMemory";
}
-
- void SkillsMemory::onInitComponent()
+ void
+ SkillsMemory::onInitComponent()
{
statechartListenerProviderSegment.init();
executableSkillCoreSegment.init();
@@ -79,24 +80,23 @@ namespace armarx
skillEventCoreSegment.init();
}
-
- void SkillsMemory::onConnectComponent()
+ void
+ SkillsMemory::onConnectComponent()
{
}
-
- void SkillsMemory::onDisconnectComponent()
+ void
+ SkillsMemory::onDisconnectComponent()
{
-
}
-
- void SkillsMemory::onExitComponent()
+ void
+ SkillsMemory::onExitComponent()
{
}
-
- armem::data::CommitResult SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
+ armem::data::CommitResult
+ SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
{
// This function is overloaded to check for skill executions
// First pass arg to parent func to ensure that data is written into memory and that clients are notified
@@ -107,14 +107,16 @@ namespace armarx
for (const auto& up : commit.updates)
{
- if (up.entityID.coreSegmentName == skills::segment::SkillExecutionRequestCoreSegment::CoreSegmentName)
+ if (up.entityID.coreSegmentName ==
+ skills::segment::SkillExecutionRequestCoreSegment::CoreSegmentName)
{
for (const auto& instance : up.instancesData)
{
ARMARX_CHECK_NOT_NULL(instance);
- skills::manager::dto::SkillExecutionRequest exInfo = skillExecutionRequestCoreSegment.convertCommit(instance);
- SkillManagerComponentPluginUser::executeSkill(exInfo, current);
+ skills::SkillExecutionRequest exInfo =
+ skillExecutionRequestCoreSegment.convertCommit(instance);
+ SkillManagerComponentPluginUser::executeSkill(exInfo.toManagerIce(), current);
}
}
}
@@ -122,52 +124,99 @@ namespace armarx
return result;
}
- void SkillsMemory::addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current ¤t)
+ void
+ SkillsMemory::addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current& current)
{
+ ARMARX_INFO << "Adding provider to skill memory: " << info.providerId.providerName;
SkillManagerComponentPluginUser::addProvider(info, current);
// log skills to memory
- executableSkillCoreSegment.addSkillProvider(info);
+ auto p = skills::ProviderInfo::FromIce(info);
+ executableSkillCoreSegment.addSkillProvider(p);
}
- void SkillsMemory::removeProvider(const std::string& skillProviderName, const Ice::Current ¤t)
+ void
+ SkillsMemory::removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current)
{
- executableSkillCoreSegment.removeSkillProvider(skillProviderName);
+ executableSkillCoreSegment.removeSkillProvider(provider.providerName);
// remove skills from memory
- SkillManagerComponentPluginUser::removeProvider(skillProviderName, current);
+ SkillManagerComponentPluginUser::removeProvider(provider, current);
}
- skills::provider::dto::SkillStatusUpdate SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t)
+ skills::manager::dto::SkillStatusUpdate
+ SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& 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);
- }
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
- skillExecutionRequestCoreSegment.addSkillExecutionRequest(requestCopy);
- return SkillManagerComponentPluginUser::executeSkill(requestCopy, current);
+ return SkillManagerComponentPluginUser::executeSkill(info, current);
}
- void SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t)
+ skills::manager::dto::SkillExecutionID
+ SkillsMemory::executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
{
- skillEventCoreSegment.addSkillUpdateEvent(update);
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
+
+ return SkillManagerComponentPluginUser::executeSkillAsync(info, current);
}
+ void
+ SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
+ const skills::callback::dto::ProviderID& providerId,
+ const Ice::Current& current)
+ {
+ auto p = skills::ProviderID::FromIce(providerId);
+ auto u = skills::SkillStatusUpdate::FromIce(update, p);
+ skillEventCoreSegment.addSkillUpdateEvent(u);
+ }
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ SkillsMemory::getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current)
+ {
+ auto eid = skills::SkillExecutionID::FromIce(executionId);
+ auto op = this->skillEventCoreSegment.getSkillStatusUpdate(eid);
+ if (op.has_value())
+ {
+ return op->toManagerIce();
+ }
+ return {};
+ }
+
+ skills::manager::dto::SkillStatusUpdateMap
+ SkillsMemory::getSkillExecutionStatuses(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillStatusUpdateMap ret;
+ auto updates = this->skillEventCoreSegment.getSkillStatusUpdates();
+ for (const auto& [k, v] : updates)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+ return ret;
+ }
/*
* Statechart stuff
*/
- void SkillsMemory::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& x, const Ice::Current&)
+ void
+ SkillsMemory::reportStatechartTransitionWithParameters(
+ const ProfilerStatechartTransitionWithParameters& x,
+ const Ice::Current&)
{
statechartListenerProviderSegment.reportStatechartTransitionWithParameters(x);
}
- void SkillsMemory::reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList& x, const Ice::Current&)
+ void
+ SkillsMemory::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& x,
+ const Ice::Current&)
{
statechartListenerProviderSegment.reportStatechartTransitionWithParametersList(x);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index 91506c16fb87793083328d173d973f19ad268e75..ead9f19122bc80266196aff40f138b4bd87c77f1 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -30,16 +30,14 @@
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
-
-#include <RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h>
-#include <RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
-#include <RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h>
#include <RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h>
-#include <RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h>
#include <RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h>
+#include <RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h>
namespace armarx
{
@@ -63,27 +61,51 @@ namespace armarx
virtual public SkillManagerComponentPluginUser
{
public:
-
SkillsMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
// Override StatechartListener
- void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
- void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+ void
+ reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&,
+ const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList&,
+ const Ice::Current&) override;
// 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;
- 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;
+ void addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current& current) override;
+
+ void removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdate
+ executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current) override;
+
+
+ skills::manager::dto::SkillExecutionID
+ executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current) override;
+
+ void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
+ const skills::callback::dto::ProviderID& id,
+ const Ice::Current& current) override;
+
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(const Ice::Current& current) override;
// WritingInterface interface
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit,
+ const Ice::Current&) override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -101,12 +123,12 @@ namespace armarx
private:
-
static constexpr const char* MemoryName = "Skill";
struct Properties
{
};
+
Properties p;
skills::segment::StatechartListenerProviderSegment statechartListenerProviderSegment;
@@ -114,4 +136,4 @@ namespace armarx
skills::segment::SkillEventCoreSegment skillEventCoreSegment;
skills::segment::SkillExecutionRequestCoreSegment skillExecutionRequestCoreSegment;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index afbb7ef2abd07d6e718c7480ba7d84455574041d..b13f663e25b6c31fbb4ab3b919ce81431d617b70 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -13,10 +13,20 @@ set(COMPONENT_LIBS
set(SOURCES
SkillProviderExample.cpp
+ HelloWorld.cpp
+ Incomplete.cpp
+ Chaining.cpp
+ Callback.cpp
+ Timeout.cpp
)
set(HEADERS
SkillProviderExample.h
+ HelloWorld.h
+ Incomplete.h
+ Chaining.h
+ Callback.h
+ Timeout.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6dcea52a55191f03716aea507d13cd6c831d8d87
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
@@ -0,0 +1,37 @@
+
+#include "Callback.h"
+
+namespace armarx::skills::provider
+{
+ CallbackSkill::CallbackSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ CallbackSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = skills::SkillID{.skillName = "ShowMeCallbacks"},
+ .description = "This skill does shows callbacks",
+ .timeout = armarx::core::time::Duration::MilliSeconds(1000)};
+ }
+
+ 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"));
+
+ in.callback(skills::SkillStatus::Running, up1);
+
+ auto up2 = std::make_shared<aron::data::Dict>();
+ up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
+ in.callback(skills::SkillStatus::Running, up2);
+
+ auto up3 = std::make_shared<aron::data::Dict>();
+ up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
+ in.callback(skills::SkillStatus::Running, up3);
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.h b/source/RobotAPI/components/skills/SkillProviderExample/Callback.h
new file mode 100644
index 0000000000000000000000000000000000000000..c29c2b6f3d6007b1fda30b0c781b91be03a15232
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.h
@@ -0,0 +1,41 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class CallbackSkill : public SimpleSkill
+ {
+ public:
+ CallbackSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..983902512c03bc28c8d4429fd22c4690aeb14833
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
@@ -0,0 +1,40 @@
+
+
+#include "Chaining.h"
+
+namespace armarx::skills::provider
+{
+
+ ChainingSkill::ChainingSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ ChainingSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "ChainingSkill"},
+ .description =
+ "This skill calls the Timeout skill three times. The last "
+ "execution is aborted due to a timeout of this skill.",
+ .timeout = armarx::core::time::Duration::MilliSeconds(5000)};
+ }
+
+ Skill::MainResult
+ ChainingSkill::main(const MainInput& in)
+ {
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Timeout"});
+
+ ARMARX_INFO << "CALL PROXY FIRST TIME";
+ callSubskill(prx);
+ ARMARX_INFO << "CALL PROXY SECOND TIME";
+ callSubskill(prx);
+ ARMARX_INFO << "CALL PROXY THIRD TIME";
+ callSubskill(prx);
+
+ this->throwIfSkillShouldTerminate();
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h
new file mode 100644
index 0000000000000000000000000000000000000000..991c4a5aaab5d1be83f9684ca5bdbca1638b946d
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h
@@ -0,0 +1,40 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class ChainingSkill : public SimpleSkill
+ {
+ public:
+ ChainingSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6779f1d6a7fb872b80d56cf9547aaceb560218d7
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -0,0 +1,50 @@
+
+
+#include "HelloWorld.h"
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
+
+namespace armarx::skills::provider
+{
+ HelloWorldSkill::HelloWorldSkill() :
+ SimpleSpecializedSkill<skills::Example::HelloWorldAcceptedType>(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ HelloWorldSkill::GetSkillDescription()
+ {
+ armarx::skills::Example::HelloWorldAcceptedType root_profile_params;
+ root_profile_params.some_float = 5;
+ root_profile_params.some_int = 42;
+ root_profile_params.some_text = "YOLO";
+ root_profile_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
+ root_profile_params.some_matrix = Eigen::Matrix3f::Zero();
+
+ return SkillDescription{.skillId = skills::SkillID{.skillName = "HelloWorld"},
+ .description = "This skill logs a message on ARMARX_IMPORTANT",
+ .rootProfileDefaults = root_profile_params.toAron(),
+ .timeout = armarx::core::time::Duration::MilliSeconds(1000),
+ .resultType =
+ armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+ }
+
+ Skill::MainResult
+ HelloWorldSkill::main(const SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n"
+ << "I received the following data: \n"
+ << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
+ in.parameters.toAron())
+ .dump(2)
+ << "\n"
+ << "Type fulfilled? "
+ << parameters->fullfillsType(
+ armarx::skills::Example::HelloWorldAcceptedType::ToAronType())
+ << "\n"
+ << "(executed at: " << IceUtil::Time::now() << ")";
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h
new file mode 100644
index 0000000000000000000000000000000000000000..840b058fb171685db777126157fc21028db89c99
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h
@@ -0,0 +1,42 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+namespace armarx::skills::provider
+{
+ // Skills:
+ class HelloWorldSkill : public SimpleSpecializedSkill<skills::Example::HelloWorldAcceptedType>
+ {
+ public:
+ HelloWorldSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const SpecializedMainInput& in) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..264876a7e0c8094b38b6032a32e115dbbb336c18
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
@@ -0,0 +1,54 @@
+
+
+#include "Incomplete.h"
+
+#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
+
+#include "HelloWorld.h"
+
+namespace armarx::skills::provider
+{
+
+ IncompleteSkill::IncompleteSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ IncompleteSkill::GetSkillDescription()
+ {
+ auto d = HelloWorldSkill::GetSkillDescription();
+ return SkillDescription{.skillId = {.skillName = "IncompleteSkill"},
+ .description = d.description,
+ .timeout = d.timeout + armarx::core::time::Duration::Seconds(2),
+ .parametersType = d.parametersType};
+ }
+
+ Skill::PrepareResult
+ IncompleteSkill::prepare()
+ {
+ if (!first_prepared)
+ {
+ first_prepared = true;
+
+ // set parameters after two seconds...
+ std::thread foo(
+ [&]()
+ {
+ auto d = HelloWorldSkill::GetSkillDescription();
+ std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+ this->setParameters(d.rootProfileDefaults);
+ });
+ foo.detach();
+ }
+
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ IncompleteSkill::main(const MainInput& in)
+ {
+ auto s = HelloWorldSkill();
+ s.setParameters(in.parameters);
+ return s.mainOfSkill();
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h
new file mode 100644
index 0000000000000000000000000000000000000000..6cf7b6e73b597b201d9d5213db160151d3a971c8
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h
@@ -0,0 +1,44 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class IncompleteSkill : public SimpleSkill
+ {
+ public:
+ IncompleteSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::PrepareResult prepare() final;
+ Skill::MainResult main(const MainInput&) final;
+
+ std::atomic_bool first_prepared = false;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 65b94a3807343d850003a53bbdbabc438a2d4bcc..5ab480cd5c05556e2c7ea486ac0ec3976b1eb6df 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -1,140 +1,8 @@
-
#include "SkillProviderExample.h"
-#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
-
namespace armarx::skills::provider
{
- HelloWorldSkill::HelloWorldSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- HelloWorldSkill::GetSkillDescription()
- {
- armarx::skills::Example::HelloWorldAcceptedType default_params;
- default_params.some_float = 5;
- default_params.some_int = 42;
- default_params.some_text = "YOLO";
- default_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
- //default_params.some_matrix = Eigen::Matrix3f::Zero();
-
- return SkillDescription{"HelloWorld",
- "This skill logs a message on ARMARX_IMPORTANT",
- {},
- armarx::core::time::Duration::MilliSeconds(1000),
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
- default_params.toAron()};
- }
-
- Skill::MainResult
- HelloWorldSkill::main(const MainInput& in)
- {
- ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n"
- << "I received the following data: \n"
- << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
- in.params)
- .dump(2)
- << "\n"
- << "(executed at: " << IceUtil::Time::now() << ")";
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
- ChainingSkill::ChainingSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- ChainingSkill::GetSkillDescription()
- {
- return SkillDescription{"ChainingSkill",
- "This skill calls the HelloWorld skill three times.",
- {},
- armarx::core::time::Duration::MilliSeconds(3000),
- nullptr};
- }
-
- Skill::MainResult
- ChainingSkill::main(const MainInput& in)
- {
- armarx::skills::Example::HelloWorldAcceptedType exec1;
- armarx::skills::Example::HelloWorldAcceptedType exec2;
- armarx::skills::Example::HelloWorldAcceptedType exec3;
-
- exec1.some_text = "Hello from the ChainingSkill 1";
- exec2.some_text = "Hello from the ChainingSkill 2";
- exec3.some_text = "Hello from the ChainingSkill 3";
-
- SkillProxy skillExecPrx(manager, {"SkillProviderExample", "HelloWorld"});
-
- skillExecPrx.executeFullSkill(getSkillId().toString(), exec1.toAron());
- skillExecPrx.executeFullSkill(getSkillId().toString(), exec2.toAron());
- skillExecPrx.executeFullSkill(getSkillId().toString(), exec3.toAron());
-
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
- TimeoutSkill::TimeoutSkill() :
- PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
- {
- }
-
- SkillDescription
- TimeoutSkill::GetSkillDescription()
- {
- return SkillDescription{"Timeout",
- "This fails with timeout reached",
- {},
- armarx::core::time::Duration::MilliSeconds(1000),
- nullptr};
- }
-
- PeriodicSkill::StepResult
- TimeoutSkill::step(const MainInput& in)
- {
- // do heavy work
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
-
- return {ActiveOrTerminatedSkillStatus::Running, nullptr};
- }
-
- CallbackSkill::CallbackSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- CallbackSkill::GetSkillDescription()
- {
- return SkillDescription{"ShowMeCallbacks",
- "This skill does shows callbacks",
- {},
- armarx::core::time::Duration::MilliSeconds(1000),
- nullptr};
- }
-
- 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"));
- in.callback(up1);
-
- auto up2 = std::make_shared<aron::data::Dict>();
- up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
- in.callback(up2);
-
- auto up3 = std::make_shared<aron::data::Dict>();
- up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
- in.callback(up3);
-
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
SkillProviderExample::SkillProviderExample() : SkillProviderComponentPluginUser()
{
}
@@ -157,51 +25,38 @@ namespace armarx::skills::provider
SkillProviderExample::onInitComponent()
{
// Add example skill
- addSkill(std::make_unique<HelloWorldSkill>());
-
- // Add another lambda example skill
- {
- skills::SkillDescription fooDesc;
- fooDesc.acceptedType = nullptr; // accept everything
- fooDesc.description = "This skill does exactly nothing.";
- fooDesc.skillName = "Foo";
- 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);
- }
+ ARMARX_INFO << "Adding skill HelloWorldSkill";
+ addSkillFactory<HelloWorldSkill>();
// 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);
+ skills::SkillDescription fooDesc{.skillId = SkillID{.skillName = "Foo"},
+ .description = "This skill does exactly nothing.",
+ .timeout =
+ armarx::core::time::Duration::MilliSeconds(1000)};
+ addSkillFactory(fooDesc,
+ []()
+ {
+ std::cout << "Hello from Foo." << std::endl;
+ return TerminatedSkillStatus::Succeeded;
+ });
}
// Add another example skill
- addSkill(std::make_unique<CallbackSkill>());
+ ARMARX_INFO << "Adding skill CallbackSkill";
+ addSkillFactory<CallbackSkill>();
// Add timeout skill
- addSkill(std::make_unique<TimeoutSkill>());
+ ARMARX_INFO << "Adding skill TimeoutSkill";
+ addSkillFactory<TimeoutSkill>();
// chaining
- addSkill(std::make_unique<ChainingSkill>());
+ ARMARX_INFO << "Adding skill ChainingSkill";
+ addSkillFactory<ChainingSkill>();
+
+ // incomplete and prepare
+ ARMARX_INFO << "Adding skill IncompleteSkill";
+ addSkillFactory<IncompleteSkill>();
}
void
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 2aad033f1f00a2523dd8dcf15b03979eab92b339..b8391b0881f625af03b00b601983251c4d90558b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -28,55 +28,15 @@
// RobotAPI
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
-#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
+
+#include "Callback.h"
+#include "Chaining.h"
+#include "HelloWorld.h"
+#include "Incomplete.h"
+#include "Timeout.h"
namespace armarx::skills::provider
{
- // Skills:
- class HelloWorldSkill : public Skill
- {
- public:
- HelloWorldSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main(const MainInput& in) final;
- };
-
- class ChainingSkill : public Skill
- {
- public:
- ChainingSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main(const MainInput& in) final;
- };
-
- class TimeoutSkill : public PeriodicSkill
- {
- public:
- TimeoutSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- PeriodicSkill::StepResult step(const MainInput& in) final;
- };
-
- class CallbackSkill : public Skill
- {
- public:
- CallbackSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main(const MainInput& in) final;
- };
-
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31dc03ae05caf0e2be256487e3a6a4294243e9d0
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
@@ -0,0 +1,28 @@
+
+#include "Timeout.h"
+
+namespace armarx::skills::provider
+{
+
+ TimeoutSkill::TimeoutSkill() :
+ PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
+ {
+ }
+
+ SkillDescription
+ TimeoutSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = SkillID{.skillName = "Timeout"},
+ .description = "This fails with timeout reached",
+ .timeout = armarx::core::time::Duration::MilliSeconds(2000)};
+ }
+
+ PeriodicSkill::StepResult
+ TimeoutSkill::step()
+ {
+ // do heavy work
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+ return {ActiveOrTerminatedSkillStatus::Running, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
new file mode 100644
index 0000000000000000000000000000000000000000..347e216a956c22d8d38b3b0b434ec0b5f4cf0225
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
@@ -0,0 +1,42 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/PeriodicSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class TimeoutSkill : public PeriodicSkill
+ {
+ public:
+ TimeoutSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ PeriodicSkill::StepResult step() final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 41ffefe98882f69004610a233dee16614f5b4f60..b434d1e3983096de91e73ddd2319fc843d3504a0 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -24,45 +24,51 @@
#include "GraspCandidateObserver.h"
//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
using namespace armarx;
using namespace armarx::grasping;
-GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
+GraspCandidateObserver::GraspCandidateObserver()
{
}
-void GraspCandidateObserver::onInitObserver()
+void
+GraspCandidateObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
-
-
}
-void GraspCandidateObserver::onConnectObserver()
+void
+GraspCandidateObserver::onConnectObserver()
{
- configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
- graspCandidateWriter.connect();
+ configTopic = getTopic<GraspCandidateProviderInterfacePrx>(
+ getProperty<std::string>("ConfigTopicName").getValue());
+ graspCandidateWriter.connect(memoryNameSystem());
}
-PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
+PropertyDefinitionsPtr
+GraspCandidateObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new GraspCandidateObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new GraspCandidateObserverPropertyDefinitions(getConfigIdentifier()));
}
-bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
+bool
+GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter,
+ const std::string& providerName,
+ const GraspCandidatePtr& candidate)
{
if (filter->providerName != "*" && filter->providerName != providerName)
{
@@ -72,11 +78,13 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
{
return false;
}
- if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+ if (filter->approach != AnyApproach &&
+ (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
{
return false;
}
- if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+ if (filter->preshape != AnyAperture &&
+ (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
{
return false;
}
@@ -87,7 +95,8 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
return true;
}
-std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
+std::string
+GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
{
switch (type)
{
@@ -102,7 +111,8 @@ std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
}
}
-void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
+void
+GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
{
if (updateCounters.count(providerName) == 0)
{
@@ -118,26 +128,40 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
{
offerChannel(providerName, "Channel of " + providerName);
}
- offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
- offerOrUpdateDataField(providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
+ offerOrUpdateDataField(providerName,
+ "updateCounter",
+ Variant(updateCounters[providerName]),
+ "Counter that increases for each update");
+ offerOrUpdateDataField(
+ providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
}
-void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportGraspCandidates(const std::string& providerName,
+ const GraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
this->candidates[providerName] = candidates;
- graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::Now(), providerName);
+ graspCandidateWriter.commitGraspCandidateSeq(
+ candidates, armarx::armem::Time::Now(), providerName);
handleProviderUpdate(providerName, candidates.size());
}
-void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName,
+ const BimanualGraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
this->bimanualCandidates[providerName] = candidates;
handleProviderUpdate(providerName, candidates.size());
}
-void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
+void
+GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
+ const ProviderInfoPtr& info,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
providers[providerName] = info;
@@ -154,36 +178,37 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
}
-InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+InfoMap
+GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
return providers;
}
-StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
return getAvailableProviderNames();
}
-
-
-ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ProviderInfoPtr
+GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return providers[providerName];
}
-bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
return hasProvider(providerName);
}
-
-
-GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getAllCandidates(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
@@ -194,11 +219,16 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
return all;
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName,
+ const Ice::Current& c)
{
return getCandidatesByProviders(Ice::StringSeq{providerName});
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c)
+
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
@@ -213,7 +243,9 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::St
return all;
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq matching;
@@ -230,20 +262,26 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
return matching;
}
-Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
+Ice::Int
+GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return updateCounters[providerName];
}
-IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
+IntMap
+GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
{
std::unique_lock lock(dataMutex);
return updateCounters;
}
-bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+bool
+GraspCandidateObserver::setProviderConfig(const std::string& providerName,
+ const StringVariantBaseMap& config,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
if (providers.count(providerName) == 0)
@@ -254,19 +292,23 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
return true;
}
-void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
selectedCandidates = candidates;
}
-GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
return selectedCandidates;
}
-BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
BimanualGraspCandidateSeq all;
@@ -277,19 +319,25 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
return all;
}
-void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedBimanualCandidates(
+ const grasping::BimanualGraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
selectedBimanualCandidates = candidates;
}
-BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
return selectedBimanualCandidates;
}
-void GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName, const Ice::Current&)
+void
+GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -299,18 +347,24 @@ void GraspCandidateObserver::clearCandidatesByProvider(const std::string& provid
}
}
-bool GraspCandidateObserver::hasProvider(const std::string& providerName)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName)
{
return providers.count(providerName) > 0;
}
-void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
+
+void
+GraspCandidateObserver::checkHasProvider(const std::string& providerName)
{
if (!hasProvider(providerName))
{
- throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
+ throw LocalException("Unknown provider name '")
+ << providerName << "'. Available providers: " << getAvailableProviderNames();
}
}
-StringSeq GraspCandidateObserver::getAvailableProviderNames()
+
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames()
{
StringSeq names;
for (const auto& pair : providers)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index b40d5a2fcf9c480a71f0576d049ad0a311813815..a6d2ff63c1462557b5c55fc9538228dc46509ef1 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -63,6 +63,7 @@ set(COMPONENT_LIBS
RobotAPIInterfaces
aron
RobotAPISkills
+ SkillsMemory
aronjsonconverter
SimpleConfigDialog
)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 7c25a1b848daab3347a7247199aa9a54eefb99bc..b96d3850e2c7181892e22030c600157963fb41af 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -20,8 +20,38 @@
<string>SkillManagerMonitorWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
- <item row="0" column="0">
+ <item row="3" column="0">
+ <widget class="QCheckBox" name="checkBoxAutoUpdate">
+ <property name="text">
+ <string>Auto Update</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QPushButton" name="pushButtonRefreshNow">
+ <property name="text">
+ <string>Refresh Now</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Update Frequency:</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0" colspan="4">
<widget class="QSplitter" name="splitter_2">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
@@ -34,25 +64,59 @@
<property name="childrenCollapsible">
<bool>false</bool>
</property>
- <widget class="QGroupBox" name="groupBoxActiveSkills">
+ <widget class="QGroupBox" name="groupBoxSkillExecutions">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
- <string>Active Skills</string>
+ <string>Executions</string>
</property>
<layout class="QGridLayout" name="gridLayout_4">
- <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>
+ <item row="1" column="0" colspan="3">
+ <widget class="QTreeWidget" name="treeWidgetSkillExecutions">
+ <column>
+ <property name="text">
+ <string>ExecutionID</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Executor</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>SkillID</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsConstructing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsInitializing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsPreparing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsRunning</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Finished</string>
+ </property>
+ </column>
</widget>
</item>
</layout>
@@ -78,17 +142,7 @@
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <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="4" column="0" colspan="3">
+ <item row="5" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
<property name="text">
@@ -97,33 +151,36 @@
</column>
<column>
<property name="text">
- <string>HasType</string>
+ <string>HasInputType</string>
</property>
</column>
<column>
<property name="text">
- <string>State</string>
+ <string>HasOutputType</string>
</property>
</column>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QCheckBox" name="autoUpdateCheckBox">
+ <item row="4" column="1">
+ <widget class="QPushButton" name="pushButtonSearch">
<property name="text">
- <string>Auto Update</string>
+ <string>Search</string>
</property>
</widget>
</item>
- <item row="2" column="2">
- <widget class="QPushButton" name="refreshNowPushButton">
+ <item row="4" column="0">
+ <widget class="QLineEdit" name="lineEditSearch">
<property name="text">
- <string>Refresh Now</string>
+ <string>Search...</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBoxSkillDetails">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>0</horstretch>
@@ -134,7 +191,28 @@
<string>Skill Details</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0" colspan="4">
+ <item row="8" column="3">
+ <widget class="QPushButton" name="pushButtonExecuteSkill">
+ <property name="text">
+ <string>Request Execution</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
+ <property name="text">
+ <string>Set args from clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset args to profile</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0" colspan="4">
<widget class="QTreeWidget" name="treeWidgetSkillDetails">
<property name="contextMenuPolicy">
<enum>Qt::CustomContextMenu</enum>
@@ -159,39 +237,11 @@
</column>
<column>
<property name="text">
- <string>defaultValue</string>
+ <string>defaultValue (hidden in GUI)</string>
</property>
</column>
</widget>
</item>
- <item row="6" column="3">
- <widget class="QPushButton" name="pushButtonExecuteSkill">
- <property name="text">
- <string>Request Execution</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonPaste">
- <property name="text">
- <string>Set from clipboard</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QPushButton" name="pushButtonStopSkill">
- <property name="text">
- <string>Stop current skill</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset args</string>
- </property>
- </widget>
- </item>
<item row="0" column="1">
<widget class="QPushButton" name="pushButtonCopy">
<property name="text">
@@ -199,6 +249,15 @@
</property>
</widget>
</item>
+ <item row="1" column="1" colspan="3">
+ <widget class="QComboBox" name="comboBoxProfiles">
+ <item>
+ <property name="text">
+ <string><No Profile selected. Using root></string>
+ </property>
+ </item>
+ </widget>
+ </item>
</layout>
</widget>
</widget>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index f9fa41252ad6a0f1144bf7eb043f62771061b5fc..191c160194207e561a1be7582c038361299a81ac 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -24,7 +24,7 @@
#include <string>
-#include <RobotAPI/libraries/skills/provider/Skill.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
@@ -38,10 +38,11 @@
#include <QDoubleSpinBox>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include "aronTreeWidget/Data.h"
-//config
+//configSk
namespace armarx
{
QPointer<QDialog>
@@ -50,8 +51,8 @@ namespace armarx
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>(
- "SkillManager", "", "Skill*");
+ dialog->addProxyFinder<skills::dti::SkillMemoryInterfacePrx>(
+ "SkillMemory", "", "SkillMem*");
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
@@ -59,37 +60,60 @@ namespace armarx
void
SkillManagerMonitorWidgetController::configured()
{
- observerName = dialog->getProxyName("SkillManager");
+ observerName = dialog->getProxyName("SkillMemory");
}
void
SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
{
- observerName = settings->value("SkillManager", "SkillManager").toString().toStdString();
+ observerName = settings->value("SkillMemory", "SkillMemory").toString().toStdString();
}
void
SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("SkillManager", QString::fromStdString(observerName));
+ settings->setValue("SkillMemory", QString::fromStdString(observerName));
}
} // namespace armarx
// Others
namespace armarx
{
+ SkillExecutionInfoTreeWidgetItem*
+ SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(
+ SkillExecutionInfoTreeWidgetItem* haystack,
+ const skills::SkillExecutionID& needle)
+ {
+ if (!haystack)
+ {
+ return nullptr;
+ }
+
+ if (needle == haystack->executionId)
+ {
+ return haystack;
+ }
+ for (int i = 0; i < haystack->childCount(); ++i)
+ {
+ auto el = static_cast<SkillExecutionInfoTreeWidgetItem*>(haystack->child(i));
+ return SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(el, needle);
+ }
+ return nullptr;
+ }
+
SkillManagerMonitorWidgetController::SkillManagerMonitorWidgetController()
{
widget.setupUi(getWidget());
- widget.doubleSpinBoxUpdateFreq->setValue(5.0);
- widget.doubleSpinBoxUpdateFreq->setMinimum(0);
- widget.doubleSpinBoxUpdateFreq->setMaximum(100);
+ double default_hz = 2;
+ widget.doubleSpinBoxUpdateFreq->setValue(default_hz);
+ widget.doubleSpinBoxUpdateFreq->setMinimum(0.5);
+ widget.doubleSpinBoxUpdateFreq->setMaximum(20);
widget.doubleSpinBoxUpdateFreq->setSingleStep(0.5);
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
refreshSkillsResultTimer = new QTimer(this);
- refreshSkillsResultTimer->setInterval(1000 / 2); // Keep this stable.
+ updateTimerFrequency();
refreshSkillsResultTimer->start();
connect(widget.doubleSpinBoxUpdateFreq,
@@ -99,7 +123,7 @@ namespace armarx
connect(refreshSkillsResultTimer,
&QTimer::timeout,
this,
- &SkillManagerMonitorWidgetController::refreshSkillsPeriodically);
+ &SkillManagerMonitorWidgetController::refreshSkillsAndExecutions);
connect(widget.pushButtonCopy,
&QPushButton::clicked,
@@ -114,17 +138,17 @@ namespace armarx
&QPushButton::clicked,
this,
&SkillManagerMonitorWidgetController::executeSkill);
- connect(widget.pushButtonStopSkill,
- &QPushButton::clicked,
- this,
- &SkillManagerMonitorWidgetController::stopSkill);
+ // connect(widget.pushButtonStopSkill,
+ // &QPushButton::clicked,
+ // this,
+ // &SkillManagerMonitorWidgetController::stopSkill);
connect(widget.treeWidgetSkills,
&QTreeWidget::currentItemChanged,
this,
&SkillManagerMonitorWidgetController::skillSelectionChanged);
- connect(widget.refreshNowPushButton,
+ connect(widget.pushButtonRefreshNow,
&QPushButton::clicked,
this,
&SkillManagerMonitorWidgetController::refreshSkills);
@@ -148,7 +172,7 @@ namespace armarx
QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
- getProxy(manager, observerName, 1000);
+ getProxy(memory, observerName, 1000);
connected = true;
}
@@ -156,15 +180,15 @@ namespace armarx
SkillManagerMonitorWidgetController::onDisconnectComponent()
{
connected = false;
- manager = nullptr;
+ memory = nullptr;
// reset all
skills.clear();
widget.treeWidgetSkills->clear();
widget.treeWidgetSkillDetails->clear();
skillsArgumentsTreeWidgetItem = nullptr;
- selectedSkill.providerName = "";
- selectedSkill.skillName = "";
+ selectedSkill.skillId.providerId->providerName = "";
+ selectedSkill.skillId.skillName = "";
}
void
@@ -175,28 +199,21 @@ namespace armarx
}
void
- SkillManagerMonitorWidgetController::refreshSkillsPeriodically()
+ SkillManagerMonitorWidgetController::refreshSkillsAndExecutions()
{
- if (widget.autoUpdateCheckBox->isChecked())
+ if (widget.checkBoxAutoUpdate->isChecked())
{
refreshSkills();
+ refreshExecutions();
}
}
void
SkillManagerMonitorWidgetController::refreshSkills()
{
- static std::map<skills::provider::dto::Execution::Status, std::string>
- ExecutionStatus2String = {
- {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {skills::provider::dto::Execution::Status::Failed, "Failed"},
- {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
- {skills::provider::dto::Execution::Status::Running, "Running"},
- {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}};
-
- if (!manager)
+ if (!memory)
{
+ // check if null
return;
}
@@ -204,149 +221,210 @@ namespace armarx
// remove non-existing ones
try
{
- 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();)
- {
- // 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++;
- }
- }
+ std::scoped_lock l(updateMutex);
- // add new ones
- ARMARX_DEBUG << "GET NEW PROVIDERS AND ADD THEM TO MAP";
- std::vector<std::string> newProviders;
- for (const auto& [providerName, providerSkills] : managerSkills)
+ auto managerSkills = memory->getSkillDescriptions();
+
+ // completely recreate internal skills map
+ skills.clear();
+ for (const auto& [sid, desc] : managerSkills)
{
- if (skills.find(providerName) == skills.end())
- {
- ARMARX_DEBUG << "ADD " << providerName;
- skills.insert(std::make_pair(providerName, providerSkills));
- newProviders.push_back(providerName);
- }
+ auto description = skills::SkillDescription::FromIce(desc);
+ auto skillId = skills::SkillID::FromIce(sid);
+ auto providerId = skillId.providerId.value_or(
+ skills::ProviderID{.providerName = "UNKNOWN PROVIDER NAME"});
+
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ auto& providedSkillsMap = skills[providerId]; // create new if not existent
+ providedSkillsMap.insert({skillId, description});
}
- /* CHECK TREE VIEW */
- // remove providers from tree
- ARMARX_DEBUG << "REMOVE PROVIDERS FROM TREE VIEW";
+ // update tree view. Remove non-existing elements
int i = 0;
while (i < widget.treeWidgetSkills->topLevelItemCount())
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (auto it = std::find(removedProviders.begin(),
- removedProviders.end(),
- item->text(0).toStdString());
- it != removedProviders.end())
+ auto* providerItem = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = providerItem->text(0).toStdString();
+ skills::ProviderID providerId{.providerName = providerName};
+
+ if (skills.find(providerId) == skills.end())
{
- ARMARX_DEBUG << "REMOVE PROVIDER " << *it;
- delete widget.treeWidgetSkills->takeTopLevelItem(i);
+ providerItem = nullptr; // reset
+ auto remove = widget.treeWidgetSkills->takeTopLevelItem(i);
+ delete remove;
+ continue;
}
- else
+
+ ++i;
+ auto& providedSkills = skills.at(providerId);
+
+ int j = 0;
+ while (j < providerItem->childCount())
{
- ++i;
+ auto* skillItem = providerItem->child(j);
+ auto skillName = skillItem->text(0).toStdString();
+
+ skills::SkillID skillId{.providerId =
+ skills::ProviderID{.providerName = providerName},
+ .skillName = skillName};
+
+ if (providedSkills.find(skillId) == providedSkills.end())
+ {
+ skillItem = nullptr;
+ auto remove = providerItem->takeChild(j);
+ delete remove;
+ continue;
+ }
+
+ ++j;
}
}
- // add new providers
- ARMARX_DEBUG << "ADD NEW PROVIDERS TO TREE VIEW";
- for (const auto& [providerName, providerSkills] : skills)
+ // update tree view. Add new elements
+ for (const auto& [providerId, providedSkills] : skills)
{
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
- it != newProviders.end())
+ QTreeWidgetItem* providerItem = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
- auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
- item->setText(0, QString::fromStdString(providerName));
- for (const auto& [name, sk] : providerSkills)
+ auto el = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = el->text(0).toStdString();
+ skills::ProviderID elProviderId{.providerName = providerName};
+
+ if (providerId == elProviderId)
{
- ARMARX_DEBUG << "ADD PROVIDER " << *it;
- auto itsk = new QTreeWidgetItem(item);
- item->addChild(itsk);
- itsk->setText(0, QString::fromStdString(name));
+ providerItem = el;
+ break;
}
}
- }
- // 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)
- {
- try
+ if (!providerItem)
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- auto providerName = item->text(0).toStdString();
-
- ARMARX_DEBUG << "UPDATE STATI FOR PROVIDER " << providerName;
- auto allStatusesForProvider = managerStatuses.at(providerName);
+ providerItem = new QTreeWidgetItem(widget.treeWidgetSkills);
+ providerItem->setText(0, QString::fromStdString(providerId.providerName));
+ }
- for (int j = 0; j < item->childCount(); ++j)
+ for (const auto& [skillId, skill] : providedSkills)
+ {
+ QTreeWidgetItem* skillItem = nullptr;
+ for (int i = 0; i < providerItem->childCount(); ++i)
{
- 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
+ auto el = providerItem->child(i);
+ auto skillName = el->text(0).toStdString();
+ skills::SkillID elSkillId{providerId, skillName};
+
+ if (skillId == elSkillId)
{
- ARMARX_DEBUG << "ADD SKILL TO ACTIVE " << currentSkillId.skillName;
- activeSkillsAndPrefixes.insert(
- {currentSkillId, statusForSkill.header.executorName});
+ skillItem = el;
+ break;
}
}
- }
- catch (...)
- {
- // Perhaps the skill provider died after the check at the beginning of this method
- continue;
+
+ if (!skillItem)
+ {
+ skillItem = new SkillInfoTreeWidgetItem(skill, providerItem);
+ skillItem->setText(0, QString::fromStdString(skillId.skillName));
+ }
}
}
+ }
+ catch (...)
+ {
+ // perhaps the manager died during the method?
+ }
+ }
+
+ void
+ SkillManagerMonitorWidgetController::refreshExecutions()
+ {
+ static std::map<skills::SkillStatus, std::string> ExecutionStatus2String = {
+ // Terminating
+ {skills::SkillStatus::Aborted, "Aborted"},
+ {skills::SkillStatus::Failed, "Failed"},
+ {skills::SkillStatus::Succeeded, "Succeeded"},
+
+ // Others
+ {skills::SkillStatus::Constructing, "Under construction"},
+ {skills::SkillStatus::Running, "Running"},
+ {skills::SkillStatus::Initializing, "Initializing"},
+ {skills::SkillStatus::Preparing, "Preparing"}};
+
+ if (!memory)
+ {
+ // check if null
+ return;
+ }
+
+ try
+ {
+ std::scoped_lock l(updateMutex);
- // finally update the view of active skills
- ARMARX_DEBUG << "UPDATE ACTIVE SKILLS";
- widget.listWidgetActiveSkills->clear();
- for (const auto& [id, prefix] : activeSkillsAndPrefixes)
+ auto currentManagerStatuses =
+ memory
+ ->getSkillExecutionStatuses(); // we assume that there are no more than 100 new skills..
+
+ for (const auto& [k, v] : currentManagerStatuses)
{
- auto prefixedStr = id.toString(prefix);
- bool longest = true;
- for (
- const auto& [id2, prefix2] :
- activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ auto executionId = skills::SkillExecutionID::FromIce(k);
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);
+
+ SkillExecutionInfoTreeWidgetItem* found = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkillExecutions->topLevelItemCount(); ++i)
{
- auto prefixedStr2 = id.toString(prefix2);
- if (prefixedStr == prefixedStr2)
- {
- continue;
- }
+ auto c = static_cast<SkillExecutionInfoTreeWidgetItem*>(
+ widget.treeWidgetSkillExecutions->topLevelItem(i));
+
+ found =
+ SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(c, executionId);
- if (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ if (found)
{
- longest = false;
+ // update values
+ found->setText(3,
+ QString::fromStdString(
+ statusUpdate.hasBeenConstructed() ? " yes " : " no "));
+ found->setText(4,
+ QString::fromStdString(
+ statusUpdate.hasBeenInitialized() ? " yes " : " no "));
+ found->setText(5,
+ QString::fromStdString(
+ statusUpdate.hasBeenPrepared() ? " yes " : " no "));
+ found->setText(6,
+ QString::fromStdString(
+ statusUpdate.hasBeenRunning() ? " yes " : " no "));
+ found->setText(7,
+ QString::fromStdString(
+ statusUpdate.hasBeenTerminated() ? " yes " : " no "));
break;
}
}
- if (longest)
+ if (!found)
{
- widget.listWidgetActiveSkills->addItem(
- QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
+ // TODO: Sort to executor!
+ auto item = new SkillExecutionInfoTreeWidgetItem(
+ executionId, widget.treeWidgetSkillExecutions);
+
+ item->setText(0,
+ QString::fromStdString(
+ executionId.executionStartedTime.toDateTimeString()));
+ item->setText(1, QString::fromStdString(executionId.executorName));
+ item->setText(2, QString::fromStdString(executionId.skillId.toString()));
+ item->setText(3,
+ QString::fromStdString(
+ statusUpdate.hasBeenConstructed() ? " yes " : " no "));
+ item->setText(4,
+ QString::fromStdString(
+ statusUpdate.hasBeenInitialized() ? " yes " : " no "));
+ item->setText(
+ 5,
+ QString::fromStdString(statusUpdate.hasBeenPrepared() ? " yes " : " no "));
+ item->setText(
+ 6,
+ QString::fromStdString(statusUpdate.hasBeenRunning() ? " yes " : " no "));
+ item->setText(7,
+ QString::fromStdString(
+ statusUpdate.hasBeenTerminated() ? " yes " : " no "));
}
}
}
@@ -359,14 +437,16 @@ namespace armarx
void
SkillManagerMonitorWidgetController::executeSkill()
{
- if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
+ if (not selectedSkill.skillId.isFullySpecified())
{
return;
}
- std::scoped_lock l(skillMutex);
- const auto& skillDescriptions = skills.at(selectedSkill.providerName);
- if (!skillDescriptions.count(selectedSkill.skillName))
+ std::scoped_lock l(updateMutex);
+
+ auto providerId = *selectedSkill.skillId.providerId;
+ const auto& skillDescriptions = skills.at(providerId);
+ if (skillDescriptions.find(selectedSkill.skillId) == skillDescriptions.end())
{
return;
}
@@ -374,45 +454,44 @@ namespace armarx
auto data = getConfigAsAron();
char hostname[HOST_NAME_MAX];
-
gethostname(hostname, HOST_NAME_MAX);
- skills::manager::dto::SkillExecutionRequest exInfo;
- exInfo.executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")";
- exInfo.skillId = {selectedSkill.providerName, selectedSkill.skillName};
- exInfo.params = aron::data::Dict::ToAronDictDTO(data);
+ skills::SkillExecutionRequest req{selectedSkill.skillId,
+ "Skills.Manager GUI (hostname: " + std::string(hostname) +
+ ")",
+ data};
- ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/"
- << selectedSkill.skillName << ". The data was: " << data;
+ ARMARX_CHECK(selectedSkill.skillId.isFullySpecified()); // sanity check
+ ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.skillId << ".";
// Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
- manager->begin_executeSkill(exInfo);
+ memory->begin_executeSkill(req.toManagerIce());
}
void
SkillManagerMonitorWidgetController::stopSkill()
{
- std::scoped_lock l(skillMutex);
- if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
- {
- return;
- }
+ // std::scoped_lock l(updateMutex);
+ // if (selectedSkill.skillId.isFullySpecified())
+ // {
+ // return;
+ // }
- const auto& skillDescriptions = skills.at(selectedSkill.providerName);
- if (!skillDescriptions.count(selectedSkill.skillName))
- {
- return;
- }
+ // const auto& skillDescriptions = skills.at(*selectedSkill.skillId.providerId);
+ // if (!skillDescriptions.count(selectedSkill.skillId.skillName))
+ // {
+ // return;
+ // }
- ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/"
- << selectedSkill.skillName;
- manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
+ // ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.skillId;
+
+ // manager->abortSkill(selectedSkill.skillId);
}
void
SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
QTreeWidgetItem*)
{
- std::scoped_lock l(skillMutex);
+ std::scoped_lock l(updateMutex);
widget.groupBoxSkillDetails->setEnabled(false);
if (!current)
@@ -423,48 +502,40 @@ namespace armarx
if (!current->parent())
{
- // no parent available. Should not happen
+ // no parent available. Perhaps provider clicked?
return;
}
- SelectedSkill newSelectedSkill;
-
- // setup selected skill
- newSelectedSkill.providerName = current->parent()->text(0).toStdString();
- newSelectedSkill.skillName = current->text(0).toStdString();
-
- // setup groupBox
- widget.groupBoxSkillDetails->setTitle(QString::fromStdString(
- newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
- widget.groupBoxSkillDetails->setEnabled(true);
+ auto c = static_cast<SkillInfoTreeWidgetItem*>(current);
+ auto skillDescription = c->skillDescription;
- if (newSelectedSkill.providerName == selectedSkill.providerName and
- newSelectedSkill.skillName == selectedSkill.skillName)
+ if (selectedSkill.skillId == skillDescription.skillId)
{
+ // no change
return;
}
- selectedSkill = newSelectedSkill;
+ selectedSkill.skillId = skillDescription.skillId;
+
+ // setup groupBox
+ widget.groupBoxSkillDetails->setTitle(
+ QString::fromStdString(selectedSkill.skillId.toString()));
+ widget.groupBoxSkillDetails->setEnabled(true);
// setup table view
widget.treeWidgetSkillDetails->clear();
aronTreeWidgetController = nullptr;
skillsArgumentsTreeWidgetItem = nullptr;
- auto skillDesc = skills.at(selectedSkill.providerName).at(selectedSkill.skillName);
+ // We assert that the skill exists
+ ARMARX_CHECK(skills.count(*selectedSkill.skillId.providerId) > 0);
+ ARMARX_CHECK(skills.at(*selectedSkill.skillId.providerId).count(selectedSkill.skillId) > 0);
+ auto skillDesc = skills.at(*selectedSkill.skillId.providerId).at(selectedSkill.skillId);
{
- auto it = new QTreeWidgetItem(
- widget.treeWidgetSkillDetails,
- {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
- widget.treeWidgetSkillDetails->addTopLevelItem(it);
- }
-
- {
- auto it = new QTreeWidgetItem(
- widget.treeWidgetSkillDetails,
- {QString::fromStdString("Robot"),
- QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
+ auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Name"),
+ QString::fromStdString(skillDesc.skillId.skillName)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
@@ -479,17 +550,36 @@ namespace armarx
auto it = new QTreeWidgetItem(
widget.treeWidgetSkillDetails,
{QString::fromStdString("Timeout"),
- QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
+ QString::fromStdString(std::to_string(skillDesc.timeout.toMilliSeconds())) +
+ " ms"});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
+ // select root profile
+ widget.comboBoxProfiles->setCurrentIndex(0);
+
+ // remove any profile
+ while (widget.comboBoxProfiles->count() > 1)
+ {
+ widget.comboBoxProfiles->removeItem(1);
+ }
+
+ // add new profiles for this skill
+ // TODO: Where stored?
+
skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
{QString::fromStdString("Arguments")});
- auto aron_args = aron::type::Object::FromAronObjectDTO(skillDesc.acceptedType);
- auto default_args = aron::data::Dict::FromAronDictDTO(skillDesc.defaultParams);
+ auto aron_args = skillDesc.parametersType;
+ auto default_args_of_profile = skillDesc.rootProfileDefaults;
+
+ aronTreeWidgetController =
+ std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails,
+ skillsArgumentsTreeWidgetItem,
+ aron_args,
+ default_args_of_profile);
- aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(
- widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
+ // automatically expand args
+ skillsArgumentsTreeWidgetItem->setExpanded(true);
}
aron::data::DictPtr
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index f1ad1bb720ea69503b86b00f75a6e6f09f6058c5..1fd2e2f110fe8d34890f8d685c8faa5bc2031b70 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -38,11 +38,51 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include "aronTreeWidget/AronTreeWidgetController.h"
namespace armarx
{
+ class SkillInfoTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+ SkillInfoTreeWidgetItem(const skills::SkillDescription& desc, QTreeWidgetItem* parent) :
+ QTreeWidgetItem(parent), skillDescription(desc)
+ {
+ }
+
+ SkillInfoTreeWidgetItem(const skills::SkillDescription& desc, QTreeWidget* parent) :
+ QTreeWidgetItem(parent), skillDescription(desc)
+ {
+ }
+
+ skills::SkillDescription skillDescription;
+ };
+
+ class SkillExecutionInfoTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+ SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id,
+ QTreeWidgetItem* parent) :
+ QTreeWidgetItem(parent), executionId(id)
+ {
+ }
+
+ SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id, QTreeWidget* parent) :
+ QTreeWidgetItem(parent), executionId(id)
+ {
+ }
+
+ static SkillExecutionInfoTreeWidgetItem*
+ SearchRecursiveForMatch(SkillExecutionInfoTreeWidgetItem* el,
+ const skills::SkillExecutionID& needle);
+
+ skills::SkillExecutionID executionId;
+ };
+
class ARMARXCOMPONENT_IMPORT_EXPORT SkillManagerMonitorWidgetController :
public armarx::ArmarXComponentWidgetControllerTemplate<SkillManagerMonitorWidgetController>
{
@@ -81,7 +121,8 @@ namespace armarx
void updateTimerFrequency();
void refreshSkills();
- void refreshSkillsPeriodically();
+ void refreshExecutions();
+ void refreshSkillsAndExecutions();
void copyCurrentConfig();
void pasteCurrentConfig();
@@ -99,20 +140,25 @@ namespace armarx
QPointer<SimpleConfigDialog> dialog;
std::string observerName = "SkillManager";
- skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
-
- struct SelectedSkill
- {
- std::string providerName;
- std::string skillName;
- };
+ skills::dti::SkillMemoryInterfacePrx memory = nullptr;
// Data taken from observer (snapshot of it)
- mutable std::mutex skillMutex;
- skills::manager::dto::SkillDescriptionMapMap skills = {};
+ mutable std::mutex updateMutex;
+ std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>> skills =
+ {};
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillStatusUpdates = {};
// User Input
- SelectedSkill selectedSkill;
+ struct SelectedSkill
+ {
+ skills::SkillID skillId;
+
+ // make default constructable
+ SelectedSkill() :
+ skillId{.providerId = skills::ProviderID{.providerName = ""}, .skillName = ""}
+ {
+ }
+ } selectedSkill;
// Helper to get the treeWidgetItem easily
QTreeWidgetItem* skillsArgumentsTreeWidgetItem = nullptr;
@@ -121,9 +167,6 @@ namespace armarx
// others
QTimer* refreshSkillsResultTimer;
- // skillExecutions
- std::vector<std::thread> executions;
-
// connected flag
std::atomic_bool connected = false;
};
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index 088d28561684e1a604595f63af4aad6903e4caaf..f22738bda8b71cac09faf71620e226135ac93532 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -32,40 +32,103 @@ module armarx
{
module dto
{
- // Inputs
- struct SkillExecutionRequest
+ struct ProviderID
+ {
+ string providerName;
+ };
+
+ // A skill ID. Must be unique within one SkillManager
+ struct SkillID
+ {
+ ProviderID providerId;
+ string skillName;
+ };
+
+ struct SkillDescription
{
- 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
+ SkillID skillId;
+ string description;
+ armarx::core::time::dto::Duration timeout;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
+ aron::data::dto::Dict rootProfileDefaults;
};
+
+ dictionary<SkillID, dto::SkillDescription> SkillDescriptionMap;
+
struct ProviderInfo
{
- string providerName;
- provider::dti::SkillProviderInterface* provider;
- provider::dto::SkillDescriptionMap providedSkills;
+ ProviderID providerId;
+ provider::dti::SkillProviderInterface* providerInterface;
+ dto::SkillDescriptionMap providedSkills;
+ };
+
+ struct SkillExecutionID
+ {
+ SkillID skillId;
+ string executorName;
+ armarx::core::time::dto::DateTime executionStartedTime;
+ string uuid;
};
- // Provider data types
- dictionary<string, provider::dto::SkillDescriptionMap> SkillDescriptionMapMap;
- dictionary<string, provider::dto::SkillStatusUpdateMap> SkillStatusUpdateMapMap;
+ struct SkillStatusUpdate
+ {
+ SkillExecutionID executionId;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
+ core::dto::Execution::Status status;
+ aron::data::dto::Dict result;
+ };
+
+ dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
+
+ struct SkillExecutionRequest
+ {
+ SkillID skillId;
+ string executorName;
+ aron::data::dto::Dict parameters;
+ };
}
module dti
{
- interface SkillManagerInterface extends callback::dti::SkillProviderCallbackInterface
+ interface SkillManagerInterface extends
+ callback::dti::SkillProviderCallbackInterface
{
- // There is by design no method to get the ProviderInfo. You should only communicate with the manager
+ // !! There is by design no method to get a proxy of the provider. You should only communicate with the manager !!
void addProvider(dto::ProviderInfo providerInfo);
- void removeProvider(string providerName);
- dto::SkillDescriptionMapMap getSkillDescriptions();
- dto::SkillStatusUpdateMapMap getSkillExecutionStatuses();
+ void removeProvider(dto::ProviderID providerId);
+
+ optional(1) dto::SkillDescription getSkillDescription(
+ dto::SkillID skillId); // get one skilldescriptions from a provider
+
+ dto::SkillDescriptionMap
+ getSkillDescriptions(); // get all skilldescriptions from all providers
+
+ optional(2) dto::SkillStatusUpdate
+ getSkillExecutionStatus(dto::SkillExecutionID executionId);
+
+ dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(); // returns the current executions from all providers
+
+ dto::SkillStatusUpdate
+ executeSkill(dto::SkillExecutionRequest
+ skillExecutionRequest); // blocks until skill is finished.
+
+ dto::SkillExecutionID executeSkillAsync(
+ dto::SkillExecutionRequest
+ skillExecutionRequest); // directly returns the execution id
- provider::dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest skillExecutionInfo);
- void abortSkill(string providerName, string skillName);
+ provider::dto::ParameterUpdateResult updateSkillParameters(
+ dto::SkillExecutionID executionId,
+ aron::data::dto::Dict parameters); // add params to a skill
+ // notify a skill to stop ASAP.
+ provider::dto::AbortSkillResult abortSkill(dto::SkillExecutionID executionId);
+ provider::dto::AbortSkillResult
+ abortSkillAsync(dto::SkillExecutionID executionId);
};
}
}
diff --git a/source/RobotAPI/interface/skills/SkillMemoryInterface.ice b/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
index c8e3212efa1b0ae3bf2768a124a53441898cd6de..3d0d8ee61b23a5e184514b87792323d379c81173 100644
--- a/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
@@ -24,10 +24,10 @@
#include <ArmarXCore/interface/core/Profiler.ice>
-#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/skills/SkillManagerInterface.ice>
+#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
#include <RobotAPI/interface/skills/StatechartListenerInterface.ice>
-#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
module armarx
{
@@ -35,9 +35,8 @@ module armarx
{
module dti
{
- interface SkillMemoryInterface extends armem::server::MemoryInterface, dti::StatechartListenerInterface, manager::dti::SkillManagerInterface
- {
- };
+ interface SkillMemoryInterface extends armem::server::MemoryInterface,
+ dti::StatechartListenerInterface, manager::dti::SkillManagerInterface{};
}
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index ed40611c800bfccba6f44310ca5d88639d83c02f..3066c962e45326fecfdf8f08c6985bb89bf6c3d7 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -22,6 +22,8 @@
#pragma once
+#include <ArmarXCore/interface/core/time.ice>
+
#include <RobotAPI/interface/aron.ice>
@@ -37,6 +39,43 @@ module armarx
interface SkillProviderCallbackInterface;
}
}
+
+ module provider
+ {
+ module dti
+ {
+ interface SkillProviderInterface;
+ }
+ }
+ }
+}
+
+module armarx
+{
+ module skills
+ {
+ module core
+ {
+ module dto
+ {
+ // The status enum of a skill
+ module Execution
+ {
+ enum Status
+ {
+ Constructing,
+ Initializing,
+ Preparing,
+ Running,
+
+ // terminating values
+ Failed,
+ Succeeded,
+ Aborted
+ };
+ }
+ }
+ }
}
}
@@ -49,88 +88,112 @@ module armarx
{
module dto
{
- sequence<string> StringList;
+
+ // A profile is something that is client side!
+ // // A parameterization profile
+ // struct SkillProfile
+ // {
+ // string profileName;
+ // string predecessorProfileName; // may be empty
+ // aron::data::dto::Dict
+ // parameterization; // may be only a partial set of accepted type
+ // };
+
+ // // A list of parameterization profiles. Note that the first element is recognized as root. Following elements overwrite the previous.
+ // dictionary<string, SkillProfile> SkillProfileDict;
// 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)
+ // A skill is nothing but a executable thing, which can be executed
struct SkillDescription
{
- string skillName; // the name of the skill
- 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
+ SkillID skillId;
+ string description;
+ armarx::core::time::dto::Duration timeout;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
+ aron::data::dto::Dict rootProfileDefaults;
};
- dictionary<string, SkillDescription> SkillDescriptionMap;
+
+ dictionary<SkillID, SkillDescription> SkillDescriptionMap;
// Input to a provider to execute a skill
struct SkillExecutionRequest
{
- 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
+ SkillID skillId;
+ string executorName;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
};
- // The status enum of a skill
- module Execution
+ // The minimum information that is needed to uniquely identifying a past skill execution
+ struct SkillExecutionID
{
- enum Status
- {
- Idle, // a skill can only be idled if it has never been scheduled
- Scheduled,
- Running,
-
- // terminating values
- Failed,
- Succeeded,
- Aborted
- };
- }
+ SkillID skillId;
+ string executorName;
+ armarx::core::time::dto::DateTime executionStartedTime;
+ string uuid;
+ };
// Status updates of a skill
- struct SkillStatusUpdateHeader
+ struct SkillStatusUpdate
{
- 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
+ SkillExecutionID executionId;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
+ core::dto::Execution::Status status;
+ aron::data::dto::Dict result;
+ /// @todo maybe add in future:
+ // aron::data::dto::Dict feedback;
};
- struct SkillStatusUpdate
+ dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
+
+ struct ParameterUpdateResult
{
- SkillStatusUpdateHeader header;
- 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
+ bool success; // false if skill is not running (anymore) or already prepared
};
- dictionary<string, SkillStatusUpdate> SkillStatusUpdateMap;
+ struct AbortSkillResult
+ {
+ bool success; // false if skill is not running (anymore)
+ };
}
module dti
{
interface SkillProviderInterface
{
- dto::SkillDescription getSkillDescription(string name);
+ optional(1) dto::SkillDescription getSkillDescription(dto::SkillID skill);
+
dto::SkillDescriptionMap getSkillDescriptions();
- dto::SkillStatusUpdate getSkillExecutionStatus(string name);
- dto::SkillStatusUpdateMap getSkillExecutionStatuses();
+
+ optional(2) dto::SkillStatusUpdate
+ getSkillExecutionStatus(dto::SkillExecutionID executionId);
+
+ dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(); // returns all current skill executions
// 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
+ // TODO: Explain skill phases
// This method returns a status update where the status is ALWAYS one of the terminating values
dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest executionInfo);
+ dto::SkillExecutionID
+ executeSkillAsync(dto::SkillExecutionRequest executionInfo);
+
+ dto::ParameterUpdateResult
+ updateSkillParameters(dto::SkillExecutionID executionId,
+ aron::data::dto::Dict params); // add params to a skill
+
// try to kill a skill as soon as possible. When the skill is stopped depends on the implementation.
- void abortSkill(string skill);
+ dto::AbortSkillResult abortSkill(dto::SkillExecutionID skill);
+ dto::AbortSkillResult abortSkillAsync(dto::SkillExecutionID skill);
};
}
}
@@ -144,15 +207,23 @@ module armarx
{
module callback
{
+ module dto
+ {
+ struct ProviderID
+ {
+ string providerName;
+ };
+ }
+
module dti
{
interface SkillProviderCallbackInterface
{
// used for callbacks from providers to update their skill execution status
- void updateStatusForSkill(provider::dto::SkillStatusUpdate statusUpdate);
+ void updateStatusForSkill(provider::dto::SkillStatusUpdate statusUpdate,
+ dto::ProviderID providerId);
}
}
}
}
}
-
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 4e14baacb6040f7bf97a4855d3da7e4d252bce03..0eecc8e79de97cb3e164b195dd43cedc57b789cf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -10,13 +10,12 @@
namespace armarx::armem
{
- GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ GraspCandidateReader::GraspCandidateReader()
{
}
void
- GraspCandidateReader::connect(bool use)
+ GraspCandidateReader::connect(armem::client::MemoryNameSystem& memoryNameSystem, bool use)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 384f5cdb0c1158b714ba8b3038e09f4707dd74bb..4b7bba78baae8c78c7c37fdc77ebdaff8e6fd154 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -12,9 +12,9 @@ namespace armarx::armem
class GraspCandidateReader
{
public:
- GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ GraspCandidateReader();
- void connect(bool use = true);
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem, bool use = true);
::armarx::grasping::GraspCandidatePtr
queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
@@ -65,8 +65,6 @@ namespace armarx::armem
} properties;
const std::string propertyPrefix = "mem.grasping.";
-
- armem::client::MemoryNameSystem& memoryNameSystem;
};
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
index 7aa20ba28af1b49404118cfedb3abd006d784738..1f14e344ee988f984813e2a402903cfb0627b940 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -6,16 +6,11 @@
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
-
namespace armarx::armem
{
- GraspCandidateWriter::GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem)
- : memoryNameSystem(memoryNameSystem)
- {
- }
-
- void GraspCandidateWriter::connect()
+ void
+ GraspCandidateWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
@@ -23,33 +18,37 @@ namespace armarx::armem
try
{
memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
- ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '" << properties.memoryName << "'";
+ ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '"
+ << properties.memoryName << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
ARMARX_ERROR << e.what();
return;
}
-
}
- void GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.graspMemoryName, prefix + "GraspMemoryName",
+ def->optional(properties.graspMemoryName,
+ prefix + "GraspMemoryName",
"Name of the grasping memory core segment to use.");
- def->optional(properties.bimanualGraspMemoryName, prefix + "BimanualGraspMemoryName",
+ def->optional(properties.bimanualGraspMemoryName,
+ prefix + "BimanualGraspMemoryName",
"Name of the bimanual grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
-
}
- bool GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
std::string objectName = "UnknownObject";
@@ -65,8 +64,11 @@ namespace armarx::armem
return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
}
- bool GraspCandidateWriter::commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitBimanualGraspCandidate(
+ const armarx::grasping::BimanualGraspCandidate& candidate,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
@@ -80,11 +82,15 @@ namespace armarx::armem
auto dict = aronGraspCandidate.toAron();
- return commitToMemory({dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
+ return commitToMemory(
+ {dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
}
- bool GraspCandidateWriter::commitGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitGraspCandidateSeq(
+ const armarx::grasping::GraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
bool success = true;
@@ -107,7 +113,7 @@ namespace armarx::armem
for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
+ if (!commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
{
success = false;
}
@@ -115,8 +121,10 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitBimanualGraspCandidateSeq(
- const armarx::grasping::BimanualGraspCandidateSeq& candidates, const armem::Time& timestamp,
+ bool
+ GraspCandidateWriter::commitBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
const std::string& provider)
{
bool success = true;
@@ -138,7 +146,7 @@ namespace armarx::armem
for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
+ if (!commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
{
success = false;
}
@@ -146,14 +154,16 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
- const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
- const std::string& coreMemoryName)
+ bool
+ GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
+ const std::string& providerName,
+ const std::string& entityName,
+ const armem::Time& timestamp,
+ const std::string& coreMemoryName)
{
std::lock_guard g{memoryWriterMutex};
- const auto result =
- memoryWriter.addSegment(coreMemoryName, providerName);
+ const auto result = memoryWriter.addSegment(coreMemoryName, providerName);
if (not result.success)
{
@@ -164,8 +174,7 @@ namespace armarx::armem
}
const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(entityName).withTimestamp(timestamp);
+ const auto entityID = providerId.withEntityName(entityName).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -184,4 +193,4 @@ namespace armarx::armem
return updateResult.success;
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
index 88793cb534cb18af0fefddcd72be4215eaa70e2b..526f16b0ae327f6e3465c7adfc3d2c1c7fb1b26b 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
@@ -1,58 +1,60 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client.h>
#include <mutex>
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
namespace armarx::armem
{
class GraspCandidateWriter
{
public:
- GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ GraspCandidateWriter() = default;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
bool commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider);
+ const armem::Time& timestamp,
+ const std::string& provider);
bool commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider);
+ const armem::Time& timestamp,
+ const std::string& provider);
bool commitGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider);
- bool commitBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider);
-
+ const armem::Time& timestamp,
+ const std::string& provider);
+ bool commitBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
+ const std::string& provider);
private:
-
bool commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
- const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
+ const std::string& providerName,
+ const std::string& entityName,
+ const armem::Time& timestamp,
const std::string& coreMemoryName);
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Writer memoryWriter;
struct Properties
{
- std::string memoryName = "Grasp";
- std::string graspMemoryName = "GraspCandidate";
+ std::string memoryName = "Grasp";
+ std::string graspMemoryName = "GraspCandidate";
std::string bimanualGraspMemoryName = "BimanualGraspCandidate";
} properties;
-
std::mutex memoryWriterMutex;
const std::string propertyPrefix = "mem.grasping.";
-
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 875250eb836740ec46dc3cc009ded155e97a2264..d476b25ef2003edeeb183fbe4e74bcd70a6176c0 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,9 +2,9 @@
// STD/STL
+#include <mutex>
#include <optional>
#include <vector>
-#include <mutex>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
@@ -15,7 +15,6 @@
#include "Query.h"
-
namespace armarx::armem::client
{
@@ -26,11 +25,11 @@ namespace armarx::armem::client
{
public:
-
/**
* @brief Construct a memory reader.
* @param memory The memory proxy.
*/
+ Reader(const Reader&) = default;
Reader(server::ReadingMemoryInterfacePrx readingMemory = nullptr,
server::PredictingMemoryInterfacePrx predictingMemory = nullptr);
@@ -41,8 +40,10 @@ namespace armarx::armem::client
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
- QueryResult query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- QueryResult query(const armem::query::data::MemoryQuerySeq& queries, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(armem::query::data::MemoryQueryPtr query,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(const armem::query::data::MemoryQuerySeq& queries,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
QueryResult query(const QueryBuilder& queryBuilder) const;
@@ -114,7 +115,8 @@ namespace armarx::armem::client
* @return The query result.
*/
QueryResult
- queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ queryMemoryIDs(const std::vector<MemoryID>& ids,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -132,8 +134,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getLatestSnapshotsIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
* @brief Get the latest snapshot under the given memory ID.
@@ -141,8 +144,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The latest contained snapshot, if any.
*/
- std::optional<wm::EntitySnapshot>
- getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ std::optional<wm::EntitySnapshot> getLatestSnapshotIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -150,8 +154,8 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getAllLatestSnapshots(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getAllLatestSnapshots(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -163,7 +167,8 @@ namespace armarx::armem::client
getAll(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- server::dto::DirectlyStoreResult directlyStore(const server::dto::DirectlyStoreInput& input) const;
+ server::dto::DirectlyStoreResult
+ directlyStore(const server::dto::DirectlyStoreInput& input) const;
void startRecording() const;
void stopRecording() const;
@@ -201,7 +206,6 @@ namespace armarx::armem::client
int recursionDepth);
public:
-
server::ReadingMemoryInterfacePrx readingPrx;
server::PredictingMemoryInterfacePrx predictionPrx;
};
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 6c4b223ce12e0ee80cd5564e2d2d3ad688661cbc..61eeef93be3164d79d09f6c5aa3371cb362b58b2 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -1,10 +1,8 @@
#pragma once
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
namespace armarx::armem::client
{
@@ -24,17 +22,21 @@ namespace armarx::armem::client
class Writer
{
public:
-
/**
* @brief Construct a memory writer.
* @param memory The memory proxy.
*/
+ Writer(const Writer&) = default;
Writer(server::WritingMemoryInterfacePrx memory = nullptr);
- data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const MemoryID& providerSegmentID, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const MemoryID& providerSegmentID,
+ bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names,
+ bool clearWhenExists = false);
data::AddSegmentResult addSegment(const data::AddSegmentInput& input);
data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input);
@@ -46,10 +48,9 @@ namespace armarx::armem::client
/// Commit a single entity update.
EntityUpdateResult commit(const EntityUpdate& update);
/// Commit a single entity update.
- EntityUpdateResult commit(
- const MemoryID& entityID,
- const std::vector<aron::data::DictPtr>& instancesData,
- Time referencedTime);
+ EntityUpdateResult commit(const MemoryID& entityID,
+ const std::vector<aron::data::DictPtr>& instancesData,
+ Time referencedTime);
// with bare-ice types
data::CommitResult commit(const data::Commit& commit);
@@ -63,18 +64,15 @@ namespace armarx::armem::client
}
private:
-
/// Sets `timeSent` on all entity updates and performs the commit,
data::CommitResult _commit(data::Commit& commit);
public:
-
server::WritingMemoryInterfacePrx memory;
-
};
-}
+} // namespace armarx::armem::client
namespace armarx::armem::data
{
@@ -82,4 +80,4 @@ namespace armarx::armem::data
std::ostream& operator<<(std::ostream& os, const AddSegmentsInput& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentResult& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentsResult& rhs);
-}
+} // namespace armarx::armem::data
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
index 168686040e37f0346757b4ee259dd0107b0e4a37..868d3ef30cd593ab09c4ac55bcdcdcbdf2f7f81d 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
@@ -23,13 +23,12 @@
#include <string>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
namespace armarx::armem::client::plugins
{
@@ -45,8 +44,8 @@ namespace armarx::armem::client::plugins
class ReaderWriterPlugin : public armarx::ComponentPlugin
{
public:
- ReaderWriterPlugin(ManagedIceObject& parent, const std::string pre) :
- ComponentPlugin(parent, pre), readerWriter(memoryNameSystem())
+ ReaderWriterPlugin(ManagedIceObject& parent, const std::string& pre) :
+ ComponentPlugin(parent, pre)
{
if (not armemPlugin)
{
@@ -74,7 +73,7 @@ namespace armarx::armem::client::plugins
return;
}
- readerWriter.connect();
+ readerWriter.connect(memoryNameSystem());
}
bool
@@ -114,9 +113,8 @@ namespace armarx::armem::client::plugins
}
armarx::armem::client::plugins::Plugin* armemPlugin = nullptr;
-
- T readerWriter;
+ T readerWriter;
};
} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index 856bc217146eb651770fc192dd9ce8a03ebcdf3c..e14ce8848fd1fe8242d681770563bf9cddefd7cd 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -6,8 +6,7 @@
namespace armarx::armem::client::util
{
- SimpleReaderBase::SimpleReaderBase(MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ SimpleReaderBase::SimpleReaderBase()
{
}
@@ -25,14 +24,13 @@ namespace armarx::armem::client::util
}
void
- SimpleReaderBase::connect()
+ SimpleReaderBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryReaderClient =
- memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
+ memoryReaderClient = mns.useReader(MemoryID().withMemoryName(props.memoryName));
ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
<< "'";
}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index 45b26e7c9e8c4dd124b762d297d2fb0dca29a317..2b637193765215b81519c55397bcc51a7436041b 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -27,7 +27,6 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
-
namespace armarx::armem::client
{
class MemoryNameSystem;
@@ -43,40 +42,36 @@ namespace armarx::armem::client::util
// Properties
struct Properties
{
- std::string memoryName = "";
- std::string coreSegmentName = "";
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
};
- SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
+ SimpleReaderBase();
virtual ~SimpleReaderBase() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- virtual void connect();
+ virtual void connect(armarx::armem::client::MemoryNameSystem& mns);
const Properties& properties() const;
- void setProperties(const Properties& p)
+
+ void
+ setProperties(const Properties& p)
{
- props = p;
+ props = p;
}
protected:
-
- virtual std::string propertyPrefix() const = 0;
+ virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
std::mutex& memoryReaderMutex();
const armem::client::Reader& memoryReader() const;
-
- MemoryNameSystem& memoryNameSystem;
-
private:
-
Properties props;
armem::client::Reader memoryReaderClient;
std::mutex memoryMutex;
-
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index 9e27df2c2ef5d7af53529502489085e4f43c231d..e5d468c552d96d70d3ef4ae92196cdb714f39163 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -1,17 +1,14 @@
#include "SimpleWriterBase.h"
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::util
{
- SimpleWriterBase::SimpleWriterBase(MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ SimpleWriterBase::SimpleWriterBase()
{
}
-
void
SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -26,21 +23,19 @@ namespace armarx::armem::client::util
// TODO(fabian.reister): this might also be part of the subclass
// if the provider name has to be derived from e.g. the component name
- def->optional(props.providerName,
- prefix + "Provider",
- "Name of this provider");
+ def->optional(props.providerName, prefix + "Provider", "Name of this provider");
}
-
- void SimpleWriterBase::connect()
+ void
+ SimpleWriterBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
- << props.memoryName << "' ...";
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryWriterClient = memoryNameSystem.useWriter(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName << "'";
+ memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -49,20 +44,20 @@ namespace armarx::armem::client::util
}
}
-
- std::mutex& SimpleWriterBase::memoryWriterMutex()
+ std::mutex&
+ SimpleWriterBase::memoryWriterMutex()
{
return memoryMutex;
}
-
- armem::client::Writer& SimpleWriterBase::memoryWriter()
+ armem::client::Writer&
+ SimpleWriterBase::memoryWriter()
{
return memoryWriterClient;
}
-
- const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+ const SimpleWriterBase::Properties&
+ SimpleWriterBase::properties() const
{
return props;
}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
index 1413299a3081f36dfaa83cce425c50994e7018c2..13a54cc5b213a90e1949f73c1b11de716c0593cc 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -27,7 +27,6 @@
#include <RobotAPI/libraries/armem/client/Writer.h>
-
namespace armarx::armem::client
{
class MemoryNameSystem;
@@ -39,26 +38,24 @@ namespace armarx::armem::client::util
class SimpleWriterBase
{
public:
-
- SimpleWriterBase(MemoryNameSystem& memoryNameSystem);
+ SimpleWriterBase();
virtual ~SimpleWriterBase() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armarx::armem::client::MemoryNameSystem& mns);
protected:
-
struct Properties
{
- std::string memoryName = "";
+ std::string memoryName = "";
std::string coreSegmentName = "";
- std::string providerName = ""; // required property
+ std::string providerName = ""; // required property
};
const Properties& properties() const;
- virtual std::string propertyPrefix() const = 0;
+ virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
std::mutex& memoryWriterMutex();
@@ -66,14 +63,10 @@ namespace armarx::armem::client::util
private:
-
Properties props;
armem::client::Writer memoryWriterClient;
std::mutex memoryMutex;
-
- MemoryNameSystem& memoryNameSystem;
-
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index e7ceabba97898778af1c13d9ad9c805357f79ac2..359ad8e8d978d0bde3e480388e5ed2b1e7f39f30 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -44,16 +44,14 @@
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
-
namespace armarx::armem::laser_scans::client
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ Reader::Reader()
{
}
- Reader::~Reader() = default;
+ Reader::~Reader() = default;
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -64,7 +62,7 @@ namespace armarx::armem::laser_scans::client
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 55beab204b12a7f72432ef2e38789f1856058934..5bcfd2c63b9b8bc5682f2b58a2dfbebf7a78a56c 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -29,10 +29,9 @@
#include "RobotAPI/libraries/armem_laser_scans/types.h"
#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
namespace armarx
{
@@ -62,11 +61,10 @@ namespace armarx::armem::laser_scans::client
class Reader
{
public:
-
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader();
virtual ~Reader();
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
struct Query
{
@@ -99,12 +97,9 @@ namespace armarx::armem::laser_scans::client
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- armarx::armem::client::query::Builder
- buildQuery(const Query& query) const;
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
@@ -113,7 +108,6 @@ namespace armarx::armem::laser_scans::client
} properties;
const std::string propertyPrefix = "mem.vision.laser_scans.";
-
};
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
index 8b43cef166b729c2bb7a0bb3d64df74268d1975d..771f49c22a76585026dc735f7f9697e1b28abf7b 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
@@ -6,16 +6,14 @@
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
-
namespace armarx::armem::laser_scans::client
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ Writer::Writer()
{
}
- Writer::~Writer() = default;
+ Writer::~Writer() = default;
void
Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -23,11 +21,10 @@ namespace armarx::armem::laser_scans::client
ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
-
}
void
- Writer::connect()
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
index 6a75f24b53d13c0ba2d885c588b5c3e944388a57..8b5a90a1867da3d84a225c73e543c694e3d632f3 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-
namespace armarx::armem::laser_scans::client
{
@@ -48,11 +47,11 @@ namespace armarx::armem::laser_scans::client
class Writer
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ Writer();
virtual ~Writer();
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
// MappingDataWriterInterface
/// to be called in Component::onConnectComponent
@@ -67,7 +66,6 @@ namespace armarx::armem::laser_scans::client
const armem::Time& timestamp);
private:
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
// Properties
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
index 0fd0c6a1b5258d7960e9179c649ab0286b26be5a..13cc8ff5a3e515e79e80838a875acad24a6186cd 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -10,13 +10,6 @@
namespace armarx::armem::locations::client
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem),
- objReader(memoryNameSystem),
- robotReader(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -28,7 +21,7 @@ namespace armarx::armem::locations::client
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -45,8 +38,8 @@ namespace armarx::armem::locations::client
return;
}
- objReader.connect();
- robotReader.connect();
+ objReader.connect(memoryNameSystem);
+ robotReader.connect(memoryNameSystem);
}
std::map<std::string, armarx::navigation::location::arondto::Location>
@@ -63,11 +56,11 @@ namespace armarx::armem::locations::client
{
auto i = e.findLatestInstance();
if (i)
- {
- auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
- ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
}
- });
+ });
return ret;
}
@@ -83,7 +76,7 @@ namespace armarx::armem::locations::client
for (auto& [locName, location] : locations)
{
- (void) locName;
+ (void)locName;
if (location.framedPose.header.frame == armarx::GlobalFrame)
{
location.framedPose.header.agent = ""; //sanity set
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.h b/source/RobotAPI/libraries/armem_locations/client/Reader.h
index ad895c269e52fd2e4ec1b6e6bbda037fbefedb04..7a25269d181a9079ec0f770f27fd240d25d7a5af 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.h
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -23,11 +23,11 @@ namespace armarx::armem::locations::client
std::string memoryName = "Navigation";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
Properties
getProperties()
@@ -44,7 +44,6 @@ namespace armarx::armem::locations::client
const std::string propertyPrefix = "mem.nav.location";
- armem::client::MemoryNameSystem& memoryNameSystem;
armarx::armem::obj::instance::Reader objReader;
armarx::armem::robot_state::RobotReader robotReader;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index ac01fb3b87a1dc4bc53b40982a38002a80cc0d23..4c9ec304ab9195abfd49fb5dcd2cadc18211fc88 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -5,19 +5,20 @@
#include <Eigen/Geometry>
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include "RobotAPI/libraries/armem/core/Commit.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
@@ -30,20 +31,17 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName << "' ...";
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
+ << "' ...";
try
{
memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName << "'";
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -96,7 +94,9 @@ namespace armarx::armem::articulated_object
}
std::optional<ArticulatedObject>
- Reader::get(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::get(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
const auto splits = simox::alg::split(name, "/");
ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
@@ -118,7 +118,8 @@ namespace armarx::armem::articulated_object
ArticulatedObject
Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
- const std::string& instanceName, const std::optional<std::string>& providerName)
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName)
{
ArticulatedObject obj{.description = description,
.instance = instanceName,
@@ -131,7 +132,9 @@ namespace armarx::armem::articulated_object
}
bool
- Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
@@ -148,7 +151,8 @@ namespace armarx::armem::articulated_object
}
std::vector<robot::RobotDescription>
- Reader::queryDescriptions(const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescriptions(const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -174,12 +178,14 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotDescription>
- Reader::queryDescription(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescription(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
- if(providerName.has_value()) // query single provider
+ if (providerName.has_value()) // query single provider
{
// clang-format off
qb
@@ -189,7 +195,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
- else // query all providers
+ else // query all providers
{
// clang-format off
qb
@@ -199,7 +205,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
-
+
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -215,7 +221,9 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotState>
- Reader::queryState(const std::string& instanceName, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -242,7 +250,6 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
-
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
@@ -262,7 +269,8 @@ namespace armarx::armem::articulated_object
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues, .proprioception=std::nullopt};
+ .jointMap = objectPose.objectJointValues,
+ .proprioception = std::nullopt};
return robotState;
}
@@ -288,7 +296,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
@@ -308,7 +315,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index fc8f1da389f443a90c12aa0c0fce70c24b53c3f1..0eb5bd104f4de49f3b7dfa6cc14ce232a99933fc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -39,11 +39,15 @@ namespace armarx::armem::articulated_object
class Reader : virtual public ReaderInterface
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def){}
- void connect();
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ }
+
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
@@ -88,8 +92,6 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
};
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 20c3458aa7941cf7caa5a4a0dab9010f2267c035..b8755486996f7092e1687bcefc55f302566d62d2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -22,15 +22,10 @@
#include "utils.h"
-
namespace armarx::armem::articulated_object
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -46,10 +41,12 @@ namespace armarx::armem::articulated_object
"Name of the memory core segment to use for object classes.");
ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
- def->required(properties.providerName, prefix + "write.ProviderName", "Name of this provider");
+ def->required(
+ properties.providerName, prefix + "write.ProviderName", "Name of this provider");
}
- void Writer::connect()
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -80,7 +77,8 @@ namespace armarx::armem::articulated_object
memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
}
- void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+ void
+ Writer::updateKnownObject(const armem::MemoryID& snapshotId)
{
arondto::RobotDescription aronArticulatedObjectDescription;
// aronArticulatedObjectDescription.fromAron(snapshotId.ent);
@@ -88,21 +86,24 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
+ void
+ Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
}
- void Writer::updateKnownObjects()
+ void
+ Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(Time::Now());
ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
- std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeOrGetClass(const ArticulatedObject& obj)
{
ARMARX_TRACE;
@@ -125,7 +126,8 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeClass(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -154,7 +156,7 @@ namespace armarx::armem::articulated_object
toAron(aronArticulatedObjectDescription, obj.description);
update.instancesData = {aronArticulatedObjectDescription.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -173,17 +175,20 @@ namespace armarx::armem::articulated_object
return updateResult.snapshotID;
}
- std::string Writer::getProviderName() const
+ std::string
+ Writer::getProviderName() const
{
return properties.providerName;
}
- void Writer::setProviderName(const std::string& providerName)
+ void
+ Writer::setProviderName(const std::string& providerName)
{
this->properties.providerName = providerName;
}
- bool Writer::storeInstance(const ArticulatedObject& obj)
+ bool
+ Writer::storeInstance(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -192,12 +197,13 @@ namespace armarx::armem::articulated_object
ARMARX_CHECK(not obj.instance.empty()) << "An object instance name must be provided!";
const std::string entityName = obj.description.name + "/" + obj.instance;
- ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '" << properties.providerName << "')";
+ ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '"
+ << properties.providerName << "')";
const auto providerId = armem::MemoryID()
- .withMemoryName(properties.memoryName)
- .withCoreSegmentName(properties.coreInstanceSegmentName)
- .withProviderSegmentName(properties.providerName);
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreInstanceSegmentName)
+ .withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
update.entityID = providerId.withEntityName(entityName);
@@ -235,7 +241,7 @@ namespace armarx::armem::articulated_object
objectInstance.pose.attachmentValid = false;
update.instancesData = {objectInstance.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -250,7 +256,8 @@ namespace armarx::armem::articulated_object
return updateResult.success;
}
- bool Writer::store(const ArticulatedObject& obj)
+ bool
+ Writer::store(const ArticulatedObject& obj)
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
@@ -292,22 +299,23 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(properties.coreClassSegmentName);
std::unordered_map<std::string, armem::MemoryID> descriptions;
- coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
- {
- if (entity.empty())
+ coreSegment.forEachEntity(
+ [&descriptions](const wm::Entity& entity)
{
- ARMARX_WARNING << "No entity found";
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return true;
+ }
+
+ const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
+ if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
return true;
- }
-
- const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
- if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
- {
- const armem::MemoryID snapshotID(sn.id());
- descriptions.insert({robotDescription->name, snapshotID});
- }
- return true;
- });
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 3e827d66a496ddf8df4448788f39ca1c36722290..6245ef374edc289afa452900ee97e769ed4eae8b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -32,19 +32,17 @@
#include "interfaces.h"
-
namespace armarx::armem::articulated_object
{
- class Writer:
- virtual public WriterInterface
+ class Writer : virtual public WriterInterface
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystemopti);
+ Writer() = default;
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool store(const ArticulatedObject& obj) override;
@@ -59,33 +57,33 @@ namespace armarx::armem::articulated_object
private:
-
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
- void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs);
void updateKnownObjects();
void updateKnownObject(const armem::MemoryID& snapshotId);
// TODO duplicate
- std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
- std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
-
+ std::unordered_map<std::string, armem::MemoryID>
+ queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
struct Properties
{
- std::string memoryName = "Object";
+ std::string memoryName = "Object";
std::string coreInstanceSegmentName = "Instance";
- std::string coreClassSegmentName = "Class";
- std::string providerName = "";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "";
- bool allowClassCreation = false;
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
@@ -97,4 +95,4 @@ namespace armarx::armem::articulated_object
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index b2b740b735e933245a44bbfd3ce8d1c437ff30b2..399fce56d0cc22c10de24a8b1a4ab29eb6668581 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -21,11 +21,6 @@
namespace armarx::armem::obj::instance
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -35,7 +30,7 @@ namespace armarx::armem::obj::instance
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -48,8 +43,8 @@ namespace armarx::armem::obj::instance
this->objPoseStorage =
objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
- ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '"
- << p.memoryName << "'";
+ ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -93,8 +88,6 @@ namespace armarx::armem::obj::instance
return requestResult.results.at(requestObject).result.success;
}
return false;
-
-
}
std::optional<objpose::ObjectPose>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 81a0fc03092bbbe67c476d3e5f2146a28d62517a..18f52a6111d96525c0b5940b2e03084b8ff29a96 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -45,11 +45,11 @@ namespace armarx::armem::obj::instance
std::string memoryName = "Object";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
// localization stuff. Requires an instance index to be set.
std::map<std::string, bool> requestLocalization(const ObjectID& instanceId,
@@ -74,7 +74,8 @@ namespace armarx::armem::obj::instance
return this->p;
}
- objpose::ObjectPoseStorageInterfacePrx getObjectPoseStorage() const
+ objpose::ObjectPoseStorageInterfacePrx
+ getObjectPoseStorage() const
{
return objPoseStorage;
}
@@ -84,7 +85,6 @@ namespace armarx::armem::obj::instance
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
objpose::ObjectPoseStorageInterfacePrx objPoseStorage;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
index 8ee596b27069f82c9ff19d63efcd40e7cc819699..80d1af5590886994c04a69adb025d2767b8f7ffb 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -3,29 +3,29 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
namespace armarx::armem::obj::instance
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {}
+ Writer::Writer()
+ {
+ }
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -38,8 +38,8 @@ namespace armarx::armem::obj::instance
"Name of the memory core segment to use for object instances.");
}
-
- void Writer::connect()
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -55,7 +55,10 @@ namespace armarx::armem::obj::instance
}
}
- bool Writer::commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time& t)
+ bool
+ Writer::commitObject(const armem::arondto::ObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time& t)
{
armem::Commit c;
auto& e = c.add();
@@ -63,19 +66,21 @@ namespace armarx::armem::obj::instance
e.entityID.memoryName = properties.memoryName;
e.entityID.coreSegmentName = properties.coreSegmentName;
e.entityID.providerSegmentName = provider;
- e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className + "/" + inst.pose.objectID.instanceName;
+ e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className +
+ "/" + inst.pose.objectID.instanceName;
e.referencedTime = t;
e.sentTime = armem::Time::Now();
- e.instancesData = { inst.toAron() };
+ e.instancesData = {inst.toAron()};
auto res = memoryWriter.commit(c);
- if(!res.allSuccess())
+ if (!res.allSuccess())
{
- ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: " << res.allErrorMessages();
+ ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: "
+ << res.allErrorMessages();
return false;
}
return true;
}
-} // namespace armarx::armem::instance
+} // namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
index 972227993aa5c6369ff656a594fa2e2005b0f9dd..d449502849b09de61e3abc65ab0364928502a083 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
@@ -28,40 +28,37 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
-
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::instance
{
class Writer
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ Writer();
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time&);
+ bool commitObject(const armem::arondto::ObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time&);
private:
-
-
struct Properties
{
- std::string memoryName = "Object";
- std::string coreSegmentName = "Instance";
+ std::string memoryName = "Object";
+ std::string coreSegmentName = "Instance";
} properties;
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
mutable std::mutex memoryWriterMutex;
};
-} // namespace armarx::armem::instance
+} // namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 5fb5004c926f4997a9b3e252e8a882c92d0462ea..8d94400263d0deb04152cda8f96d9b9dcbd8c9f2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -31,10 +31,11 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
-
-
- RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
+ RobotReader::RobotReader(const RobotReader& r) :
+ properties(r.properties),
+ propertyPrefix(r.propertyPrefix),
+ memoryReader(r.memoryReader),
+ transformReader(r.transformReader)
{
}
@@ -45,9 +46,9 @@ namespace armarx::armem::robot_state
}
void
- RobotReader::connect()
+ RobotReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- transformReader.connect();
+ transformReader.connect(memoryNameSystem);
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 6f90a9b249286efb7d1db277ef3df492e0dd8319..dbf81e69a9e4cbc8e488df482634d3542487376c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,14 +43,16 @@ namespace armarx::armem::robot_state
class RobotReader : virtual public robot::ReaderInterface
{
public:
- RobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotReader() = default;
+ RobotReader(const RobotReader&);
virtual ~RobotReader() = default;
- virtual void connect();
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem);
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) const override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj,
+ const armem::Time& timestamp) const override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) const override;
@@ -68,7 +69,7 @@ namespace armarx::armem::robot_state
std::optional<armarx::armem::arondto::Proprioception>
queryProprioception(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -135,7 +136,8 @@ namespace armarx::armem::robot_state
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
std::optional<armarx::armem::arondto::Proprioception>
- getRobotProprioception(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
@@ -160,9 +162,8 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
client::robot_state::localization::TransformReader transformReader;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index f971c88c3e41bf962500a1ac49e0b5ad7e5a39be..e92e13d6421c71313bdd04a488e1f43397d1f222 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -34,21 +34,20 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotWriter::~RobotWriter() = default;
-
-
- RobotWriter::RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ RobotWriter::RobotWriter(const RobotWriter& r) :
+ properties(r.properties), propertyPrefix(r.propertyPrefix), memoryWriter(r.memoryWriter)
{
}
+ RobotWriter::~RobotWriter() = default;
+
void
RobotWriter::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
}
void
- RobotWriter::connect()
+ RobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotWriter: Waiting for memory '" << constants::memoryName << "' ...";
@@ -65,7 +64,6 @@ namespace armarx::armem::robot_state
}
}
-
bool
RobotWriter::storeDescription(const robot::RobotDescription& description,
const armem::Time& timestamp)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
index f1e20614ee4ee45880e75d98ef223709b85932ae..d8ca351d6cf8643960f1707a49ef26b9697d4082 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,10 +43,11 @@ namespace armarx::armem::robot_state
class RobotWriter : virtual public robot::WriterInterface
{
public:
- RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotWriter() = default;
+ RobotWriter(const RobotWriter&);
~RobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
@@ -72,7 +72,6 @@ namespace armarx::armem::robot_state
const std::string& robotName,
const armem::Time& timestamp);
-
struct Properties
{
@@ -80,9 +79,7 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
armem::client::Writer memoryWriter;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 0e972f79138fbfc973dbab543206d94965227f03..0effa5c23d6763ff984610100b1770024e452bee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -15,34 +15,28 @@
namespace armarx::armem::robot_state
{
-
- VirtualRobotReader::VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotReader(memoryNameSystem)
- {
- }
-
- void VirtualRobotReader::connect()
- {
- RobotReader::connect();
- }
-
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) const
+ bool
+ VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp) const
{
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(), .xml = PackagePath{"", ""}};
+ const robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
+ << robot.getName() << "` "
<< "(type `" << robot.getType() << "`)!";
return false;
}
@@ -53,15 +47,19 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
- ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name << "'";
+ ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name
+ << "'";
const auto description = queryDescription(name, timestamp);
if (not description)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name << "` is not a available!";
+ ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name
+ << "` is not a available!";
return nullptr;
}
@@ -69,8 +67,8 @@ namespace armarx::armem::robot_state
const std::string xmlFilename = description->xml.toSystemPath();
ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
- ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name << "' from XML file '"
- << xmlFilename << "'";
+ ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name
+ << "' from XML file '" << xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file `" << xmlFilename << "`";
@@ -80,24 +78,30 @@ namespace armarx::armem::robot_state
return robot;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(const std::string& name,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
return _getSynchronizedRobot(name, armem::Time::Invalid(), loadMode, blocking);
}
VirtualRobot::RobotPtr
- VirtualRobotReader::getSynchronizedRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- const bool blocking)
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode,
+ const bool blocking)
{
return _getSynchronizedRobot(name, timestamp, loadMode, blocking);
}
- VirtualRobot::RobotPtr VirtualRobotReader::_getSynchronizedRobot(const std::string& name, const Time& timestamp,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::_getSynchronizedRobot(
+ const std::string& name,
+ const Time& timestamp,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
while (blocking)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 7a2f39c17939d30b26ef75057e1c9d302b0f68a8..1d91f05829b1c49fc5a07782ecbead486e075a5c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -40,10 +40,10 @@ namespace armarx::armem::robot_state
class VirtualRobotReader : virtual public RobotReader
{
public:
- VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ using RobotReader::RobotReader;
+
~VirtualRobotReader() override = default;
- void connect() override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index fc3aed1eea251e199077d92ccb615249b1777570..e7525729d3f6d59d4bb8c3feaf237f5ff34d18de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -18,23 +18,15 @@
#include "constants.h"
-
namespace armarx::armem::robot_state
{
- VirtualRobotWriter::VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotWriter(memoryNameSystem)
- {
- }
-
-
VirtualRobotWriter::~VirtualRobotWriter() = default;
-
void
- VirtualRobotWriter::connect()
+ VirtualRobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- RobotWriter::connect();
+ RobotWriter::connect(memoryNameSystem);
}
// TODO(fabian.reister): register property defs
@@ -70,12 +62,14 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
-
- return RobotWriter::storeState(robotState,
- robot.getType(),
- robot.getName(),
- constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
+ .jointMap = robot.getJointValues(),
+ .proprioception = std::nullopt};
+
+ return RobotWriter::storeState(
+ robotState,
+ robot.getType(),
+ robot.getName(),
+ constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 260032c62a1ec4db1a68e78e0660125ba7f37826..29a009714506d069ebd05dcbc988acc64eafe1de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -29,7 +29,6 @@
#include "RobotWriter.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -43,10 +42,10 @@ namespace armarx::armem::robot_state
class VirtualRobotWriter : virtual public RobotWriter
{
public:
- VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ VirtualRobotWriter() = default;
~VirtualRobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
[[nodiscard]] bool storeDescription(const VirtualRobot::Robot& robot,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 21b8e55a9fa9c74d63f61dc67aea86bce381c01f..7fa14f5a1bf7ab5a9519584f8432aa92f04a5617 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -38,39 +38,31 @@
#include <SimoxUtility/math/pose/interpolate.h>
// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
// this package
-#include <RobotAPI/libraries/core/FramedPose.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/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-
-#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformReader::TransformReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformReader::~TransformReader() = default;
- void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
@@ -83,7 +75,8 @@ namespace armarx::armem::client::robot_state::localization
def->optional(properties.memoryName, prefix + "Memory");
}
- void TransformReader::connect()
+ void
+ TransformReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
@@ -100,15 +93,15 @@ namespace armarx::armem::client::robot_state::localization
}
}
- TransformResult TransformReader::getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const armem::Time& timestamp) const
+ TransformResult
+ TransformReader::getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const
{
const TransformQuery query{.header = {.parentFrame = GlobalFrame,
- .frame = robotRootFrame,
- .agent = agentName,
- .timestamp = timestamp
- }};
+ .frame = robotRootFrame,
+ .agent = agentName,
+ .timestamp = timestamp}};
return lookupTransform(query);
}
@@ -135,13 +128,14 @@ namespace armarx::armem::client::robot_state::localization
// }
- TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
+ TransformResult
+ TransformReader::lookupTransform(const TransformQuery& query) const
{
const auto& timestamp = query.header.timestamp;
ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
const IceUtil::Time durationEpsilon = IceUtil::Time::milliSeconds(-1);
- (void) durationEpsilon;
+ (void)durationEpsilon;
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -159,22 +153,20 @@ namespace armarx::armem::client::robot_state::localization
if (not qResult.success)
{
- return
- {
- .transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage
- };
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
}
- const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
+ const auto& localizationCoreSegment =
+ qResult.memory.getCoreSegment(properties.localizationSegment);
using armarx::armem::common::robot_state::localization::TransformHelper;
return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 433694ec819859bdcda209dd57aad21d22512116..4d9ee5983e783e41b7e24cbce982ca65e8ed7057 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -40,15 +40,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformReader :
- virtual public TransformReaderInterface
+ class TransformReader : virtual public TransformReaderInterface
{
public:
- TransformReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformReader() = default;
+ TransformReader(const TransformReader&) = default;
~TransformReader() override;
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
@@ -59,18 +59,15 @@ namespace armarx::armem::client::robot_state::localization
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string localizationSegment = "Localization";
} properties;
-
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 9f93ae53156e0da1e126ed7cc922905eb2f0796b..f01f8e9241a5ebc605c6b158f3256407a5e9bd17 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -21,45 +21,38 @@
#include "TransformWriter.h"
-#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
+#include <optional>
#include <Eigen/Geometry>
#include <IceUtil/Time.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.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/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformWriter::TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformWriter::~TransformWriter() = default;
- void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
@@ -69,15 +62,17 @@ namespace armarx::armem::client::robot_state::localization
"Name of the localization memory core segment to use.");
}
- void TransformWriter::connect()
+ void
+ TransformWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '"
+ << properties.coreSegmentID.memoryName << "' ...";
try
{
memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '"
+ << properties.coreSegmentID << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -86,15 +81,17 @@ namespace armarx::armem::client::robot_state::localization
}
}
- bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
+ bool
+ TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
- const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
+ const MemoryID providerId =
+ properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
// const auto& timestamp = transform.header.timestamp;
- const MemoryID entityID = providerId.withEntityName(
- transform.header.parentFrame + "," + transform.header.frame);
- const Time timestamp = Time::Now(); // FIXME remove
+ const MemoryID entityID =
+ providerId.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ const Time timestamp = Time::Now(); // FIXME remove
armem::EntityUpdate update;
update.entityID = entityID;
@@ -117,4 +114,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index e553b073464e5fcf1444e14a4fef48e2034e808c..182981846d14eb775256b3a29aeed9e2a4eb483b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -29,7 +29,6 @@
#include "interfaces.h"
-
namespace armarx::armem::client::robot_state::localization
{
@@ -44,17 +43,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformWriter :
- virtual public TransformWriterInterface
+ class TransformWriter : virtual public TransformWriterInterface
{
public:
-
- TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformWriter() = default;
~TransformWriter() override;
// TransformWriterInterface
/// to be called in Component::onConnectComponent
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
@@ -63,18 +60,16 @@ namespace armarx::armem::client::robot_state::localization
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- MemoryID coreSegmentID { "RobotState", "Localization" };
+ MemoryID coreSegmentID{"RobotState", "Localization"};
} properties;
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
index ae35822c6080701195c0e6d7d8b6c8c4ad282684..5689058cc27f39dbe86b0e5e1b3171466407ac50 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -41,7 +42,7 @@ namespace armarx::armem::client::robot_state::localization
virtual ~TransformInterface() = default;
virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem) = 0;
};
class TransformReaderInterface : virtual public TransformInterface
@@ -65,4 +66,4 @@ namespace armarx::armem::client::robot_state::localization
virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
};
-} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
index abd89ebd82a7d30d19c1945444d1dd8f5d650bfd..9b247cb3973cf28befe37289a3ff42160133f21b 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -11,72 +11,118 @@ The memory should look like the following:
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::skills::arondto::SkillDescription'>
+ <Object name='armarx::skills::arondto::ProviderID'>
+ <ObjectChild key='providerName'>
+ <String />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::skills::arondto::SkillID'>
+ <ObjectChild key='providerId'>
+ <armarx::skills::arondto::ProviderID />
+ </ObjectChild>
<ObjectChild key='skillName'>
<String />
</ObjectChild>
+ </Object>
- <ObjectChild key='robots'>
- <List>
- <String />
- </List>
+
+ <!-- TODO -->
+ <Object name='armarx::skills::arondto::SkillProfile'>
+
+ <ObjectChild key='profileName'>
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key='predecessorProfileName'>
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key='parameterization'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::skills::arondto::SkillDescription'>
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
<ObjectChild key='description'>
<String />
</ObjectChild>
+ <ObjectChild key='rootProfileParameterization'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
<ObjectChild key='iceInfo'>
<String />
</ObjectChild>
- <ObjectChild key='timeoutMs'>
- <int64 />
+ <ObjectChild key='timeout'>
+ <Duration />
+ </ObjectChild>
+
+ <ObjectChild key='parametersType'>
+ <AnyObject shared_ptr="1" />
</ObjectChild>
- <ObjectChild key='acceptedType'>
+ <ObjectChild key='resultType'>
<AnyObject shared_ptr="1" />
</ObjectChild>
</Object>
- <Object name='armarx::skills::arondto::SkillExecutionRequest'>
+ <!-- TODO -->
+ <Object name='armarx::skills::arondto::CompositeSkillExecutionParams'>
<ObjectChild key='providerName'>
- <String />
+ <List>
+ <String /> <!--<armarx::skills::arondto::SkillExecutionInfo />-->
+ </List>
</ObjectChild>
+ </Object>
- <ObjectChild key='skillName'>
- <String />
+ <Object name='armarx::skills::arondto::SkillExecutionRequest'>
+
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
<ObjectChild key='executorName'>
<String />
</ObjectChild>
- <ObjectChild key='params'>
+ <ObjectChild key='parameters'>
<AnyObject shared_ptr="1" />
</ObjectChild>
-
</Object>
- <Object name='armarx::skills::arondto::SkillExecutionEvent'>
- <ObjectChild key='providerName'>
- <String />
+ <Object name='armarx::skills::arondto::SkillStatusUpdate'>
+
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
- <ObjectChild key='skillName'>
+ <ObjectChild key='executorName'>
<String />
</ObjectChild>
+ <ObjectChild key='executionStartedTimestamp'>
+ <DateTime />
+ </ObjectChild>
+
<ObjectChild key='status'>
<String />
</ObjectChild>
- <ObjectChild key='params'>
+ <ObjectChild key='parameters'>
<AnyObject shared_ptr="1" />
</ObjectChild>
- <ObjectChild key='data'>
+ <ObjectChild key='result'>
<AnyObject shared_ptr="1" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
index 1d8383b68a8ce2620a48718ed7584814c29b276a..3b1d8925fc6694ba7117a4ce3e0db07ecab68586 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
@@ -1,26 +1,50 @@
#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/converter/datatype/DatatypeConverterVisitor.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
namespace armarx::armem
{
- std::map<armarx::eStateType, armarx::skills::arondto::Statechart::StateType> toAronStateTypeMap =
- {
- {eNormalState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::NORMAL},
- {eFinalState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::FINAL},
- {eRemoteState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::REMOTE},
- {eDynamicRemoteState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE},
- {eUndefined, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::UNDEFINED},
+ std::map<armarx::eStateType, armarx::skills::arondto::Statechart::StateType>
+ toAronStateTypeMap = {
+ {eNormalState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::NORMAL},
+ {eFinalState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::FINAL},
+ {eRemoteState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::REMOTE},
+ {eDynamicRemoteState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE},
+ {eUndefined,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::UNDEFINED},
};
- std::map<armarx::skills::arondto::Statechart::StateType, armarx::eStateType> fromAronStateTypeMap =
- {
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::NORMAL, eNormalState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::FINAL, eFinalState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::REMOTE, eRemoteState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE, eDynamicRemoteState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::UNDEFINED, eUndefined},
+ std::map<armarx::skills::arondto::Statechart::StateType, armarx::eStateType>
+ fromAronStateTypeMap = {
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::NORMAL,
+ eNormalState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::FINAL,
+ eFinalState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::REMOTE,
+ eRemoteState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE,
+ eDynamicRemoteState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::UNDEFINED,
+ eUndefined},
};
- void fromAron(const skills::arondto::Statechart::StateType& dto, eStateType& bo)
+ void
+ fromAron(const skills::arondto::Statechart::StateType& dto, eStateType& bo)
{
if (fromAronStateTypeMap.find(dto) != fromAronStateTypeMap.end())
{
@@ -32,7 +56,8 @@ namespace armarx::armem
}
}
- void toAron(skills::arondto::Statechart::StateType& dto, const eStateType& bo)
+ void
+ toAron(skills::arondto::Statechart::StateType& dto, const eStateType& bo)
{
if (toAronStateTypeMap.find(bo) != toAronStateTypeMap.end())
{
@@ -44,7 +69,8 @@ namespace armarx::armem
}
}
- void fromAron(const skills::arondto::Statechart::ParameterMap& dto, StateParameterMap& bo)
+ void
+ fromAron(const skills::arondto::Statechart::ParameterMap& dto, StateParameterMap& bo)
{
// todo: implement
// for (auto const& [key, val] : dto.parameters)
@@ -54,7 +80,8 @@ namespace armarx::armem
// }
}
- void toAron(skills::arondto::Statechart::ParameterMap& dto, const StateParameterMap& bo)
+ void
+ toAron(skills::arondto::Statechart::ParameterMap& dto, const StateParameterMap& bo)
{
for (auto const& [key, val] : bo)
{
@@ -62,7 +89,9 @@ namespace armarx::armem
}
}
- void fromAron(const skills::arondto::Statechart::Transition& dto, ProfilerStatechartTransitionWithParameters& bo)
+ void
+ fromAron(const skills::arondto::Statechart::Transition& dto,
+ ProfilerStatechartTransitionWithParameters& bo)
{
bo.processId = dto.processId;
bo.sourceStateIdentifier = dto.sourceStateIdentifier;
@@ -74,7 +103,9 @@ namespace armarx::armem
fromAron(dto.outputParameters, bo.outputParameters);
}
- void toAron(skills::arondto::Statechart::Transition& dto, const ProfilerStatechartTransitionWithParameters& bo)
+ void
+ toAron(skills::arondto::Statechart::Transition& dto,
+ const ProfilerStatechartTransitionWithParameters& bo)
{
dto.processId = bo.processId;
dto.sourceStateIdentifier = bo.sourceStateIdentifier;
@@ -85,4 +116,122 @@ namespace armarx::armem
toAron(dto.localParameters, bo.localParameters);
toAron(dto.outputParameters, bo.outputParameters);
}
-}
+
+ void
+ fromAron(const armarx::skills::arondto::ProviderID& dto, skills::ProviderID& bo)
+ {
+ bo.providerName = dto.providerName;
+ }
+
+ void
+ toAron(armarx::skills::arondto::ProviderID& dto, const skills::ProviderID& bo)
+ {
+ dto.providerName = bo.providerName;
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillID& dto, skills::SkillID& bo)
+ {
+ bo.skillName = dto.skillName;
+ bo.providerId = skills::ProviderID{.providerName = ""};
+ fromAron(dto.providerId, *bo.providerId);
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillID& dto, const skills::SkillID& bo)
+ {
+ dto.skillName = bo.skillName;
+ dto.providerId.resetHard();
+ if (bo.providerId.has_value())
+ {
+ toAron(dto.providerId, *bo.providerId);
+ }
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillDescription& dto, skills::SkillDescription& bo)
+ {
+ fromAron(dto.skillId, bo.skillId);
+ bo.description = dto.description;
+ bo.timeout = dto.timeout;
+ bo.rootProfileDefaults = dto.rootProfileParameterization;
+ if (dto.parametersType)
+ {
+ throw armarx::LocalException("Not implemented yet");
+ }
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillDescription& dto, const skills::SkillDescription& bo)
+ {
+ toAron(dto.skillId, bo.skillId);
+ dto.description = bo.description;
+ dto.timeout = bo.timeout;
+ dto.rootProfileParameterization = bo.rootProfileDefaults;
+ if (bo.parametersType)
+ {
+ aron::type::converter::AronDatatypeConverterVisitor c;
+ aron::type::visit(c, bo.parametersType);
+
+ dto.parametersType = aron::data::Dict::DynamicCastAndCheck(c.latest);
+ }
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillExecutionRequest& dto,
+ skills::SkillExecutionRequest& bo)
+ {
+ fromAron(dto.skillId, bo.skillId);
+ bo.executorName = dto.executorName;
+ bo.parameters = dto.parameters;
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillExecutionRequest& dto,
+ const skills::SkillExecutionRequest& bo)
+ {
+ toAron(dto.skillId, bo.skillId);
+ dto.executorName = bo.executorName;
+ dto.parameters = bo.parameters;
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto, skills::SkillStatusUpdate& bo)
+ {
+ static std::map<std::string, skills::SkillStatus> map{
+ {"Constructing", skills::SkillStatus::Constructing},
+ {"Initializing", skills::SkillStatus::Initializing},
+ {"Preparing", skills::SkillStatus::Preparing},
+ {"Running", skills::SkillStatus::Running},
+ {"Failed", skills::SkillStatus::Failed},
+ {"Aborted", skills::SkillStatus::Aborted},
+ {"Succeeded", skills::SkillStatus::Succeeded}};
+
+ fromAron(dto.skillId, bo.executionId.skillId);
+ bo.executionId.executionStartedTime = dto.executionStartedTimestamp;
+ bo.executionId.executorName = dto.executorName;
+ bo.parameters = dto.parameters;
+ bo.result = dto.result;
+ bo.status = map.at(dto.status);
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillStatusUpdate& dto, const skills::SkillStatusUpdate& bo)
+ {
+ static std::map<skills::SkillStatus, std::string> map{
+ {skills::SkillStatus::Constructing, "Constructing"},
+ {skills::SkillStatus::Initializing, "Initializing"},
+ {skills::SkillStatus::Preparing, "Preparing"},
+ {skills::SkillStatus::Running, "Running"},
+ {skills::SkillStatus::Failed, "Failed"},
+ {skills::SkillStatus::Aborted, "Aborted"},
+ {skills::SkillStatus::Succeeded, "Succeeded"}};
+
+ toAron(dto.skillId, bo.executionId.skillId);
+ dto.executorName = bo.executionId.executorName;
+ dto.executionStartedTimestamp = bo.executionId.executionStartedTime;
+ dto.parameters = bo.parameters;
+ dto.result = bo.result;
+ dto.status = map.at(bo.status);
+ }
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.h b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
index 37092255c9dd47c5204907a23128be857456d146..38ffbe0c43ecc492cf76398abdd2f3cad122db7c 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
@@ -3,16 +3,45 @@
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
+#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
namespace armarx::armem
{
- void fromAron(const armarx::skills::arondto::Statechart::StateType& dto, armarx::eStateType& bo);
+ void fromAron(const armarx::skills::arondto::Statechart::StateType& dto,
+ armarx::eStateType& bo);
void toAron(armarx::skills::arondto::Statechart::StateType& dto, const armarx::eStateType& bo);
- void fromAron(const armarx::skills::arondto::Statechart::ParameterMap& dto, armarx::StateParameterMap& bo);
- void toAron(armarx::skills::arondto::Statechart::ParameterMap& dto, const armarx::StateParameterMap& bo);
+ void fromAron(const armarx::skills::arondto::Statechart::ParameterMap& dto,
+ armarx::StateParameterMap& bo);
+ void toAron(armarx::skills::arondto::Statechart::ParameterMap& dto,
+ const armarx::StateParameterMap& bo);
- void fromAron(const armarx::skills::arondto::Statechart::Transition& dto, armarx::ProfilerStatechartTransitionWithParameters& bo);
- void toAron(armarx::skills::arondto::Statechart::Transition& dto, const armarx::ProfilerStatechartTransitionWithParameters& bo);
-}
+ void fromAron(const armarx::skills::arondto::Statechart::Transition& dto,
+ armarx::ProfilerStatechartTransitionWithParameters& bo);
+ void toAron(armarx::skills::arondto::Statechart::Transition& dto,
+ const armarx::ProfilerStatechartTransitionWithParameters& bo);
+
+ void fromAron(const armarx::skills::arondto::ProviderID& dto, skills::ProviderID& bo);
+ void toAron(armarx::skills::arondto::ProviderID& dto, const skills::ProviderID& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillID& dto, skills::SkillID& bo);
+ void toAron(armarx::skills::arondto::SkillID& dto, const skills::SkillID& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillDescription& dto,
+ skills::SkillDescription& bo);
+ void toAron(armarx::skills::arondto::SkillDescription& dto, const skills::SkillDescription& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillExecutionRequest& dto,
+ skills::SkillExecutionRequest& bo);
+ void toAron(armarx::skills::arondto::SkillExecutionRequest& dto,
+ const skills::SkillExecutionRequest& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto,
+ skills::SkillStatusUpdate& bo);
+ void toAron(armarx::skills::arondto::SkillStatusUpdate& dto,
+ const skills::SkillStatusUpdate& bo);
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
index 89e7e1c92af94e46c2493a5c18825c94b0551ac7..d2f99544ee1b2bde93a9c8609f0f100ac2baaf4f 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
@@ -2,8 +2,11 @@
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/datatype/DatatypeConverterVisitor.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
@@ -29,32 +32,16 @@ namespace armarx::skills::segment
}
void
- ExecutableSkillLibraryCoreSegment::addSkillProvider(
- const skills::manager::dto::ProviderInfo& info)
+ ExecutableSkillLibraryCoreSegment::addSkillProvider(const skills::ProviderInfo& info)
{
// add skills
auto skills = info.providedSkills;
+ auto provId = id().withProviderSegmentName(info.providerId.providerName);
- auto provId = id().withProviderSegmentName(info.providerName);
-
- for (const auto& [key, desc] : skills)
+ for (const auto& [s, d] : skills)
{
armarx::skills::arondto::SkillDescription skillDescription;
- skillDescription.skillName = desc.skillName;
- skillDescription.description = desc.description;
- skillDescription.iceInfo = info.provider->ice_toString();
- skillDescription.robots = desc.robots;
- skillDescription.timeoutMs = desc.timeoutMs;
-
- if (desc.acceptedType)
- {
- auto t = aron::type::Object::FromAronObjectDTO(desc.acceptedType);
-
- aron::type::converter::AronDatatypeConverterVisitor c;
- aron::type::visit(c, t);
-
- skillDescription.acceptedType = aron::data::Dict::DynamicCastAndCheck(c.latest);
- }
+ armem::toAron(skillDescription, d);
armem::Commit commit;
auto& entityUpdate = commit.add();
@@ -63,7 +50,7 @@ namespace armarx::skills::segment
entityUpdate.sentTime = armem::Time::Now();
entityUpdate.arrivedTime = armem::Time::Now();
entityUpdate.instancesData = {skillDescription.toAron()};
- entityUpdate.entityID = provId.withEntityName(skillDescription.skillName);
+ entityUpdate.entityID = provId.withEntityName(d.skillId.skillName);
// Commit data to memory and notify
iceMemory.commit(commit);
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
index a2d4227bb2c0789c028b31ef47beb21b68891ee4..e71a8a74ab714be9c9c7cd1faafb084a105e4576 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
@@ -4,19 +4,18 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
namespace armarx::skills::segment
{
- class ExecutableSkillLibraryCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class ExecutableSkillLibraryCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -25,10 +24,10 @@ namespace armarx::skills::segment
ExecutableSkillLibraryCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- void addSkillProvider(const skills::manager::dto::ProviderInfo& info);
+ void addSkillProvider(const skills::ProviderInfo& info);
void removeSkillProvider(const std::string& providerName);
size_t size() const;
@@ -36,4 +35,4 @@ namespace armarx::skills::segment
private:
std::map<std::string, std::map<std::string, skills::manager::dto::ProviderInfo>> skills;
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 1fa0989e8938206462674f93d9646f48b065123f..11dc8d0dfb40953de6acc8f81921884808339376 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -1,53 +1,44 @@
#include "SkillEventSegment.h"
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx::skills::segment
{
- SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
- Base(iceMemory, CoreSegmentName, armarx::skills::arondto::SkillExecutionRequest::ToAronType())
+ SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory,
+ CoreSegmentName,
+ armarx::skills::arondto::SkillExecutionRequest::ToAronType())
{
}
- void SkillEventCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix)
+ void
+ SkillEventCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
// No properties! (meaning no name and no max size)
}
- void SkillEventCoreSegment::init()
+ void
+ SkillEventCoreSegment::init()
{
Base::init();
}
- void SkillEventCoreSegment::addSkillUpdateEvent(const skills::provider::dto::SkillStatusUpdate& update)
+ void
+ SkillEventCoreSegment::addSkillUpdateEvent(const skills::SkillStatusUpdate& update)
{
- // add update for skill to memory
- static std::map<armarx::skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
- {armarx::skills::provider::dto::Execution::Status::Idle, "Idle"},
- {armarx::skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {armarx::skills::provider::dto::Execution::Status::Running, "Running"},
- {armarx::skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {armarx::skills::provider::dto::Execution::Status::Failed, "Failed"},
- {armarx::skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
- };
-
// create commit about new update
- armarx::skills::arondto::SkillExecutionEvent event;
- 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);
+ armarx::skills::arondto::SkillStatusUpdate event;
+ armem::toAron(event, update);
armem::MemoryID commitId = id();
- commitId.providerSegmentName = event.providerName;
- commitId.entityName = event.skillName;
+ commitId.providerSegmentName = event.skillId.providerId.providerName;
+ commitId.entityName = event.skillId.skillName;
auto aron = event.toAron();
@@ -55,9 +46,65 @@ namespace armarx::skills::segment
auto& entityUpdate = comm.add();
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.entityID = commitId;
- iceMemory.commit(comm);
+ iceMemory.commitLocking(comm);
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ SkillEventCoreSegment::getSkillStatusUpdates()
+ {
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
+ auto coreSegment = this->segmentPtr;
+ ARMARX_CHECK(coreSegment);
+
+ coreSegment->forEachInstance(
+ [&](const armem::wm::EntityInstance& i)
+ {
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
+ armem::fromAron(event, up);
+
+ if (auto it = ret.find(up.executionId); it != ret.end() && up < it->second)
+ {
+ return;
+ }
+
+ // set or replace
+ ret[up.executionId] = up;
+ });
+
+ // for (const auto& [k, v] : ret)
+ // {
+ // ARMARX_IMPORTANT << "Skill " << k.skillId << " has stati: " << int(v.status);
+ // }
+
+ return ret;
+ }
+
+ std::optional<skills::SkillStatusUpdate>
+ SkillEventCoreSegment::getSkillStatusUpdate(const skills::SkillExecutionID& id)
+ {
+ std::optional<skills::SkillStatusUpdate> ret = std::nullopt;
+ auto coreSegment = this->segmentPtr;
+ ARMARX_CHECK(coreSegment);
+
+ coreSegment->forEachInstance(
+ [&](const armem::wm::EntityInstance& i)
+ {
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
+ armem::fromAron(event, up);
+
+ if (up.executionId == id)
+ {
+ if (!ret || (ret.has_value() && *ret < up))
+ {
+ ret = up;
+ }
+ }
+ });
+ return ret;
}
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
index 93df4bf7a429a459ccd51603af035d96f3f9e3d6..0537cbdbdf97d80649e10baa30786d2d1fb5870c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
@@ -8,13 +8,12 @@
// ArmarX
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
namespace armarx::skills::segment
{
- class SkillEventCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class SkillEventCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -23,11 +22,16 @@ namespace armarx::skills::segment
SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- void addSkillUpdateEvent(const skills::provider::dto::SkillStatusUpdate& update);
+ void addSkillUpdateEvent(const skills::SkillStatusUpdate& update);
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillStatusUpdates();
+
+ std::optional<skills::SkillStatusUpdate>
+ getSkillStatusUpdate(const skills::SkillExecutionID& id);
private:
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 818f3d13bb4315ebf69909c744c042b1ac268a39..833deb3f68144a10a71812f9eb39dbec455d9c0c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -1,33 +1,36 @@
#include "SkillExecutionRequestSegment.h"
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx::skills::segment
{
- SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
+ SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, CoreSegmentName, skills::arondto::SkillExecutionRequest::ToAronType())
{
}
- void SkillExecutionRequestCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix)
+ void
+ SkillExecutionRequestCoreSegment::defineProperties(PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
// No properties! (meaning no name and no max size)
}
- void SkillExecutionRequestCoreSegment::init()
+ void
+ SkillExecutionRequestCoreSegment::init()
{
Base::init();
}
-
- skills::manager::dto::SkillExecutionRequest SkillExecutionRequestCoreSegment::convertCommit(const aron::data::dto::DictPtr& commitData)
+ skills::SkillExecutionRequest
+ SkillExecutionRequestCoreSegment::convertCommit(const aron::data::dto::DictPtr& commitData)
{
// convert ice commitData to aron
auto commitDataAron = std::make_shared<aron::data::Dict>(commitData);
@@ -36,23 +39,22 @@ namespace armarx::skills::segment
skills::arondto::SkillExecutionRequest request;
request.fromAron(commitDataAron);
- skills::manager::dto::SkillExecutionRequest info;
- info.skillId = {request.providerName, request.skillName};
- info.params = request.params->toAronDictDTO();
+ skills::SkillExecutionRequest info{
+ .skillId = SkillID{.providerId = ProviderID{.providerName = ""}, .skillName = ""},
+ .executorName = ""};
+ armem::fromAron(request, info);
return info;
}
-
- void SkillExecutionRequestCoreSegment::addSkillExecutionRequest(const skills::manager::dto::SkillExecutionRequest& info)
+ void
+ SkillExecutionRequestCoreSegment::addSkillExecutionRequest(
+ const skills::SkillExecutionRequest& info)
{
// override directly execution to add a request to the memory
armem::Commit comm;
skills::arondto::SkillExecutionRequest request;
- request.executorName = info.executorName;
- request.providerName = info.skillId.providerName;
- request.skillName = info.skillId.skillName;
- request.params = aron::data::Dict::FromAronDictDTO(info.params);
+ armem::toAron(request, info);
auto aron = request.toAron();
@@ -60,11 +62,11 @@ namespace armarx::skills::segment
auto& entityUpdate = comm.add();
armem::MemoryID skillExecutionMemID = id();
- skillExecutionMemID.providerSegmentName = request.providerName;
- skillExecutionMemID.entityName = request.skillName;
+ skillExecutionMemID.providerSegmentName = request.skillId.providerId.providerName;
+ skillExecutionMemID.entityName = request.skillId.skillName;
entityUpdate.entityID = skillExecutionMemID;
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
}
@@ -77,7 +79,7 @@ namespace armarx::skills::segment
skillExecutionMemID.entityName = "All Skill Execution Requests";
entityUpdate.entityID = skillExecutionMemID;
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
}
@@ -85,4 +87,4 @@ namespace armarx::skills::segment
iceMemory.commit(comm);
}
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
index 262bc829b6906061e92e725f88a00b6bec6ed0c6..c5af9dd8a64cff67a2e2088f1c714718f3126203 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
@@ -4,17 +4,17 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
namespace armarx::skills::segment
{
- class SkillExecutionRequestCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class SkillExecutionRequestCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -23,11 +23,11 @@ namespace armarx::skills::segment
SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- skills::manager::dto::SkillExecutionRequest convertCommit(const aron::data::dto::DictPtr& commitData);
+ skills::SkillExecutionRequest convertCommit(const aron::data::dto::DictPtr& commitData);
- void addSkillExecutionRequest(const skills::manager::dto::SkillExecutionRequest& info);
+ void addSkillExecutionRequest(const skills::SkillExecutionRequest& info);
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 21235afefb794e8c78443e840a240f56c6720b14..1d85a7f800b038befc8dedd21561c133a1f13439 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -193,6 +193,36 @@ namespace armarx::aron::data
setElementCopy(key, data);
}
+ void
+ Dict::mergeAndReplace(const DictPtr& d)
+ {
+ if (d == nullptr)
+ {
+ return;
+ }
+
+ // merge and overwrite
+ for (const auto& [k, v] : getElements())
+ {
+ this->setElement(k, v);
+ }
+ }
+
+ void
+ Dict::mergeAndReplaceCopy(const DictPtr& d)
+ {
+ if (d == nullptr)
+ {
+ return;
+ }
+
+ // merge and overwrite
+ for (const auto& [k, v] : getElements())
+ {
+ this->setElementCopy(k, v);
+ }
+ }
+
bool
Dict::hasElement(const std::string& key) const
{
@@ -231,8 +261,9 @@ namespace armarx::aron::data
const auto& p = data->getPath();
if (not p.hasDirectPrefix(this->getPath()))
{
- ARMARX_WARNING << "An element added to a dict does not have a correct path set. This "
- "may cause errors. Please use setElemetCopy() instead.";
+ ARMARX_WARNING
+ << "An element added to a dict does not have a correct path set. This "
+ "may cause errors. Please use setElemetCopy() instead.";
}
}
@@ -316,65 +347,68 @@ namespace armarx::aron::data
{
ARMARX_TRACE;
auto objectTypeNav = type::Object::DynamicCastAndCheck(type);
- for (const auto& [key, nav] : childrenNavigators)
+
+ // here we need to iterate over the elements of the type. That must be fulfilled.
+ // If this dict has more members that its fine.
+ for (const auto& [key, childTypeNav] : objectTypeNav->getMemberTypes())
{
- if (!objectTypeNav->hasMemberType(key))
+ if (!this->hasElement(key))
{
- ARMARX_TRACE;
- return false;
+ return false; // key must exist
}
- auto childTypeNav = objectTypeNav->getMemberType(key);
- if (!nav)
+ if (!childTypeNav)
{
- ARMARX_TRACE;
- if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ continue; // no information whih is fine --> continue
+ }
+
+ auto childNav = this->getElement(key);
+ if (childNav)
+ {
+ if (not childNav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ // else childnav fulfills type which is fine --> continue
+ continue;
}
- if (!nav->fullfillsType(childTypeNav))
+ else
{
- ARMARX_TRACE;
- return false;
+ if (childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ return false;
+ }
+ // else: childTypeNav == maybe && nav == null which is fine --> continue
+ continue;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::DICT:
{
ARMARX_TRACE;
auto dictTypeNav = type::Dict::DynamicCastAndCheck(type);
- for (const auto& [key, nav] : childrenNavigators)
+ for (const auto& [key, childNav] : childrenNavigators)
{
(void)key;
auto childTypeNav = dictTypeNav->getAcceptedType();
- if (!nav)
+ if (!childNav)
{
- ARMARX_TRACE;
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ // else childTypeNav is null or maybe and childNav is null which is fine.
+ continue;
}
- if (!nav->fullfillsType(childTypeNav))
+ if (!childNav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
default:
- ARMARX_TRACE;
return false;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index f6c2d93955f53af91a04364ada5f3da724ae7033..a49add482dc4e7e6f1a1d5308d76e571785dbe73 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -73,6 +73,9 @@ namespace armarx::aron::data
VariantPtr getElement(const std::string&) const;
std::map<std::string, VariantPtr> getElements() const;
+ void mergeAndReplace(const DictPtr& d);
+ void mergeAndReplaceCopy(const DictPtr& d);
+
VariantPtr at(const std::string&) const;
void removeElement(const std::string& key);
void clear();
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 83e23f76f43ca4a7ebf8b057c51142256b5ba618..82955eabfa8414f5e984c6ad0de704d7cab09a00 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -271,59 +271,51 @@ namespace armarx::aron::data
{
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ continue;
}
if (!nav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::TUPLE:
{
+ ARMARX_TRACE;
auto tupleTypeNav = type::Tuple::DynamicCastAndCheck(type);
unsigned int i = 0;
- for (const auto& nav : childrenNavigators)
+ for (const auto& childTypeNav : tupleTypeNav->getAcceptedTypes())
{
- if (!tupleTypeNav->hasAcceptedType(i))
+ if (!this->hasElement(i))
{
- ARMARX_TRACE;
return false;
}
- auto childTypeNav = tupleTypeNav->getAcceptedType(i);
- if (!nav)
+ auto childNav = this->getElement(i);
+ if (!childNav)
{
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
+ continue;
}
- if (!nav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
+ if (!childNav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::PAIR:
{
+ ARMARX_TRACE;
auto pairTypeNav = type::Pair::DynamicCastAndCheck(type);
if (childrenSize() != 2)
{
- ARMARX_TRACE;
return false;
}
auto firstChildTypeNav = pairTypeNav->getFirstAcceptedType();
@@ -331,24 +323,17 @@ namespace armarx::aron::data
{
if (firstChildTypeNav && firstChildTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
}
auto secondChildTypeNav = pairTypeNav->getSecondAcceptedType();
if (!childrenNavigators[1])
{
if (secondChildTypeNav && secondChildTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
}
- ARMARX_TRACE;
return childrenNavigators[0]->fullfillsType(firstChildTypeNav) &&
childrenNavigators[1]->fullfillsType(secondChildTypeNav);
}
diff --git a/source/RobotAPI/libraries/robotapi/CMakeLists.txt b/source/RobotAPI/libraries/robotapi/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d62c2b5f8297012bfb4be7c6a90f74ac6d542c11
--- /dev/null
+++ b/source/RobotAPI/libraries/robotapi/CMakeLists.txt
@@ -0,0 +1,3 @@
+add_subdirectory(core)
+add_subdirectory(client)
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 53f82b11a33901183e7eb321e7b3a18829f15527..372d6ddfe231c98a882aa5d3d4fa53523bfdf4be 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -1,59 +1,23 @@
+add_subdirectory(core)
+add_subdirectory(provider)
+add_subdirectory(manager)
+
+
+
set(LIB_NAME RobotAPISkills)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
- LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- ArmarXCoreObservers
-
- RobotAPI::Core
- aronjsonconverter
- arondatatypeconverter
-
- SOURCES
- ./error/Exception.cpp
- ./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/LambdaSkill.cpp
- ./provider/SkillContext.cpp
- ./provider/SkillID.cpp
- ./provider/detail/SkillImplementationWrapper.cpp
+ LIBS
+ RobotAPISkillsCore
+ RobotAPISkillsProvider
+ RobotAPISkillsManager
+ SOURCES
+ skills.cpp
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/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
+ skills.h
)
add_library(RobotAPI::skills ALIAS RobotAPISkills)
diff --git a/source/RobotAPI/libraries/skills/core/CMakeLists.txt b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..29145e60e305fa10decf06200685dcb99ec6a3d5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
@@ -0,0 +1,44 @@
+set(LIB_NAME RobotAPISkillsCore)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ error/Exception.cpp
+ SkillID.cpp
+ ProviderID.cpp
+ ProviderInfo.cpp
+ SkillExecutionRequest.cpp
+ SkillStatusUpdate.cpp
+ SkillExecutionID.cpp
+ SkillPreparationInput.cpp
+ SkillParameterization.cpp
+ Skill.cpp
+ SkillProxy.cpp
+ SkillDescription.cpp
+ HEADERS
+ error/Exception.h
+ SkillID.h
+ ProviderID.h
+ ProviderInfo.h
+ SkillExecutionRequest.h
+ SkillStatusUpdate.h
+ SkillExecutionID.h
+ SkillPreparationInput.h
+ SkillParameterization.h
+ Skill.h
+ SkillProxy.h
+ SkillDescription.h
+)
+
+add_library(RobotAPI::skills::core ALIAS RobotAPISkillsCore)
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.cpp b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53d6aad6807a7002a80a097d94b1e1fb5ec9f864
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
@@ -0,0 +1,69 @@
+#include "ProviderID.h"
+
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ bool
+ ProviderID::operator==(const ProviderID& other) const
+ {
+ return toString() == other.toString();
+ }
+
+ bool
+ ProviderID::operator!=(const ProviderID& other) const
+ {
+ return not(*this == other);
+ }
+
+ bool
+ ProviderID::operator<(const ProviderID& other) const
+ {
+ return toString() < other.toString();
+ }
+
+ bool
+ ProviderID::operator<=(const ProviderID& other) const
+ {
+ return toString() <= other.toString();
+ }
+
+ ProviderID
+ ProviderID::FromIce(const manager::dto::ProviderID& s)
+ {
+ return ProviderID{.providerName = s.providerName};
+ }
+
+ ProviderID
+ ProviderID::FromIce(const callback::dto::ProviderID& s)
+ {
+ return ProviderID{.providerName = s.providerName};
+ }
+
+ manager::dto::ProviderID
+ ProviderID::toManagerIce() const
+ {
+ return {providerName};
+ }
+
+ callback::dto::ProviderID
+ ProviderID::toCallbackIce() const
+ {
+ return {providerName};
+ }
+
+ std::string
+ ProviderID::toString() const
+ {
+ return providerName;
+ }
+ } // namespace skills
+
+ std::ostream&
+ skills::operator<<(std::ostream& os, const ProviderID& id)
+ {
+ return os << "'" << id.toString() << "'";
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.h b/source/RobotAPI/libraries/skills/core/ProviderID.h
new file mode 100644
index 0000000000000000000000000000000000000000..5687f649185fa472d0afe1b7a60e6eb2eba46d34
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillID;
+
+ class ProviderID
+ {
+ public:
+ bool operator==(const ProviderID& other) const;
+ bool operator!=(const ProviderID& other) const;
+ bool operator<(const ProviderID& other) const;
+ bool operator<=(const ProviderID& other) const;
+
+ manager::dto::ProviderID toManagerIce() const;
+ callback::dto::ProviderID toCallbackIce() const;
+
+ static ProviderID FromIce(const manager::dto::ProviderID&);
+ static ProviderID FromIce(const callback::dto::ProviderID&);
+
+ std::string toString() const;
+
+ std::string providerName;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const ProviderID& id);
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..135b81c594aaf004d65170c81f65a1a6b9529a07
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
@@ -0,0 +1,32 @@
+#include "ProviderInfo.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ skills::manager::dto::ProviderInfo
+ ProviderInfo::toIce() const
+ {
+ skills::manager::dto::ProviderInfo ret;
+ ret.providerId = providerId.toManagerIce();
+ ret.providerInterface = providerInterface;
+
+ for (const auto& [k, v] : providedSkills)
+ {
+ ret.providedSkills[k.toManagerIce()] = v.toManagerIce();
+ }
+ return ret;
+ }
+
+ ProviderInfo
+ ProviderInfo::FromIce(const manager::dto::ProviderInfo& i)
+ {
+ std::map<SkillID, SkillDescription> m;
+ for (const auto& [k, v] : i.providedSkills)
+ {
+ m.insert({skills::SkillID::FromIce(k), skills::SkillDescription::FromIce(v)});
+ }
+ return ProviderInfo{skills::ProviderID::FromIce(i.providerId), i.providerInterface, m};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.h b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
new file mode 100644
index 0000000000000000000000000000000000000000..438a2cf9d894f25f97150dc10ab6c5d9538cdc1e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "ProviderID.h"
+#include "SkillDescription.h"
+#include "SkillID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class ProviderInfo
+ {
+ public:
+ ProviderID providerId;
+ provider::dti::SkillProviderInterfacePrx providerInterface;
+ std::map<SkillID, SkillDescription> providedSkills;
+
+ skills::manager::dto::ProviderInfo toIce() const;
+
+ static ProviderInfo FromIce(const manager::dto::ProviderInfo&);
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..34895c9066ca7202bb6e96c2885869f22ec26875
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -0,0 +1,386 @@
+#include "Skill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ Skill::Skill(const SkillDescription& desc) : description(desc), constructing(true)
+ {
+ // replace constructor if you want to have a specific logging tag
+ Logging::setTag("armarx::skills::" + description.skillId.toString());
+ }
+
+ // install a local condition via a lambda
+ void
+ Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
+ {
+ std::scoped_lock l(conditionCallbacksMutex);
+ conditionCallbacks.push_back({f, cb});
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillProxy& prx, const aron::data::DictPtr& params)
+ {
+ auto eid = callSubskillAsync(prx, params);
+ auto ret = prx.join(eid);
+ ARMARX_IMPORTANT << "FINISHED SUBSKILL. RET IS " << ret.has_value();
+ return ret;
+ }
+
+ skills::SkillExecutionID
+ Skill::callSubskillAsync(const skills::SkillProxy& prx, const aron::data::DictPtr& params)
+ {
+ std::unique_lock l(subskillsMutex);
+
+ std::string executorHistory = this->executorName + "->" + getSkillId().toString();
+ auto eid = prx.executeSkillAsync(executorHistory, params);
+ this->subskills.push_back(eid);
+ return eid;
+ }
+
+ void
+ Skill::updateParameters(const aron::data::DictPtr& d)
+ {
+ std::scoped_lock l(this->parametersMutex);
+ if (this->parameters == nullptr)
+ {
+ // set params as there has been no update before.
+ this->parameters = d;
+ }
+ else
+ {
+ // merge params into existing. Note that this may update already set params.
+ this->parameters->mergeAndReplaceCopy(d);
+ }
+ }
+
+ void
+ Skill::setParameters(const aron::data::DictPtr& d)
+ {
+ // we only set the params if the skill is not already running
+ if (running or exiting or finished)
+ {
+ return;
+ }
+
+ std::scoped_lock l(this->parametersMutex);
+ this->parameters = d;
+ }
+
+ aron::data::DictPtr
+ Skill::getParameters() const
+ {
+ std::scoped_lock l(this->parametersMutex);
+ return this->parameters;
+ }
+
+ Skill::InitResult
+ Skill::_init()
+ {
+ //ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
+ this->initializing = true;
+ this->constructing = false;
+ this->preparing = false;
+ this->running = false;
+ this->exiting = false;
+ this->finished = false;
+
+ // install timeout condition
+ installConditionWithCallback(
+ [&]() {
+ return (armarx::core::time::DateTime::Now() >= (started + description.timeout));
+ },
+ [&]() { notifyTimeoutReached(); });
+
+ conditionCheckingThread = std::thread(
+ [&]()
+ {
+ armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
+ while (initializing or preparing or
+ running) // when the skill ends/aborts this variable will be set to false
+ {
+ {
+ std::scoped_lock l(conditionCallbacksMutex);
+ for (auto& p : conditionCallbacks)
+ {
+ auto& f = p.first;
+ auto& cb = p.second;
+ if (f())
+ {
+ cb();
+ }
+ }
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_WARNING << deactivateSpam()
+ << "ConditionCheckingThread: execution took too long ("
+ << -sleepDuration << " vs "
+ << conditionCheckingThreadFrequency.toCycleDuration()
+ << ")";
+ }
+ }
+ });
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::PrepareResult
+ Skill::_prepare()
+ {
+ this->preparing = true;
+ this->initializing = false;
+ this->constructing = false;
+ this->running = false;
+ this->exiting = false;
+ this->finished = false;
+
+ if (shouldSkillTerminate())
+ {
+ return {.status = ActiveOrTerminatedSkillStatus::Aborted};
+ }
+
+ // Default nothing to prepare
+ if (not description.parametersType)
+ {
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+ if (this->parameters && this->parameters->fullfillsType(description.parametersType))
+ {
+ // wait until parameters fulfill type
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ // false if we have to wait for parameters
+ return {.status = ActiveOrTerminatedSkillStatus::Running};
+ }
+
+ Skill::MainResult
+ Skill::_main()
+ {
+ this->running = true;
+ this->initializing = false;
+ this->constructing = false;
+ this->preparing = false;
+ this->exiting = false;
+ this->finished = false;
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::ExitResult
+ Skill::_exit()
+ {
+ // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
+ this->exiting = true;
+ this->running = false;
+ this->initializing = false;
+ this->constructing = false;
+ this->preparing = false;
+ this->finished = false;
+
+ if (conditionCheckingThread.joinable())
+ {
+ conditionCheckingThread.join();
+ }
+ exited = armarx::core::time::DateTime::Now();
+
+ this->finished = true;
+ this->exiting = false;
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::InitResult
+ Skill::initSkill()
+ {
+ std::scoped_lock l(parametersMutex);
+ auto _res = this->_init();
+ auto res = this->init();
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
+ }
+
+ Skill::PrepareResult
+ Skill::prepareSkill()
+ {
+ std::scoped_lock l(parametersMutex);
+ auto _res = this->_prepare();
+ auto res = this->prepare();
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
+ }
+
+ Skill::MainResult
+ Skill::mainOfSkill()
+ {
+ std::scoped_lock l(parametersMutex);
+ auto _res = this->_main();
+ auto res = this->main();
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status),
+ .data = res.data};
+ }
+
+ Skill::ExitResult
+ Skill::exitSkill()
+ {
+ std::scoped_lock l(parametersMutex);
+ auto res = this->exit();
+ auto _res = this->_exit();
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
+ }
+
+ void
+ Skill::throwIfSkillShouldTerminate(const std::function<void()>& do_before,
+ const std::string& abortedMessage)
+ {
+ if (shouldSkillTerminate())
+ {
+ do_before();
+ throwIfSkillShouldTerminate(abortedMessage);
+ }
+ }
+
+ void
+ Skill::throwIfSkillShouldTerminate(const std::string& abortedMessage)
+ {
+ if (stopped)
+ {
+ std::string message =
+ std::string("The skill '" + getSkillId().toString() + "' was asked to stop.");
+ message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
+
+ throw error::SkillAbortedException(message);
+ return;
+ }
+
+ if (timeoutReached)
+ {
+ std::string message =
+ std::string("The skill '" + getSkillId().toString() + "' reached timeout.");
+ message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
+
+ ARMARX_WARNING << message;
+ throw error::SkillFailedException(message);
+ }
+ }
+
+ Skill::MainResult
+ Skill::MakeSucceededResult(aron::data::DictPtr data)
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Succeeded,
+ .data = data,
+ };
+ }
+
+ Skill::MainResult
+ Skill::MakeFailedResult()
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Failed,
+ .data = nullptr,
+ };
+ }
+
+ Skill::MainResult
+ Skill::MakeAbortedResult()
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Aborted,
+ .data = nullptr,
+ };
+ }
+
+ void
+ Skill::notifySkillToStop()
+ {
+ std::scoped_lock l(subskillsMutex);
+ stopped = true;
+ _onStopRequested();
+ onStopRequested();
+ }
+
+ void
+ Skill::notifyTimeoutReached()
+ {
+ std::scoped_lock l(subskillsMutex);
+ timeoutReached = true;
+ _onTimeoutReached();
+ onTimeoutReached();
+ }
+
+ bool
+ Skill::shouldSkillTerminate() const
+ {
+ return stopped || timeoutReached;
+ }
+
+ // condition effects
+ void
+ Skill::_onTimeoutReached()
+ {
+ if (!manager)
+ {
+ return;
+ }
+ for (const auto& execId : subskills)
+ {
+ manager->abortSkillAsync(execId.toManagerIce());
+ }
+ }
+
+ void
+ Skill::_onStopRequested()
+ {
+ if (!manager)
+ {
+ return;
+ }
+ for (const auto& execId : subskills)
+ {
+ manager->abortSkillAsync(execId.toManagerIce());
+ }
+ }
+
+ void
+ Skill::onTimeoutReached()
+ {
+ }
+
+ void
+ Skill::onStopRequested()
+ {
+ }
+
+ // always called before prepare (should not take longer than 100ms)
+ Skill::InitResult
+ Skill::init()
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ // always called before main (should not take longer than 100ms)
+ Skill::PrepareResult
+ Skill::prepare()
+ {
+ // Default nothing to prepare
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ // always called after main or if skill fails (should not take longer than 100ms)
+ Skill::ExitResult
+ Skill::exit()
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ Skill::main()
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ec80e77d9956e8516ae84ffec16313bab0c9206
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -0,0 +1,247 @@
+#pragma once
+
+// std/stl
+#include <functional>
+#include <mutex>
+#include <queue>
+#include <thread>
+
+// base class
+#include <ArmarXCore/core/logging/Logging.h>
+
+// ArmarX
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+#include "SkillDescription.h"
+#include "SkillID.h"
+#include "SkillPreparationInput.h"
+#include "SkillProxy.h"
+#include "SkillStatusUpdate.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class Skill : public armarx::Logging
+ {
+ public:
+ using CallbackT =
+ std::function<void(const SkillStatus s, const armarx::aron::data::DictPtr&)>;
+
+ /// A result struct for skill initialization
+ struct InitResult
+ {
+ TerminatedSkillStatus status;
+ };
+
+ /// A result struct for skill preparing
+ struct PrepareResult
+ {
+ ActiveOrTerminatedSkillStatus status;
+ };
+
+ /// A result struct for th main method of a skill
+ struct MainResult
+ {
+ TerminatedSkillStatus status;
+ aron::data::DictPtr data = nullptr;
+ };
+
+ /// A result struct for skill exit function
+ struct ExitResult
+ {
+ TerminatedSkillStatus status;
+ };
+
+ /// We completely remove the default constructor! A skill without a desciption cannot exist
+ Skill() = delete;
+
+ /// Constructor of a skill for inheritance. Every skill must have a skill description
+ Skill(const SkillDescription&);
+
+ /// Virtual destructor of a skill
+ virtual ~Skill()
+ {
+ //ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
+ }
+
+ /// Get the id of the skill
+ SkillID
+ getSkillId() const
+ {
+ return description.skillId;
+ }
+
+ /// Get the description of a skill
+ SkillDescription
+ getSkillDescription() const
+ {
+ return description;
+ }
+
+ /// Set the provider id of the description of the skill.
+ /// This method is called when creating a skill in a skill provider
+ void
+ setProviderId(const skills::ProviderID& pid)
+ {
+ description.skillId.providerId = pid;
+ }
+
+ void
+ setCallback(const CallbackT& callback)
+ {
+ this->callback = callback;
+ }
+
+ void
+ setManager(const manager::dti::SkillManagerInterfacePrx& manager)
+ {
+ this->manager = manager;
+ }
+
+ void
+ setExecutorName(const std::string& executorName)
+ {
+ this->executorName = executorName;
+ }
+
+ /// Prepare a skill once. This method is called in a loop as long as it returns RUNNING
+ /// If the loop does not terminate with SUCCEDED the skill execution is failed.
+ PrepareResult prepareSkill();
+
+ /// Initialization of a skill. Called directly after construction.
+ /// If this method does not return SUCCEEDED the skill execution is failed.
+ InitResult initSkill();
+
+ /// Main method of a skill.
+ MainResult mainOfSkill();
+
+ /// Exit method of a skill. It is guaranteed that exit is always called
+ /// (unless there is a segfault or similar)
+ ExitResult exitSkill();
+
+ /// Notify the skill from extern to stop
+ void notifySkillToStop();
+
+ /// Returns whether the skill should terminate as soon as possible
+ bool shouldSkillTerminate() const;
+
+ /// Merge parameters to the local parameters of the skill
+ void updateParameters(const aron::data::DictPtr& d);
+
+ /// Hard set the parameters, ignoring everything that has been set or merged before
+ void setParameters(const aron::data::DictPtr& d);
+
+ /// Get the parameters of a skill that have been set so far
+ aron::data::DictPtr getParameters() const;
+
+ protected:
+ void throwIfSkillShouldTerminate(const std::string& abortedMessage = "");
+ void throwIfSkillShouldTerminate(const std::function<void()>& do_before,
+ const std::string& abortedMessage = "");
+
+ static MainResult MakeSucceededResult(aron::data::DictPtr data = nullptr);
+ static MainResult MakeFailedResult();
+ static MainResult MakeAbortedResult();
+
+ // fires if the skill reaches timeout
+ void notifyTimeoutReached();
+
+ private:
+ // helper methods to do all the static initialization stuff
+ InitResult _init();
+ PrepareResult _prepare();
+ MainResult _main();
+ ExitResult _exit();
+
+ void _onTimeoutReached();
+ void _onStopRequested();
+
+ protected:
+ /// Override this method with the actual implementation.
+ virtual InitResult init();
+
+ /// Override this method with the actual implementation.
+ virtual PrepareResult prepare();
+
+ /// Override this method with the actual implementation. The callback is for status updates to the calling instance
+ virtual MainResult main();
+
+ /// Override this method with the actual implementation.
+ virtual ExitResult exit();
+
+ protected:
+ /// 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);
+
+ /// call a subskill and block until the subskill terminates.
+ /// If you call a subskill this way it will be stopped if the current skill stops.
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const skills::SkillProxy& prx, const aron::data::DictPtr& = nullptr);
+
+ /// Similar to callSubskill but non-blocking
+ skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& prx,
+ const aron::data::DictPtr& = nullptr);
+
+ public:
+ // running params
+ armarx::core::time::DateTime started = armarx::core::time::DateTime::Now();
+ armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
+
+ protected:
+ // parameterization. Will be set from implementation wrapper. No getters available
+ // const after construction!!
+ CallbackT callback = CallbackT();
+ manager::dti::SkillManagerInterfacePrx manager = nullptr;
+ std::string executorName = "";
+
+ protected:
+ // non-const params. Const after preparation. Getters are available
+ mutable std::mutex parametersMutex;
+ armarx::aron::data::DictPtr parameters = nullptr;
+
+ // The descripion of the skill
+ SkillDescription description;
+
+ // Status variables
+ std::atomic_bool constructing = true;
+ std::atomic_bool initializing = false;
+ std::atomic_bool preparing = false;
+ std::atomic_bool running = false;
+ std::atomic_bool exiting = false;
+ std::atomic_bool finished = false;
+
+ // Conditionals to indicate that an event has occured. Use conditions this way
+ std::atomic_bool stopped = false;
+ std::atomic_bool timeoutReached = false;
+
+
+ private:
+ // active conditions. First is condition (bool return func)
+ mutable std::mutex conditionCallbacksMutex;
+ std::vector<std::pair<std::function<bool()>, std::function<void()>>>
+ conditionCallbacks = {};
+
+ std::thread conditionCheckingThread; // A thread that checks the conditions frequently
+ armarx::Frequency conditionCheckingThreadFrequency = armarx::Frequency::Hertz(20);
+
+ mutable std::mutex subskillsMutex;
+ std::vector<skills::SkillExecutionID> subskills;
+ };
+
+ template <class T>
+ concept isSkill = std::is_base_of<Skill, T>::value;
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fa37b33ceab97b944e55644c99c530ba3f371c3
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
@@ -0,0 +1,69 @@
+#include "SkillDescription.h"
+
+#include <ArmarXCore/core/ice_conversions.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ provider::dto::SkillDescription
+ SkillDescription::toProviderIce() const
+ {
+ provider::dto::SkillDescription ret;
+ ret.parametersType = aron::type::Object::ToAronObjectDTO(parametersType);
+ ret.resultType = aron::type::Object::ToAronObjectDTO(resultType);
+ ret.description = description;
+ ret.skillId = skillId.toProviderIce();
+ ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileDefaults);
+
+ armarx::core::time::toIce(ret.timeout, timeout);
+ return ret;
+ }
+
+ manager::dto::SkillDescription
+ SkillDescription::toManagerIce() const
+ {
+ manager::dto::SkillDescription ret;
+ ret.parametersType = aron::type::Object::ToAronObjectDTO(parametersType);
+ ret.resultType = aron::type::Object::ToAronObjectDTO(resultType);
+ ret.description = description;
+ ret.skillId = skillId.toManagerIce();
+ ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileDefaults);
+
+ armarx::core::time::toIce(ret.timeout, timeout);
+ return ret;
+ }
+
+ SkillDescription
+ SkillDescription::FromIce(const provider::dto::SkillDescription& i,
+ const std::optional<ProviderID>& pid)
+ {
+ armarx::core::time::Duration _d;
+ armarx::core::time::fromIce(i.timeout, _d);
+ return SkillDescription{
+ .skillId = SkillID::FromIce(i.skillId, pid),
+ .description = i.description,
+ .rootProfileDefaults =
+ armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
+ .timeout = _d,
+ .parametersType = armarx::aron::type::Object::FromAronObjectDTO(i.parametersType),
+ .resultType = armarx::aron::type::Object::FromAronObjectDTO(i.resultType)};
+ }
+
+ SkillDescription
+ SkillDescription::FromIce(const manager::dto::SkillDescription& i)
+ {
+ armarx::core::time::Duration _d;
+ armarx::core::time::fromIce(i.timeout, _d);
+ return SkillDescription{
+ .skillId = SkillID::FromIce(i.skillId),
+ .description = i.description,
+ .rootProfileDefaults =
+ armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
+ .timeout = _d,
+ .parametersType = armarx::aron::type::Object::FromAronObjectDTO(i.parametersType),
+ .resultType = armarx::aron::type::Object::FromAronObjectDTO(i.resultType)};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.h b/source/RobotAPI/libraries/skills/core/SkillDescription.h
new file mode 100644
index 0000000000000000000000000000000000000000..4bc8a3cd8fe92f7ebdf13c32b1e812be8596fbbd
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#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>
+
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ struct SkillDescription
+ {
+ SkillID skillId;
+ std::string description = "";
+ aron::data::DictPtr rootProfileDefaults = nullptr;
+ armarx::core::time::Duration timeout = armarx::core::time::Duration::MilliSeconds(-1);
+ aron::type::ObjectPtr parametersType = nullptr;
+ aron::type::ObjectPtr resultType = nullptr;
+
+ provider::dto::SkillDescription toProviderIce() const;
+ manager::dto::SkillDescription toManagerIce() const;
+
+ static SkillDescription FromIce(const provider::dto::SkillDescription& i,
+ const std::optional<ProviderID>& = std::nullopt);
+ static SkillDescription FromIce(const manager::dto::SkillDescription& i);
+ };
+
+ template <class T>
+ concept isSkillDescription = std::is_base_of<SkillDescription, T>::value;
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2f305c1a5e42b5dc28da3285ef46dcbe154b3e6
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
@@ -0,0 +1,44 @@
+#include "SkillExecutionID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ skills::manager::dto::SkillExecutionID
+ SkillExecutionID::toManagerIce() const
+ {
+ skills::manager::dto::SkillExecutionID ret;
+ ret.skillId = skillId.toManagerIce();
+ ret.executorName = executorName;
+ armarx::core::time::toIce(ret.executionStartedTime, executionStartedTime);
+ return ret;
+ }
+
+ skills::provider::dto::SkillExecutionID
+ SkillExecutionID::toProviderIce() const
+ {
+ skills::provider::dto::SkillExecutionID ret;
+ ret.skillId = skillId.toProviderIce();
+ ret.executorName = executorName;
+ armarx::core::time::toIce(ret.executionStartedTime, executionStartedTime);
+ return ret;
+ }
+
+ SkillExecutionID
+ SkillExecutionID::FromIce(const skills::manager::dto::SkillExecutionID& i)
+ {
+ armarx::core::time::DateTime t;
+ armarx::core::time::fromIce(i.executionStartedTime, t);
+ return {skills::SkillID::FromIce(i.skillId), i.executorName, t};
+ }
+
+ SkillExecutionID
+ SkillExecutionID::FromIce(const skills::provider::dto::SkillExecutionID& i,
+ const std::optional<skills::ProviderID>& providerName)
+ {
+ armarx::core::time::DateTime t;
+ armarx::core::time::fromIce(i.executionStartedTime, t);
+ return {skills::SkillID::FromIce(i.skillId, providerName), i.executorName, t};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c4975388e716c5b0491518a356add0b15cbbf5e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillID.h"
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ struct SkillExecutionID
+ {
+ bool
+ operator==(const SkillExecutionID& other) const
+ {
+ return this->toString() == other.toString();
+ }
+
+ bool
+ operator<(const SkillExecutionID& other) const
+ {
+ return this->toString() < other.toString();
+ }
+
+ bool
+ operator<=(const SkillExecutionID& other) const
+ {
+ return this->toString() <= other.toString();
+ }
+
+ std::string
+ toString() const
+ {
+ return skillId.toString() + " requested by " + executorName + " at " +
+ executionStartedTime.toDateTimeString();
+ }
+
+ skills::manager::dto::SkillExecutionID toManagerIce() const;
+
+ skills::provider::dto::SkillExecutionID toProviderIce() const;
+
+ static SkillExecutionID FromIce(const skills::manager::dto::SkillExecutionID&);
+
+ static SkillExecutionID FromIce(const skills::provider::dto::SkillExecutionID&,
+ const std::optional<skills::ProviderID>& providerName);
+
+
+ SkillID skillId;
+ std::string executorName;
+ armarx::core::time::DateTime executionStartedTime;
+ };
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d6a0181bbbb8990f44ada113c55a69a532ee99f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
@@ -0,0 +1,48 @@
+#include "SkillExecutionRequest.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ manager::dto::SkillExecutionRequest
+ SkillExecutionRequest::toManagerIce() const
+ {
+ manager::dto::SkillExecutionRequest ret;
+ ret.skillId = skillId.toManagerIce();
+ ret.executorName = executorName;
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
+ return ret;
+ }
+
+ provider::dto::SkillExecutionRequest
+ SkillExecutionRequest::toProviderIce() const
+ {
+ provider::dto::SkillExecutionRequest ret;
+ ret.skillId = skillId.toProviderIce();
+ ret.executorName = executorName;
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ return ret;
+ }
+
+ SkillExecutionRequest
+ SkillExecutionRequest::FromIce(const manager::dto::SkillExecutionRequest& req)
+ {
+ return {skills::SkillID::FromIce(req.skillId),
+ req.executorName,
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
+ nullptr};
+ }
+
+ SkillExecutionRequest
+ SkillExecutionRequest::FromIce(const provider::dto::SkillExecutionRequest& req,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ return {skills::SkillID::FromIce(req.skillId, providerId),
+ req.executorName,
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
+ req.callbackInterface};
+ }
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
new file mode 100644
index 0000000000000000000000000000000000000000..d1f5bbcdd175f9f3516d45856186d453af2f987e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillExecutionRequest
+ {
+ public:
+ manager::dto::SkillExecutionRequest toManagerIce() const;
+ provider::dto::SkillExecutionRequest toProviderIce() const;
+
+ static SkillExecutionRequest FromIce(const manager::dto::SkillExecutionRequest&);
+ static SkillExecutionRequest
+ FromIce(const provider::dto::SkillExecutionRequest&,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+
+ skills::SkillID skillId;
+ std::string executorName;
+ armarx::aron::data::DictPtr parameters = nullptr;
+ callback::dti::SkillProviderCallbackInterfacePrx callbackInterface = nullptr;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.cpp b/source/RobotAPI/libraries/skills/core/SkillID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ece268aeeec9234a8dd4b2dcaa437d616988317e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillID.cpp
@@ -0,0 +1,79 @@
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ bool
+ SkillID::operator==(const SkillID& other) const
+ {
+ return this->toString() == other.toString();
+ }
+
+ bool
+ SkillID::operator!=(const SkillID& other) const
+ {
+ return not(*this == other);
+ }
+
+ bool
+ SkillID::operator<(const SkillID& other) const
+ {
+ return toString() < other.toString();
+ }
+
+ bool
+ SkillID::operator<=(const SkillID& other) const
+ {
+ return toString() <= other.toString();
+ }
+
+ SkillID
+ SkillID::FromIce(const manager::dto::SkillID& s)
+ {
+ return SkillID{.providerId =
+ skills::ProviderID{.providerName = s.providerId.providerName},
+ .skillName = s.skillName};
+ }
+
+ SkillID
+ SkillID::FromIce(const provider::dto::SkillID& s,
+ const std::optional<ProviderID>& providerId)
+ {
+ if (providerId.has_value())
+ {
+ return SkillID{.providerId = *providerId, .skillName = s.skillName};
+ }
+ return SkillID{.skillName = s.skillName};
+ }
+
+ manager::dto::SkillID
+ SkillID::toManagerIce() const
+ {
+ ARMARX_CHECK(isFullySpecified());
+ return {providerId->toManagerIce(), skillName};
+ }
+
+ provider::dto::SkillID
+ SkillID::toProviderIce() const
+ {
+ return {skillName};
+ }
+
+ std::string
+ SkillID::toString() const
+ {
+ if (providerId.has_value())
+ {
+ return providerId->providerName + NAME_SEPARATOR + skillName;
+ }
+ return NAME_SEPARATOR + skillName;
+ }
+ } // namespace skills
+
+ std::ostream&
+ skills::operator<<(std::ostream& os, const SkillID& id)
+ {
+ return os << "'" << id.toString() << "'";
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
new file mode 100644
index 0000000000000000000000000000000000000000..e611a82828442ad378dde77d4d90edf477b5e5f1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "ProviderID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillID
+ {
+ public:
+ static const constexpr char* NAME_SEPARATOR = "/";
+
+ bool operator==(const SkillID& other) const;
+ bool operator!=(const SkillID& other) const;
+ bool operator<(const SkillID& other) const;
+ bool operator<=(const SkillID& other) const;
+
+ bool
+ isFullySpecified() const
+ {
+ return isSkillSpecified() and isProviderSpecified();
+ }
+
+ bool
+ isSkillSpecified() const
+ {
+ return not skillName.empty();
+ }
+
+ bool
+ isProviderSpecified() const
+ {
+ if (not providerId.has_value())
+ {
+ return false;
+ }
+ return not providerId->providerName.empty();
+ }
+
+ manager::dto::SkillID toManagerIce() const;
+ provider::dto::SkillID toProviderIce() const;
+
+ static SkillID FromIce(const manager::dto::SkillID&);
+ static SkillID FromIce(const provider::dto::SkillID&,
+ const std::optional<ProviderID>& providerId = std::nullopt);
+
+ std::string toString() const;
+
+ std::optional<ProviderID> providerId = std::nullopt;
+ std::string skillName;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const SkillID& id);
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0ec681e2b772aa773a6cc890198e24c28714207
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
@@ -0,0 +1,8 @@
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.h b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b54b3fff3bfe0aaedcb79d9c275165aefe5a37d
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0ec681e2b772aa773a6cc890198e24c28714207
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
@@ -0,0 +1,8 @@
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5cc2bb20c392864ccb31607e1ef02e9174fb211
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f73c43dfd152c756f80e5ac3e83e901fe59e8ee1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
@@ -0,0 +1,118 @@
+#include "SkillProxy.h"
+
+#include <chrono>
+#include <thread>
+
+namespace armarx
+{
+ namespace skills
+ {
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillID& skillId) :
+ manager(manager)
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skillDescription = SkillDescription::FromIce(
+ manager->getSkillDescription(skillId.toManagerIce()).value());
+ ARMARX_CHECK(skillDescription.skillId.isFullySpecified());
+ }
+
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillDescription& skillDesc) :
+ manager(manager), skillDescription(skillDesc)
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ ARMARX_CHECK(skillDesc.skillId.isFullySpecified());
+ }
+
+ SkillDescription
+ SkillProxy::getSkillDescription() const
+ {
+ return skillDescription;
+ }
+
+ SkillID
+ SkillProxy::getSkillId() const
+ {
+ return skillDescription.skillId;
+ }
+
+ TerminatedSkillStatusUpdate
+ SkillProxy::executeSkill(const std::string& executorName,
+ const aron::data::DictPtr& params) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.parameters = aron::data::Dict::ToAronDictDTO(params);
+ req.skillId = skillDescription.skillId.toManagerIce();
+
+ auto terminatingUpdateIce = manager->executeSkill(req);
+ return TerminatedSkillStatusUpdate::FromIce(terminatingUpdateIce);
+ }
+
+ SkillExecutionID
+ SkillProxy::executeSkillAsync(const std::string& executorName,
+ const aron::data::DictPtr& params) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.parameters = aron::data::Dict::ToAronDictDTO(params);
+ req.skillId = skillDescription.skillId.toManagerIce();
+
+ auto execIdIce = manager->executeSkillAsync(req);
+ return SkillExecutionID::FromIce(execIdIce);
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ SkillProxy::join(const SkillExecutionID& executionId) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto s = this->manager->getSkillExecutionStatus(executionId.toManagerIce());
+
+ while (s)
+ {
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(*s);
+
+ if (statusUpdate.hasBeenTerminated())
+ {
+ break;
+ }
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ s = this->manager->getSkillExecutionStatus(executionId.toManagerIce());
+ }
+
+ if (!s)
+ {
+ // Either the manager already removed the result (then it is unknown) or the execution id does not exist.
+ return std::nullopt;
+ }
+
+ return TerminatedSkillStatusUpdate::FromIce(*s);
+ }
+
+ bool
+ SkillProxy::abortSkill(const SkillExecutionID& id) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto r = manager->abortSkill(id.toManagerIce());
+ return r.success;
+ }
+
+ bool
+ SkillProxy::abortSkillAsync(const SkillExecutionID& id) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto r = manager->abortSkillAsync(id.toManagerIce());
+ return r.success;
+ }
+
+ aron::data::DictPtr
+ SkillProxy::getRootProfileParameters() const
+ {
+ return skillDescription.rootProfileDefaults;
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.h b/source/RobotAPI/libraries/skills/core/SkillProxy.h
new file mode 100644
index 0000000000000000000000000000000000000000..e20ca01a0bc4b6de877e34a3ad6bc0edcbaa4689
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ /* Manages the remote execution of a skill and converts the ice types */
+ class SkillProxy : public armarx::Logging
+ {
+ public:
+ /// We remove the default constructor as every skill proxy requires a manager
+ SkillProxy() = delete;
+
+ /// set the skill proxy using a skillId. Queries the manager to get the description.
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillID& skillId);
+
+ /// set the proxy using a skill description
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillDescription& skillDesc);
+
+ /// copy ctor
+ SkillProxy(const SkillProxy& o) = default;
+
+ /// get the skill description
+ SkillDescription getSkillDescription() const;
+
+ /// get the skill id from the skill description
+ SkillID getSkillId() const;
+
+ // Provide a similar API as the skillprovider
+ /// execute a skill and block until skill terminates
+ TerminatedSkillStatusUpdate
+ executeSkill(const std::string& executorName,
+ const aron::data::DictPtr& params = nullptr) const;
+
+ /// execute a skill. Do not block during execution
+ SkillExecutionID executeSkillAsync(const std::string& executorName,
+ const aron::data::DictPtr& params = nullptr) const;
+
+ /// poll execution status and block until its null or terminated
+ std::optional<TerminatedSkillStatusUpdate>
+ join(const SkillExecutionID& executionId) const;
+
+ /// ask skill to abort ASAP. Blocks until skill stopped
+ bool abortSkill(const SkillExecutionID& executionId) const;
+
+ /// ask skill to abort ASAP
+ bool abortSkillAsync(const SkillExecutionID& executionId) const;
+
+ // Utiliy methods
+ /// get the default parameters of the skill. TODO: Skill profiles in memory!
+ aron::data::DictPtr getRootProfileParameters() const;
+
+ protected:
+ manager::dti::SkillManagerInterfacePrx manager;
+ SkillDescription skillDescription;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..986aab1bc9be867aa918c14e4471ac9fe3e7cf3f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -0,0 +1,388 @@
+#include "SkillStatusUpdate.h"
+
+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!");
+ }
+
+ TerminatedSkillStatus
+ mergeSkillStatuseses(const TerminatedSkillStatus t1, const TerminatedSkillStatus t2)
+ {
+ // if both are equal
+ if (t1 == t2)
+ {
+ return t1;
+ }
+
+ // if one is failed (most unspecific result)
+ if (t1 == TerminatedSkillStatus::Failed or t2 == TerminatedSkillStatus::Failed)
+ {
+ return TerminatedSkillStatus::Failed;
+ }
+
+ // else one must be aborted and the other one must be succeeded
+ return TerminatedSkillStatus::Aborted;
+ }
+
+ ActiveOrTerminatedSkillStatus
+ mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2)
+ {
+ // if both are equal
+ if (t1 == t2)
+ {
+ return t1;
+ }
+
+ // if one is failed (most unspecific result)
+ if (t1 == ActiveOrTerminatedSkillStatus::Failed or
+ t2 == ActiveOrTerminatedSkillStatus::Failed)
+ {
+ return ActiveOrTerminatedSkillStatus::Failed;
+ }
+
+ // if none is failed and one is aborted (second most unspecific result)
+ if (t1 == ActiveOrTerminatedSkillStatus::Aborted or
+ t2 == ActiveOrTerminatedSkillStatus::Aborted)
+ {
+ return ActiveOrTerminatedSkillStatus::Aborted;
+ }
+
+ // else one must be running and the other one must be succeeded
+ return ActiveOrTerminatedSkillStatus::Running;
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const SkillStatus& status)
+ {
+ switch (status)
+ {
+ case SkillStatus::Constructing:
+ ret = core::dto::Execution::Status::Constructing;
+ return;
+ case SkillStatus::Initializing:
+ ret = core::dto::Execution::Status::Initializing;
+ return;
+ case SkillStatus::Preparing:
+ ret = core::dto::Execution::Status::Preparing;
+ return;
+ case SkillStatus::Running:
+ ret = core::dto::Execution::Status::Running;
+ return;
+ case SkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case SkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case SkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status)
+ {
+ switch (status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ ret = core::dto::Execution::Status::Running;
+ return;
+ case ActiveOrTerminatedSkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const TerminatedSkillStatus& status)
+ {
+ switch (status)
+ {
+ case TerminatedSkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case TerminatedSkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case TerminatedSkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ fromIce(const core::dto::Execution::Status& status, TerminatedSkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Initializing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Preparing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Running:
+ break;
+ case core::dto::Execution::Status::Failed:
+ ret = TerminatedSkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = TerminatedSkillStatus::Succeeded;
+ return;
+ case core::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 core::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Initializing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Preparing:
+ break;
+ case core::dto::Execution::Status::Running:
+ ret = ActiveOrTerminatedSkillStatus::Running;
+ return;
+ case core::dto::Execution::Status::Failed:
+ ret = ActiveOrTerminatedSkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = ActiveOrTerminatedSkillStatus::Succeeded;
+ return;
+ case core::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 core::dto::Execution::Status& status, SkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ ret = SkillStatus::Constructing;
+ return;
+ case core::dto::Execution::Status::Initializing:
+ ret = SkillStatus::Initializing;
+ return;
+ case core::dto::Execution::Status::Preparing:
+ ret = SkillStatus::Preparing;
+ return;
+ case core::dto::Execution::Status::Running:
+ ret = SkillStatus::Running;
+ return;
+ case core::dto::Execution::Status::Failed:
+ ret = SkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = SkillStatus::Succeeded;
+ return;
+ case core::dto::Execution::Status::Aborted:
+ ret = SkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ manager::dto::SkillStatusUpdate
+ SkillStatusUpdateBase::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret;
+ ret.executionId.skillId = executionId.skillId.toManagerIce();
+ ret.executionId.executorName = executionId.executorName;
+ armarx::core::time::toIce(ret.executionId.executionStartedTime,
+ executionId.executionStartedTime);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ SkillStatusUpdateBase::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret;
+ ret.executionId.skillId = executionId.skillId.toProviderIce();
+ ret.executionId.executorName = executionId.executorName;
+ armarx::core::time::toIce(ret.executionId.executionStartedTime,
+ executionId.executionStartedTime);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ TerminatedSkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ TerminatedSkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ SkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ SkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ TerminatedSkillStatusUpdate
+ TerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+
+ TerminatedSkillStatusUpdate
+ TerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+
+ SkillStatusUpdate
+ SkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ SkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+
+ SkillStatusUpdate
+ SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ SkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+
+ ActiveOrTerminatedSkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ ActiveOrTerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+
+ ActiveOrTerminatedSkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::FromIce(
+ const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ ActiveOrTerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
+ skills::fromIce(update.status, ret.status);
+ return ret;
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
new file mode 100644
index 0000000000000000000000000000000000000000..d517fc1ec8aa862d041a4d18fcbcc981f3c6f3f4
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -0,0 +1,209 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillExecutionID.h"
+#include "SkillID.h"
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ enum class SkillStatus
+ {
+ Constructing = 0,
+ Initializing = 1,
+ Preparing = 2,
+ Running = 4,
+ Failed = 8,
+ Succeeded = 16,
+ Aborted = 32
+ };
+
+ 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(core::dto::Execution::Status& ret, const SkillStatus& status);
+ void toIce(core::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status);
+ void toIce(core::dto::Execution::Status& ret, const TerminatedSkillStatus& status);
+
+ void fromIce(const core::dto::Execution::Status& status, TerminatedSkillStatus& ret);
+ void fromIce(const core::dto::Execution::Status& status,
+ ActiveOrTerminatedSkillStatus& ret);
+ void fromIce(const core::dto::Execution::Status& status, SkillStatus& ret);
+
+ TerminatedSkillStatus mergeSkillStatuseses(const TerminatedSkillStatus t1,
+ const TerminatedSkillStatus t2);
+ ActiveOrTerminatedSkillStatus mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2);
+
+ struct SkillStatusUpdateBase
+ {
+ // header
+ SkillExecutionID executionId;
+ aron::data::DictPtr parameters;
+ callback::dti::SkillProviderCallbackInterfacePrx callbackInterface;
+
+ // data
+ aron::data::DictPtr result = nullptr;
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+ };
+
+ // Will be returned after the execution of a skill
+ struct TerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
+
+ bool
+ hasBeenTerminated() const
+ {
+ return true;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static TerminatedSkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static TerminatedSkillStatusUpdate
+ FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be returned from periodic skills which can still run
+ struct ActiveOrTerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
+
+ bool
+ hasBeenTerminated() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded ||
+ status == ActiveOrTerminatedSkillStatus::Failed ||
+ status == ActiveOrTerminatedSkillStatus::Aborted;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static ActiveOrTerminatedSkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static ActiveOrTerminatedSkillStatusUpdate
+ FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be used as status updates from skills to the callback interface
+ struct SkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ SkillStatus status = SkillStatus::Constructing;
+
+ bool
+ operator<(const SkillStatusUpdate& o) const
+ {
+ if (!hasBeenConstructed() and o.hasBeenConstructed())
+ {
+ return true;
+ }
+ if (!hasBeenInitialized() and o.hasBeenInitialized())
+ {
+ return true;
+ }
+ if (!hasBeenPrepared() and o.hasBeenPrepared())
+ {
+ return true;
+ }
+ if (!hasBeenRunning() and o.hasBeenRunning())
+ {
+ return true;
+ }
+ if (!hasBeenTerminated() and o.hasBeenTerminated())
+ {
+ return true;
+ }
+ return false;
+ }
+
+ bool
+ operator<=(const SkillStatusUpdate& o) const
+ {
+ if (status == o.status)
+ {
+ return true;
+ }
+ return *this < o;
+ }
+
+ bool
+ hasBeenConstructed() const
+ {
+ return status != SkillStatus::Constructing;
+ }
+
+ bool
+ hasBeenInitialized() const
+ {
+ return status != SkillStatus::Initializing && hasBeenConstructed();
+ }
+
+ bool
+ hasBeenPrepared() const
+ {
+ return status != SkillStatus::Preparing && hasBeenInitialized();
+ }
+
+ bool
+ hasBeenRunning() const
+ {
+ return status != SkillStatus::Running || hasBeenPrepared();
+ }
+
+ bool
+ hasBeenTerminated() const
+ {
+ return status == SkillStatus::Succeeded || status == SkillStatus::Failed ||
+ status == SkillStatus::Aborted;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static SkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static SkillStatusUpdate FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/error/Exception.cpp b/source/RobotAPI/libraries/skills/core/error/Exception.cpp
similarity index 100%
rename from source/RobotAPI/libraries/skills/error/Exception.cpp
rename to source/RobotAPI/libraries/skills/core/error/Exception.cpp
diff --git a/source/RobotAPI/libraries/skills/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
similarity index 61%
rename from source/RobotAPI/libraries/skills/error/Exception.h
rename to source/RobotAPI/libraries/skills/core/error/Exception.h
index b73ccbf1e0271f82cf95107bd6beaecdcb4440a8..5e10baed1e9af3131226320397c11919d7a826e0 100644
--- a/source/RobotAPI/libraries/skills/error/Exception.h
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.h
@@ -24,42 +24,71 @@
#pragma once
// STD/STL
+#include <map>
#include <string>
#include <vector>
-#include <map>
// ArmarX
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/exceptions/Exception.h>
-
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::skills::error
{
/**
- * @brief A base class for aron exceptions. All aron exceptions inherit from this class
+ * @brief A base class for skill exceptions. All skill exceptions inherit from this class
*/
- class SkillException :
- public armarx::LocalException
+ class SkillException : public armarx::LocalException
{
public:
SkillException() = delete;
+
SkillException(const std::string& prettymethod, const std::string& reason) :
LocalException(prettymethod + ": " + reason + ".")
{
}
};
+ class SkillAbortedException : public armarx::LocalException
+ {
+ public:
+ SkillAbortedException() = delete;
+
+ SkillAbortedException(const std::string& reason) : LocalException(reason)
+ {
+ }
+
+ SkillAbortedException(const std::string& prettymethod, const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+ {
+ }
+ };
+
+ class SkillFailedException : public armarx::LocalException
+ {
+ public:
+ SkillFailedException() = delete;
+
+ SkillFailedException(const std::string& reason) : LocalException(reason)
+ {
+ }
+
+ SkillFailedException(const std::string& prettymethod, const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+ {
+ }
+ };
+
/**
* @brief The NotImplementedYetException class
*/
- class NotImplementedYetException :
- public SkillException
+ class NotImplementedYetException : public SkillException
{
public:
NotImplementedYetException() = delete;
+
NotImplementedYetException(const std::string& prettymethod) :
SkillException(prettymethod, "This method is not yet implemented!")
{
}
};
-}
+} // namespace armarx::skills::error
diff --git a/source/RobotAPI/libraries/skills/manager/CMakeLists.txt b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b45f4a92926c16c92ba35fdce0cda59ab1dfdd74
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
@@ -0,0 +1,24 @@
+set(LIB_NAME RobotAPISkillsManager)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ SkillManagerComponentPlugin.cpp
+ HEADERS
+ SkillManagerComponentPlugin.h
+)
+
+add_library(RobotAPI::skills::manager ALIAS RobotAPISkillsManager)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 2908d2e77a0a16627d65d2eb757d3ad8431c3271..3999c956b3ec3d85be444e7b27d56c27fbc2d98e 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,8 +1,12 @@
#include "SkillManagerComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
-#include "../error/Exception.h"
+#include <RobotAPI/libraries/skills/core/SkillID.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx::plugins
{
@@ -14,267 +18,684 @@ namespace armarx::plugins
void
SkillManagerComponentPlugin::preOnConnectComponent()
{
+ auto& p = parent<SkillManagerComponentPluginUser>();
+ p.getProxy(myPrx, -1);
}
void
SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
}
-} // namespace armarx::plugins
-namespace armarx
-{
- SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
+ skills::ProviderID
+ SkillManagerComponentPlugin::getFirstProviderNameThatHasSkill(const skills::SkillID& skillId)
{
- addPlugin(plugin);
+ // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
+ for (const auto& [providerName, providerPrx] : skillProviderMap)
+ {
+ auto allSkills = providerPrx->getSkillDescriptions();
+ for (const auto& [currentSkillID, skillDesc] : allSkills)
+ {
+ if (currentSkillID.skillName == skillId.skillName)
+ {
+ return {providerName};
+ }
+ }
+ }
+ return {"INVALID PROVIDER NAME"};
}
void
- SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
- const Ice::Current&)
+ SkillManagerComponentPlugin::addProvider(const skills::ProviderInfo& providerInfo)
{
- std::lock_guard l(skillProviderMapMutex);
- if (skillProviderMap.find(info.providerName) == skillProviderMap.end())
+ std::scoped_lock l(skillProviderMapMutex);
+ if (skillProviderMap.find(providerInfo.providerId) == skillProviderMap.end())
{
- ARMARX_INFO << "Adding a provider with name '" << info.providerName << "'.";
- skillProviderMap.insert({info.providerName, info.provider});
+ ARMARX_INFO << "Adding a provider with name '" << providerInfo.providerId.providerName
+ << "'.";
+ skillProviderMap.insert({providerInfo.providerId, providerInfo.providerInterface});
}
else
{
- ARMARX_INFO << "Trying to add a provider with name '" << info.providerName
+ ARMARX_INFO << "Trying to add a provider with name '"
+ << providerInfo.providerId.providerName
<< "' but the provider already exists. "
<< "Overwriting the old provider info.";
- skillProviderMap[info.providerName] = info.provider;
+ skillProviderMap[providerInfo.providerId] = providerInfo.providerInterface;
}
}
void
- SkillManagerComponentPluginUser::removeProvider(const std::string& providerName,
- const Ice::Current&)
+ SkillManagerComponentPlugin::removeProvider(const skills::ProviderID& providerId)
{
- std::lock_guard l(skillProviderMapMutex);
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ std::scoped_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(providerId); it != skillProviderMap.end())
{
- ARMARX_INFO << "Removing a provider with name '" << providerName << "'.";
+ ARMARX_INFO << "Removing a provider with name '" << providerId.providerName << "'.";
skillProviderMap.erase(it);
}
else
{
- ARMARX_INFO << "Trying to remove a provider with name '" << providerName
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerId.providerName
<< "' but it couldn't be found.";
}
}
- std::string
- SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
+ skills::SkillStatusUpdate
+ SkillManagerComponentPlugin::executeSkill(const skills::SkillExecutionRequest& executionRequest)
{
- for (const auto& [providerName, providerPrx] : skillProviderMap)
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
+
+ // TODO: Really support regexes!
+ if (executionRequest.skillId.providerId->providerName == "*")
{
- auto allSkills = providerPrx->getSkillDescriptions();
- for (const auto& [currentSkillName, skillDesc] : allSkills)
- {
- if (currentSkillName == skillName)
- {
- return providerName;
- }
- }
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
- return "INVALID PROVIDER NAME";
- }
- 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 == "*")
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
{
- providerName = getFirstProviderNameThatHasSkill(info.skillId.skillName);
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << provderId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
+
+ try
+ {
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
+
+ auto async =
+ provider->begin_executeSkill(provider_executionRequest.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_statusUpdate_ice = provider->end_executeSkill(async);
+
+ // convert to manager view
+ auto statusUpdate =
+ skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice, provderId);
+ return statusUpdate;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << provderId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
}
- else if (not(info.skillId.providerName.empty()))
+ else
{
- providerName = info.skillId.providerName;
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
}
+ }
+
+ skills::SkillExecutionID
+ SkillManagerComponentPlugin::executeSkillAsync(
+ const skills::SkillExecutionRequest& executionRequest)
+ {
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- SkillProviderInterfacePrxMap skillProviderMap;
+ std::unique_lock l(skillProviderMapMutex);
+
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
+
+ // TODO: Really support regexes!
+ if (executionRequest.skillId.providerId->providerName == "*")
{
- std::scoped_lock l(skillProviderMapMutex);
- skillProviderMap = this->skillProviderMap;
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
- bool remove = false;
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
{
- const auto& n = it->first;
- const auto& s = it->second;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << provderId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
try
{
- if (s)
- {
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ auto async =
+ provider->begin_executeSkillAsync(provider_executionRequest.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_executionID_ice = provider->end_executeSkillAsync(async);
- return s->executeSkill(exInfo);
- }
- else
- {
- remove = true;
- }
+ // convert to manager view
+ auto executionId =
+ skills::SkillExecutionID::FromIce(provider_executionID_ice, provderId);
+ executionId.skillId.providerId = provderId;
+ return executionId;
}
catch (...)
{
- remove = true;
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << provderId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
}
+ }
+ else
+ {
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
+ }
+ bool
+ SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const aron::data::DictPtr& data)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
- if (remove)
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
{
- 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 or buggy skill provider '" << n
- << "' during execution. Removing it from skills.";
- it = skillProviderMap.erase(it);
- }
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
+
+ try
+ {
+ auto async = provider->begin_updateSkillParameters(executionId.toProviderIce(),
+ data->toAronDictDTO());
+ l.unlock(); // allow parallel e.g. stopping
+ auto r = provider->end_updateSkillParameters(async);
+ return r.success;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
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.");
+ return false;
}
- return {}; // Never happens
}
- void
- SkillManagerComponentPluginUser::abortSkill(const std::string& providerName,
- const std::string& skillName,
- const Ice::Current& current)
+ bool
+ SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- SkillProviderInterfacePrxMap skillProviderMap;
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
{
- std::scoped_lock l(skillProviderMapMutex);
- skillProviderMap = this->skillProviderMap;
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
+
+ try
+ {
+ auto async = provider->begin_abortSkill(executionId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto r = provider->end_abortSkill(async);
+ return r.success;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
+ }
}
+ else
+ {
+ return false;
+ }
+ }
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ bool
+ SkillManagerComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
{
- const auto& n = it->first;
- const auto& s = it->second;
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
+
try
{
- if (s)
- {
- s->abortSkill(skillName);
- }
- else
- {
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills on next execute.";
- }
+ auto async = provider->begin_abortSkill(executionId.toProviderIce());
+ return true;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills on next execute.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
+ else
+ {
+ return false;
+ }
}
- void
- SkillManagerComponentPluginUser::updateStatusForSkill(
- const skills::provider::dto::SkillStatusUpdate& statusUpdate,
- const Ice::Current&)
+ std::optional<skills::SkillDescription>
+ SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
{
- (void)statusUpdate;
- // If you want to use the status, implement this method!
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*skillId.providerId); it != skillProviderMap.end())
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+
+ try
+ {
+ auto async = provider->begin_getSkillDescription(skillId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_desc_ice = provider->end_getSkillDescription(async);
+
+ if (not provider_desc_ice)
+ {
+ return std::nullopt;
+ }
+
+ // convert to manager view
+ auto desc =
+ skills::SkillDescription::FromIce(provider_desc_ice.value(), providerId);
+ return desc;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << providerId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
}
- skills::manager::dto::SkillDescriptionMapMap
- SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
+ std::map<skills::SkillID, skills::SkillDescription>
+ SkillManagerComponentPlugin::getSkillDescriptions()
{
- skills::manager::dto::SkillDescriptionMapMap ret;
+ std::map<skills::SkillID, skills::SkillDescription> ret;
std::scoped_lock l(skillProviderMapMutex);
for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- const auto& n = it->first;
- const auto& s = it->second;
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ continue;
+ }
+
try
{
- if (s)
- {
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
- ++it;
- }
- else
+ skills::provider::dto::SkillDescriptionMap m = provider->getSkillDescriptions();
+
+ for (const auto& [provider_skillId_ice, skillDescription_ice] : m)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ ret.insert(
+ {skills::SkillID::FromIce(provider_skillId_ice, providerId),
+ skills::SkillDescription::FromIce(skillDescription_ice, providerId)});
}
+
+ ++it;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
- skills::manager::dto::SkillStatusUpdateMapMap
- SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
+ std::optional<skills::SkillStatusUpdate>
+ SkillManagerComponentPlugin::getSkillExecutionStatus(
+ const skills::SkillExecutionID& executionId)
{
- skills::manager::dto::SkillStatusUpdateMapMap ret;
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
- std::scoped_lock l(skillProviderMapMutex);
- for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
{
- const auto& n = it->first;
- const auto& s = it->second;
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+
try
{
- if (s)
+ auto async = provider->begin_getSkillExecutionStatus(executionId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_statusUpdate_ice = provider->end_getSkillExecutionStatus(async);
+
+ if (not provider_statusUpdate_ice)
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
- it++;
+ return std::nullopt;
}
- else
+
+ // convert to manager view
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(
+ provider_statusUpdate_ice.value(), providerId);
+ return statusUpdate;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << providerId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ SkillManagerComponentPlugin::getSkillExecutionStatuses()
+ {
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ continue;
+ }
+
+ try
+ {
+ auto m = provider->getSkillExecutionStatuses();
+
+ for (const auto& [provider_executionId_ice, provider_statusUpdate_ice] : m)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ ret.insert(
+ {skills::SkillExecutionID::FromIce(provider_executionId_ice, providerId),
+ skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice,
+ providerId)});
}
+ it++;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
+
+} // namespace armarx::plugins
+
+namespace armarx
+{
+ SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ void
+ SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current&)
+ {
+ auto i = skills::ProviderInfo::FromIce(info);
+ this->plugin->addProvider(i);
+ }
+
+ void
+ SkillManagerComponentPluginUser::removeProvider(
+ const skills::manager::dto::ProviderID& provider,
+ const Ice::Current&)
+ {
+ auto i = skills::ProviderID::FromIce(provider);
+ this->plugin->removeProvider(i);
+ }
+
+ skills::manager::dto::SkillStatusUpdate
+ SkillManagerComponentPluginUser::executeSkill(
+ const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current&)
+ {
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ return this->plugin->executeSkill(e).toManagerIce();
+ }
+
+ skills::manager::dto::SkillExecutionID
+ SkillManagerComponentPluginUser::executeSkillAsync(
+ const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ return this->plugin->executeSkillAsync(e).toManagerIce();
+ }
+
+ skills::provider::dto::ParameterUpdateResult
+ SkillManagerComponentPluginUser::updateSkillParameters(
+ const skills::manager::dto::SkillExecutionID& info,
+ const aron::data::dto::DictPtr& params,
+ const Ice::Current& current)
+ {
+ skills::provider::dto::ParameterUpdateResult ret;
+ auto a = armarx::aron::data::Dict::FromAronDictDTO(params);
+ auto e = skills::SkillExecutionID::FromIce(info);
+ ret.success = this->plugin->updateSkillParameters(e, a);
+ return ret;
+ }
+
+ skills::provider::dto::AbortSkillResult
+ SkillManagerComponentPluginUser::abortSkill(const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& current)
+ {
+ skills::provider::dto::AbortSkillResult ret;
+ auto i = skills::SkillExecutionID::FromIce(id);
+ ret.success = this->plugin->abortSkill(i);
+ return ret;
+ }
+
+ skills::provider::dto::AbortSkillResult
+ SkillManagerComponentPluginUser::abortSkillAsync(
+ const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
+ {
+ skills::provider::dto::AbortSkillResult ret;
+ auto i = skills::SkillExecutionID::FromIce(id);
+ ret.success = this->plugin->abortSkillAsync(i);
+ return ret;
+ }
+
+ void
+ SkillManagerComponentPluginUser::updateStatusForSkill(
+ const skills::provider::dto::SkillStatusUpdate& statusUpdate,
+ const skills::callback::dto::ProviderID& pid,
+ const Ice::Current&)
+ {
+ (void)statusUpdate;
+ (void)pid;
+ // If you want to use the status, implement this method!
+ }
+
+ skills::manager::dto::SkillDescriptionMap
+ SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillDescriptionMap ret;
+
+ auto m = this->plugin->getSkillDescriptions();
+
+ for (const auto& [k, v] : m)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+
+ return ret;
+ }
+
+ IceUtil::Optional<skills::manager::dto::SkillDescription>
+ SkillManagerComponentPluginUser::getSkillDescription(const skills::manager::dto::SkillID& id,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillID::FromIce(id);
+ auto o = this->plugin->getSkillDescription(e);
+ if (o.has_value())
+ {
+ return o->toManagerIce();
+ }
+ return {};
+ }
+
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ SkillManagerComponentPluginUser::getSkillExecutionStatus(
+ const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillExecutionID::FromIce(executionId);
+ auto o = this->plugin->getSkillExecutionStatus(e);
+ if (o.has_value())
+ {
+ return o->toManagerIce();
+ }
+ return {};
+ }
+
+ skills::manager::dto::SkillStatusUpdateMap
+ SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillStatusUpdateMap ret;
+
+ auto m = this->plugin->getSkillExecutionStatuses();
+
+ for (const auto& [k, v] : m)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+
+ return ret;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 7b14adf04d141900166968c773f498b6060623cb..8aeea6f1ab7d387ce6c46e4212e1455c1c2a9435 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -6,6 +6,15 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+
+namespace armarx
+{
+ class SkillManagerComponentPluginUser; // forward declaration
+}
namespace armarx::plugins
{
@@ -19,36 +28,99 @@ namespace armarx::plugins
void preOnConnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void addProvider(const skills::ProviderInfo& info);
+
+ void removeProvider(const skills::ProviderID& id);
+
+ skills::SkillStatusUpdate executeSkill(const skills::SkillExecutionRequest& req);
+
+ skills::SkillExecutionID executeSkillAsync(const skills::SkillExecutionRequest& req);
+
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const aron::data::DictPtr& data);
+
+ bool abortSkill(const skills::SkillExecutionID& id);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& id);
+
+ std::optional<skills::SkillDescription> getSkillDescription(const skills::SkillID& id);
+
+ std::map<skills::SkillID, skills::SkillDescription> getSkillDescriptions();
+
+ std::optional<skills::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::SkillExecutionID& id);
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillExecutionStatuses();
+
+ skills::ProviderID getFirstProviderNameThatHasSkill(const skills::SkillID& skillid);
+
+ private:
+ skills::manager::dti::SkillManagerInterfacePrx myPrx;
+
+ std::mutex skillProviderMapMutex;
+ std::map<skills::ProviderID, skills::provider::dti::SkillProviderInterfacePrx>
+ skillProviderMap;
+
+ friend class armarx::SkillManagerComponentPluginUser;
};
-}
+} // namespace armarx::plugins
namespace armarx
{
class SkillManagerComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public skills::manager::dti::SkillManagerInterface
+ virtual public ManagedIceObject,
+ virtual public skills::manager::dti::SkillManagerInterface
{
public:
SkillManagerComponentPluginUser();
- void addProvider(const skills::manager::dto::ProviderInfo& providerInfo, const Ice::Current ¤t) override;
- void removeProvider(const std::string&, const Ice::Current ¤t) override;
+ void addProvider(const skills::manager::dto::ProviderInfo& providerInfo,
+ const Ice::Current& current) override;
+ void removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdate
+ executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillExecutionID
+ executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& skillExecutionRequest,
+ const Ice::Current& current) override;
+
+ skills::provider::dto::ParameterUpdateResult
+ updateSkillParameters(const skills::manager::dto::SkillExecutionID& executionId,
+ const aron::data::dto::DictPtr& params,
+ const Ice::Current& current) 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;
+ void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
+ const skills::callback::dto::ProviderID& id,
+ const Ice::Current& current) override;
- skills::manager::dto::SkillDescriptionMapMap getSkillDescriptions(const Ice::Current ¤t) override;
- skills::manager::dto::SkillStatusUpdateMapMap getSkillExecutionStatuses(const Ice::Current ¤t) override;
+ skills::provider::dto::AbortSkillResult
+ abortSkill(const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& current) override;
- protected:
- std::string getFirstProviderNameThatHasSkill(const std::string& skillName);
+ skills::provider::dto::AbortSkillResult
+ abortSkillAsync(const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& current) override;
+
+
+ skills::manager::dto::SkillDescriptionMap
+ getSkillDescriptions(const Ice::Current& current) override;
+
+ IceUtil::Optional<skills::manager::dto::SkillDescription>
+ getSkillDescription(const skills::manager::dto::SkillID& id,
+ const Ice::Current& current) override;
+
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(const Ice::Current& current) override;
private:
armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
-
- protected:
- std::mutex skillProviderMapMutex;
- std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx> skillProviderMap;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9cb59c319fdf937ee9eb341610071f44114de3c1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
@@ -0,0 +1,60 @@
+set(LIB_NAME RobotAPISkillsProvider)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ SkillProviderComponentPlugin.cpp
+ SkillFactory.cpp
+ LambdaSkill.cpp
+ SimpleSkill.cpp
+ SimpleSpecializedSkill.cpp
+ SimplePeriodicSpecializedSkill.cpp
+ SimplePeriodicSkill.cpp
+ SpecializedSkillProxy.cpp
+ PeriodicSkill.cpp
+ SpecializedSkill.cpp
+ PeriodicSpecializedSkill.cpp
+ detail/SkillImplementationWrapper.cpp
+ SkillContext.cpp
+
+
+ HEADERS
+ SkillProviderComponentPlugin.h
+ SkillFactory.h
+ LambdaSkill.h
+ SimpleSkill.h
+ SimpleSpecializedSkill.h
+ SimplePeriodicSpecializedSkill.h
+ SimplePeriodicSkill.h
+ SpecializedSkillProxy.h
+ PeriodicSkill.h
+ SpecializedSkill.h
+ PeriodicSpecializedSkill.h
+ detail/SkillImplementationWrapper.h
+ SkillContext.h
+
+
+ mixins/All.h
+ mixins/ArvizSkillMixin.h
+ mixins/MNSSkillMixin.h
+ mixins/MemoryReadingSkillMixin.h
+ mixins/RobotReadingSkillMixin.h
+ mixins/ObjectReadingSkillMixin.h
+ mixins/ObjectWritingSkillMixin.h
+ mixins/GraspReadingSkillMixin.h
+)
+
+add_library(RobotAPI::skills::provider ALIAS RobotAPISkillsProvider)
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
index 280cdb93e5eb62413a159f232b20c5da0b7e3bd7..804042b8ac47b2b1c0ec762c87c4e4ce186daf97 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
@@ -5,11 +5,12 @@ namespace armarx
namespace skills
{
- Skill::MainResult LambdaSkill::main(const MainInput& in)
+ Skill::MainResult
+ LambdaSkill::main()
{
- TerminatedSkillStatus res = fun(in.executorName, in.params);
+ TerminatedSkillStatus res = fun();
return {.status = res, .data = nullptr};
}
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
index 01e3a3897e78b444c222b352556bea118f5bc50f..16d39dd39578e25ecb901815e10311e7e57bab72 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
@@ -1,6 +1,6 @@
#pragma once
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
namespace armarx
{
@@ -9,19 +9,16 @@ namespace armarx
class LambdaSkill : public Skill
{
public:
- using FunT = std::function<TerminatedSkillStatus(const std::string clientId, const aron::data::DictPtr&)>;
+ using FunctionType = std::function<TerminatedSkillStatus()>;
LambdaSkill() = delete;
- LambdaSkill(const FunT& f, const SkillDescription& desc) :
- Skill(desc),
- fun(f)
- {};
+ LambdaSkill(const SkillDescription& desc, const FunctionType& f) : Skill(desc), fun(f){};
private:
- MainResult main(const MainInput& in) override;
+ MainResult main() override;
private:
- FunT fun;
+ FunctionType fun;
};
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index 70daa959f0256646125c8477768f304db5555312..fa354d03d242c9e19b95a4a846309227503ae182 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -26,65 +26,57 @@
#include "ArmarXCore/core/time/Frequency.h"
#include <ArmarXCore/core/time/Metronome.h>
-#include "RobotAPI/libraries/skills/provider/Skill.h"
+#include "RobotAPI/libraries/skills/core/Skill.h"
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)
{
}
- PeriodicSkill::StepResult PeriodicSkill::stepOfSkill(const MainInput& in)
+ Skill::MainResult
+ PeriodicSkill::main()
{
- return this->step(in);
- }
-
- Skill::MainResult PeriodicSkill::main(const MainInput& in)
- {
- core::time::Metronome metronome(frequency);
+ armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto res = stepOfSkill(in);
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step();
switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
// nothing to do here
break;
case ActiveOrTerminatedSkillStatus::Aborted:
- return {TerminatedSkillStatus::Aborted, res.data};
+ return MakeAbortedResult();
case ActiveOrTerminatedSkillStatus::Succeeded:
- return {TerminatedSkillStatus::Succeeded, res.data};
+ return MakeSucceededResult(res.data);
case ActiveOrTerminatedSkillStatus::Failed:
- return {TerminatedSkillStatus::Failed, res.data};
+ return MakeFailedResult();
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " too long. Expected " << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (stopped)
- {
- return {TerminatedSkillStatus::Aborted, nullptr};
- }
-
- if (timeoutReached)
- {
- ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
- return {TerminatedSkillStatus::Failed, nullptr};
- }
-
+ // never happens
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
-
- PeriodicSkill::StepResult PeriodicSkill::step(const MainInput& in)
+ PeriodicSkill::StepResult
+ PeriodicSkill::step()
{
- ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillName << "'. Please overwrite this method!";
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
+ << "'. Please overwrite this method!";
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
index 5df6e07e9b7304b8e7c4db10141d3980f31a041c..aff91f19677f0ec362fbd7ca34ab2f3976661b96 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -23,7 +23,8 @@
#include <ArmarXCore/core/time/Frequency.h>
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
#include "SpecializedSkill.h"
namespace armarx::skills
@@ -40,19 +41,16 @@ namespace armarx::skills
PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
- StepResult stepOfSkill(const MainInput& in);
-
- private:
+ protected:
/// Do not use anymore
- Skill::MainResult main(const MainInput& in) final;
+ Skill::MainResult main() final;
/// Override this method with your own step function
- virtual StepResult step(const MainInput& in);
+ virtual StepResult step();
- private:
+ protected:
const armarx::Frequency frequency;
};
-
} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 4efac5d6d3ff33206063b928cd202e45476b0b63..914c6da0221973624309a57666064669e05de14a 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -24,7 +24,8 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
#include "PeriodicSkill.h"
#include "SpecializedSkill.h"
@@ -42,71 +43,60 @@ namespace armarx::skills
using StepResult = PeriodicSkill::StepResult;
PeriodicSpecializedSkill() = delete;
- PeriodicSpecializedSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency) :
- Base(skillDescription), frequency(frequency)
- {
- }
-
- StepResult stepOfSkill(const typename Base::SpecializedMainInput& in)
+ PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
{
- return this->step(in);
}
- private:
- using Skill::stopped;
- using Skill::timeoutReached;
-
+ protected:
/// Do not use anymore
- Skill::MainResult main(const typename Base::SpecializedMainInput& in) final
+ Skill::MainResult
+ main() final
{
- core::time::Metronome metronome(frequency);
+ armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto statusUpdate = stepOfSkill(in);
- switch (statusUpdate.status)
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step();
+ switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
- // nothing to do here
+ // nothing to do here. break switch
break;
case ActiveOrTerminatedSkillStatus::Aborted:
- return {TerminatedSkillStatus::Aborted, statusUpdate.data};
+ return Skill::MakeAbortedResult();
case ActiveOrTerminatedSkillStatus::Succeeded:
- return {TerminatedSkillStatus::Succeeded, statusUpdate.data};
+ return Skill::MakeSucceededResult(res.data);
case ActiveOrTerminatedSkillStatus::Failed:
- return {TerminatedSkillStatus::Failed, statusUpdate.data};
+ return Skill::MakeFailedResult();
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " too long. Expected " << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (stopped)
- {
- return {TerminatedSkillStatus::Aborted, nullptr};
- }
-
- if (timeoutReached)
- {
- ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
- return {TerminatedSkillStatus::Failed, nullptr};
- }
-
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
/// Override this method with your own step function
- virtual StepResult step(const typename Base::SpecializedMainInput& in)
+ virtual StepResult
+ step()
{
- ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillName << "'. Please overwrite this method!";
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillName
+ << "'. Please overwrite this method!";
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
- private:
+ protected:
const armarx::Frequency frequency;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33a6475b3a9e2e01a30c40f4cee60e304a600b40
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -0,0 +1,54 @@
+#include "SimplePeriodicSkill.h"
+
+namespace armarx::skills
+{
+ SimplePeriodicSkill::SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ SimpleSkill(skillDescription), frequency(frequency)
+ {
+ }
+
+ Skill::MainResult
+ SimplePeriodicSkill::main(const MainInput& in)
+ {
+ armarx::core::time::Metronome metronome(frequency);
+
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ SimplePeriodicSkill::StepResult
+ SimplePeriodicSkill::step(const MainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
+ << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ca70d213277e50fd5350458e62b8c8428050b52
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "PeriodicSkill.h"
+#include "SimpleSkill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SimplePeriodicSkill : public SimpleSkill
+ {
+ public:
+ using Base = SimpleSkill;
+
+ using Base::Base;
+
+ using StepResult = PeriodicSkill::StepResult;
+
+ SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency);
+
+ protected:
+ /// Do not use anymore
+ Skill::MainResult main(const MainInput& in) final;
+
+ /// Override this method with your own step function
+ virtual StepResult step(const MainInput& in);
+
+ protected:
+ const armarx::Frequency frequency;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a04c2b3d821e7e821dd2158a98a636d9b3338b03
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp
@@ -0,0 +1,6 @@
+#include "SimplePeriodicSpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce3c968f587a7984fb606e8485297aa56ffff04a
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include "PeriodicSkill.h"
+#include "SimpleSpecializedSkill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SimplePeriodicSpecializedSkill : public SimpleSpecializedSkill<AronT>
+ {
+
+ public:
+ using Base = SimpleSpecializedSkill<AronT>;
+ using ParamType = AronT;
+
+ using Base::Base;
+
+ using StepResult = PeriodicSkill::StepResult;
+
+ SimplePeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
+ {
+ }
+
+ protected:
+ /// Do not use anymore
+ Skill::MainResult
+ main(const typename Base::SpecializedMainInput& in) final
+ {
+ armarx::core::time::Metronome metronome(frequency);
+
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return Skill::MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return Skill::MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return Skill::MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ /// Override this method with your own step function
+ virtual StepResult
+ step(const typename Base::SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillId
+ << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+
+ protected:
+ const armarx::Frequency frequency;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6e834b4cf3114bd0938328482d00a2d243eb60e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
@@ -0,0 +1,55 @@
+#include "SimpleSkill.h"
+
+namespace armarx::skills
+{
+ Skill::InitResult
+ SimpleSkill::init()
+ {
+ InitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ return this->init(i);
+ }
+
+ Skill::MainResult
+ SimpleSkill::main()
+ {
+ MainInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ i.callback = this->callback;
+ return this->main(i);
+ }
+
+ Skill::ExitResult
+ SimpleSkill::exit()
+ {
+ ExitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ return this->exit(i);
+ }
+
+ Skill::InitResult
+ SimpleSkill::init(const InitInput& in)
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ SimpleSkill::main(const MainInput& in)
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+
+ Skill::ExitResult
+ SimpleSkill::exit(const ExitInput& in)
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..45e4715d8bfaf4cc06c98199c67679396c071674
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SimpleSkill : public Skill
+ {
+ public:
+ using Base = Skill;
+
+ using Base::Base;
+
+ struct InitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ };
+
+ struct MainInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ CallbackT callback;
+ };
+
+ struct ExitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual InitResult init(const InitInput& in);
+ virtual MainResult main(const MainInput& in);
+ virtual ExitResult exit(const ExitInput& in);
+
+ InitResult init() final;
+ MainResult main() final;
+ ExitResult exit() final;
+
+ private:
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49a5c2e4956b3c419c0e5cb495f0de5ea11732de
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
@@ -0,0 +1,6 @@
+#include "SimpleSpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..b145311953fdd7e350a042e50b18219e365ec68e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
@@ -0,0 +1,99 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SimpleSpecializedSkill : public Skill
+ {
+ public:
+ using Base = Skill;
+ using ParamType = AronT;
+
+ using Base::Base;
+
+ struct SpecializedInitInput
+ {
+ std::string executorName;
+ AronT parameters;
+ };
+
+ struct SpecializedMainInput
+ {
+ std::string executorName;
+ AronT parameters;
+ CallbackT callback;
+ };
+
+ struct SpecializedExitInput
+ {
+ std::string executorName;
+ AronT parameters;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual InitResult
+ init(const SpecializedInitInput& in)
+ {
+ return InitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ virtual MainResult
+ main(const SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded,
+ .data = nullptr};
+ }
+
+ virtual ExitResult
+ exit(const SpecializedExitInput& in)
+ {
+ return ExitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ InitResult
+ init() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedInitInput i;
+ i.executorName = this->executorName;
+ i.parameters = p;
+ return this->init(i);
+ }
+
+ MainResult
+ main() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedMainInput i;
+ i.executorName = this->executorName;
+ i.callback = this->callback;
+ i.parameters = p;
+ return this->main(i);
+ }
+
+ ExitResult
+ exit() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedExitInput i;
+ i.executorName = this->executorName;
+ i.parameters = p;
+ return this->exit(i);
+ }
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
deleted file mode 100644
index ba48923e7beab1876c6477d04845d05ce64883b9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ /dev/null
@@ -1,203 +0,0 @@
-#include "Skill.h"
-
-namespace armarx
-{
- namespace skills
- {
- Skill::Skill(const SkillDescription& desc):
- description(desc)
- {
- // replace constructor if you want to have a specific logging tag
- Logging::setTag("armarx::skills::" + description.skillName);
- }
-
- // install a local condition via a lambda
- void Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
- {
- std::lock_guard l(callbacksMutex);
- callbacks.push_back({f, cb});
- }
-
- bool Skill::isSkillAvailable(const InitInput &in) const
- {
- return this->isAvailable(in);
- }
-
- bool Skill::isAvailable(const InitInput& in) const
- {
- (void) in;
- return true;
- }
-
- void Skill::resetSkill()
- {
- //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();
- }
-
- void Skill::waitForDependenciesOfSkill()
- {
- //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 = armarx::core::time::DateTime::Now();
-
- // install timeout condition
- installConditionWithCallback(
- [&](){ return (armarx::core::time::DateTime::Now() >= (started + description.timeout)); },
- [&](){ notifyTimeoutReached(); }
- );
-
- conditionCheckingThread = std::thread([&]()
- {
- armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
- while (running) // when the skill ends/aborts this variable will be set to false
- {
- {
- 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() << ")";
- }
- }
- });
- }
- Skill::InitResult Skill::initSkill(const InitInput& in)
- {
- this->_init();
- return this->init(in);
- }
-
- void Skill::_main()
- {
- // Nothing here yet...
- }
- Skill::MainResult Skill::mainOfSkill(const MainInput& in)
- {
- this->_main();
- return this->main(in);
- }
-
- void Skill::_exit()
- {
- // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
- running = false; // stop checking conditions
-
- if (conditionCheckingThread.joinable())
- {
- conditionCheckingThread.join();
- }
- exited = armarx::core::time::DateTime::Now();
- }
- Skill::ExitResult Skill::exitSkill(const ExitInput& in)
- {
- auto ret = this->exit(in);
- this->_exit();
- return ret;
- }
-
- void Skill::notifyTimeoutReached()
- {
- timeoutReached = true;
- onTimeoutReached();
- }
-
- Skill::MainResult Skill::MakeAbortedResult(aron::data::DictPtr data)
- {
- return MainResult{
- .status = TerminatedSkillStatus::Aborted,
- .data = data,
- };
- }
-
- void Skill::notifySkillToStopASAP()
- {
- stopped = true;
- onStopRequested();
- }
-
- bool Skill::checkWhetherSkillShouldStopASAP() const
- {
- return stopped || timeoutReached;
- }
-
- // condition effects
- void Skill::onTimeoutReached()
- {
- }
- void Skill::onStopRequested()
- {
- }
-
- // reset all local variables
- void Skill::reset()
- {
- // 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
deleted file mode 100644
index bfba4d4cce7c37a880ea93b919373bdd3e126402..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ /dev/null
@@ -1,176 +0,0 @@
-#pragma once
-
-// std/stl
-#include <mutex>
-#include <queue>
-#include <thread>
-#include <functional>
-
-// base class
-#include <ArmarXCore/core/logging/Logging.h>
-
-// ArmarX
-#include <ArmarXCore/core/time/DateTime.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
-{
- namespace skills
- {
- class Skill : public armarx::Logging
- {
- public:
- using CallbackT = std::function<void(const aron::data::DictPtr&)>;
-
- 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
- {
- std::string executorName;
- aron::data::DictPtr params;
- };
-
- Skill() = delete;
- Skill(const SkillDescription&);
- virtual ~Skill() = default;
-
- /// The id of the skill (combination of provider and name must be unique).
- SkillID getSkillId() const
- {
- return {providerName, description.skillName};
- }
-
- // 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;
-
- // 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);
-
- // 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:
- static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
-
- // 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 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);
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual MainResult main(const MainInput& in);
-
- /// 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;
-
- /// running params
- armarx::core::time::DateTime started = armarx::core::time::DateTime::Invalid();
- armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
-
- /// 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 = false;
- std::atomic_bool stopped = false;
- std::atomic_bool timeoutReached = false;
-
- private:
- 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.h b/source/RobotAPI/libraries/skills/provider/SkillContext.h
index 9bdbc0f0260729fb213941ef64c589f1b7595a05..a5efb050b3e3e2fa005f20b5f6690026ddcb8573 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillContext.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillContext.h
@@ -7,20 +7,23 @@ 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 */
+ /* A virtual 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 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) {};
+ 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:
};
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
deleted file mode 100644
index 4e195f39e3953d47c98371023ecb90d4c95f9fab..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "SkillDescription.h"
-
-namespace armarx
-{
- namespace skills
- {
- provider::dto::SkillDescription SkillDescription::toIce() const
- {
- provider::dto::SkillDescription ret;
- ret.acceptedType = aron::type::Object::ToAronObjectDTO(acceptedType);
- ret.description = description;
- ret.skillName = skillName;
- ret.robots = robots;
- 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
deleted file mode 100644
index 57c3c93b18230c81b4152d9d915e4d2337beb294..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-#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>
-
-namespace armarx
-{
- namespace skills
- {
- struct SkillDescription
- {
- std::string skillName = "NOT INITIALIZED YET";
- std::string description = "NOT INITIALIZED YET";
- std::vector<std::string> robots = {};
- armarx::core::time::Duration timeout = armarx::core::time::Duration::MilliSeconds(-1);
- aron::type::ObjectPtr acceptedType = nullptr;
- aron::data::DictPtr defaultParams = nullptr;
-
- provider::dto::SkillDescription toIce() const;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp b/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78ec9ef6d60dabdf1dab84964aa3722cec9256e5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp
@@ -0,0 +1,6 @@
+#include "SkillFactory.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.h b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e3b40e3daf092e9214fc65846ec1c58b57e2b52
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <tuple>
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillBlueprint
+ {
+ public:
+ using FunctionTypeToCreateSkill = std::function<std::unique_ptr<Skill>()>;
+
+ SkillBlueprint(const FunctionTypeToCreateSkill& s) : _createSkill(s)
+ {
+ }
+
+ SkillBlueprint(FunctionTypeToCreateSkill&& s) : _createSkill(std::move(s))
+ {
+ }
+
+ SkillBlueprint(const SkillBlueprint&) = default;
+
+ virtual ~SkillBlueprint() = default;
+
+ template <class _Skill, class... Args>
+ requires isSkill<_Skill>
+
+ static std::unique_ptr<SkillBlueprint>
+ ForSkill(Args&&... args)
+ {
+ auto _createSkill = [=]()
+ {
+ auto ptr = std::make_unique<_Skill>(args...);
+ return ptr;
+ };
+
+ auto ret = std::make_unique<SkillBlueprint>(std::move(_createSkill));
+ return ret;
+ }
+
+ virtual std::unique_ptr<Skill>
+ createSkill(const skills::ProviderID& pid) const
+ {
+ auto s = _createSkill();
+ s->setProviderId(pid);
+ return s;
+ }
+
+ virtual SkillDescription
+ createSkillDescription(const skills::ProviderID& pid) const
+ {
+ auto s = createSkill(pid);
+ return s->getSkillDescription();
+ }
+
+
+ protected:
+ FunctionTypeToCreateSkill _createSkill;
+ };
+
+ template <class T>
+ concept isSkillBlueprint = std::is_base_of<SkillBlueprint, T>::value;
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.cpp b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
deleted file mode 100644
index 07c6248b2f8c366ceeb7782b78315a6d16f74c70..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillID.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#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;
- }
- }
-
- std::ostream& skills::operator<<(std::ostream& os, const SkillID& id)
- {
- return os << "'" << id.toString() << "'";
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.h b/source/RobotAPI/libraries/skills/provider/SkillID.h
deleted file mode 100644
index 320a6e946e04694c6d703d0f78b18552d756cf7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillID.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#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;
- };
-
- std::ostream& operator<<(std::ostream& os, const SkillID& id);
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
deleted file mode 100644
index f977b2ac595fd3506856d2ba42468cdef72d2e54..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include "SkillParameterization.h"
-
-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
deleted file mode 100644
index 72ec6d2eb11eec1954e753021d6e634a641027fd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx
-{
- namespace skills
- {
- struct SkillParameterization
- {
- 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 9009db639c7e6164474312630fb1de104f1a713d..e9e86b054b7c3e2eca88d3e7d51b232acc5428f1 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -3,6 +3,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
namespace armarx::plugins
{
@@ -24,23 +25,13 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
const std::string providerName = p.getName();
- // update skill ownership
- for (auto& [skillName, impl] : skillImplementations)
- {
- impl.skill->manager = manager;
- impl.skill->providerName = providerName;
-
- impl.statusUpdate.skillId = {providerName, skillName};
- }
-
// register self to manager
- skills::manager::dto::ProviderInfo i;
- i.provider = myPrx;
- i.providerName = providerName;
- i.providedSkills = p.getSkillDescriptions();
- manager->addProvider(i);
+ skills::ProviderInfo i{.providerId = skills::ProviderID{.providerName = providerName},
+ .providerInterface = myPrx,
+ .providedSkills = getSkillDescriptions()};
- connected = true;
+ ARMARX_INFO << "Adding provider to manager: " << i.providerId;
+ manager->addProvider(i.toIce()); // add provider info to manager
}
void
@@ -49,7 +40,8 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
- manager->removeProvider(providerName);
+ auto id = skills::manager::dto::ProviderID{providerName};
+ manager->removeProvider(id);
}
void
@@ -64,130 +56,384 @@ namespace armarx::plugins
}
void
- SkillProviderComponentPlugin::addSkill(const skills::LambdaSkill::FunT& f,
- const skills::SkillDescription& desc)
+ SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&& fac)
{
- auto lambda = std::make_unique<skills::LambdaSkill>(f, desc);
- addSkill(std::move(lambda));
- }
-
- void
- SkillProviderComponentPlugin::addSkill(std::unique_ptr<skills::Skill>&& skill)
- {
- if (!skill)
+ if (!fac)
{
return;
}
- std::string skillName = skill->description.skillName;
- 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;
- }
+ auto& p = parent<SkillProviderComponentPluginUser>();
+ const std::string componentName = p.getName();
+
+ const skills::ProviderID providerId({componentName});
// lock skills map
- const std::unique_lock l(skillsMutex);
- if (skillImplementations.find(skillName) != skillImplementations.end())
+ const std::unique_lock l(skillFactoriesMutex);
+ auto skillId = fac->createSkillDescription(providerId).skillId;
+
+ if (skillFactories.find(skillId) != skillFactories.end())
{
- ARMARX_WARNING << "Try to add a skill '" + skillName +
+ ARMARX_WARNING << "Try to add a skill factory for skill '" + skillId.toString() +
"' which already exists in list. Ignoring this skill.";
return;
}
- 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(parent().getName(), skillName);
+ ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
+
+ skillFactories.emplace(skillId, std::move(fac));
+
+ // if (connected)
+ // {
+ // // if skill is added after onConnect we have to set the proxies manually.
+ // std::string providerName = parent().getName();
+ // s.first->second.skill->manager = manager;
+ // s.first->second.skill->providerName = providerName;
+ // }
}
- skills::detail::SkillImplementationWrapper&
- SkillProviderComponentPlugin::getSkill(const std::string& name)
+ skills::SkillBlueprint*
+ SkillProviderComponentPlugin::addSkillFactory(const skills::SkillDescription& desc,
+ const skills::LambdaSkill::FunctionType& f)
{
- ARMARX_CHECK_GREATER(skillImplementations.count(name), 0)
- << "Skill '" + name + "' not found.";
- const std::unique_lock l(skillsMutex);
- return skillImplementations.at(name);
+ return addSkillFactory<skills::LambdaSkill>(desc, f);
}
- skills::provider::dto::SkillStatusUpdateMap
+ skills::SkillBlueprint*
+ SkillProviderComponentPlugin::getSkillFactory(const armarx::skills::SkillID& skillId)
+ {
+ // NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ if (skillFactories.count(skillId) == 0)
+ {
+ ARMARX_INFO << "Could not find a skill factory for id: " << skillId;
+ return nullptr;
+ }
+
+ auto* facPtr = skillFactories.at(skillId).get();
+ return static_cast<skills::SkillBlueprint*>(facPtr);
+ }
+
+ std::optional<skills::SkillStatusUpdate>
+ SkillProviderComponentPlugin::getSkillExecutionStatus(
+ const skills::SkillExecutionID& execId) const
+ {
+ ARMARX_CHECK(execId.skillId.isSkillSpecified());
+
+ const std::unique_lock l(skillExecutionsMutex);
+ if (skillExecutions.find(execId) == skillExecutions.end())
+ {
+ ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
+ "' not found!";
+ return std::nullopt;
+ }
+
+ return skillExecutions.at(execId).statusUpdate;
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
SkillProviderComponentPlugin::getSkillExecutionStatuses() const
{
- skills::provider::dto::SkillStatusUpdateMap skillUpdates;
- const std::unique_lock l(skillsMutex);
- for (const auto& [key, impl] : skillImplementations)
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillUpdates;
+
+ const std::scoped_lock l(skillExecutionsMutex);
+ for (const auto& [key, impl] : skillExecutions)
{
- const std::shared_lock l2(impl.skillStatusMutex);
- skillUpdates.insert({key, impl.statusUpdate.toIce()});
+ const std::scoped_lock l2(impl.skillStatusesMutex);
+ skillUpdates.insert({key, impl.statusUpdate});
}
return skillUpdates;
}
- skills::provider::dto::SkillDescriptionMap
+ std::optional<skills::SkillDescription>
+ SkillProviderComponentPlugin::getSkillDescription(const skills::SkillID& skillId) const
+ {
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ const std::unique_lock l(skillFactoriesMutex);
+ if (skillFactories.find(skillId) == skillFactories.end())
+ {
+ std::stringstream ss;
+ ss << "Skill description for skill '" + skillId.toString() +
+ "' not found! Found instead: {"
+ << "\n";
+ for (const auto& [k, _] : skillFactories)
+ {
+ ss << "\t" << k.toString() << "\n";
+ }
+ ss << "}";
+ ARMARX_WARNING << ss.str();
+
+ return std::nullopt;
+ }
+
+ return skillFactories.at(skillId)->createSkillDescription(*skillId.providerId);
+ }
+
+ std::map<skills::SkillID, skills::SkillDescription>
SkillProviderComponentPlugin::getSkillDescriptions() const
{
- skills::provider::dto::SkillDescriptionMap skillDesciptions;
- const std::unique_lock l(skillsMutex);
- for (const auto& [key, impl] : skillImplementations)
+ std::map<skills::SkillID, skills::SkillDescription> skillDesciptions;
+ const std::unique_lock l(skillFactoriesMutex);
+ for (const auto& [key, fac] : skillFactories)
{
- skillDesciptions.insert({key, impl.skill->description.toIce()});
+ ARMARX_CHECK(key.isFullySpecified());
+ skillDesciptions.insert({key, fac->createSkillDescription(*key.providerId)});
}
return skillDesciptions;
}
-} // namespace armarx::plugins
+ skills::SkillStatusUpdate
+ SkillProviderComponentPlugin::executeSkill(
+ const skills::SkillExecutionRequest& executionRequest)
+ {
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
-namespace armarx
-{
- SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
+ skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .executionStartedTime =
+ armarx::core::time::DateTime::Now()};
+
+ skills::SkillStatusUpdate ret{
+ {executionId, executionRequest.parameters, executionRequest.callbackInterface}};
+
+ skills::detail::SkillRuntime* wrapper;
+ {
+ auto l1 = std::unique_lock{skillFactoriesMutex};
+
+ const auto& fac = getSkillFactory(executionId.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
+
+ {
+ const std::unique_lock l2{skillExecutionsMutex};
+ auto it =
+ skillExecutions.emplace(std::piecewise_construct,
+ std::make_tuple(executionId),
+ std::make_tuple(*fac,
+ executionId,
+ executionRequest.parameters,
+ executionRequest.callbackInterface));
+ wrapper = &it.first->second;
+ }
+
+ // async start execution. But we wait for the execution to finish at the end of this method
+ wrapper->execution = std::thread(
+ [&]()
+ {
+ // execute waits until the previous execution finishes.
+ auto x = wrapper->executeSkill();
+ ret.result = x.result;
+ ret.status = armarx::skills::toSkillStatus(x.status);
+ });
+ } // 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 (wrapper && wrapper->execution.joinable())
+ {
+ wrapper->execution.join();
+
+ // tidy up map of executions
+ const std::unique_lock l2{skillExecutionsMutex};
+ if (auto it = skillExecutions.find(executionId); it != skillExecutions.end())
+ {
+ skillExecutions.erase(it);
+ }
+ }
+ return ret;
+ }
+
+ skills::SkillExecutionID
+ SkillProviderComponentPlugin::executeSkillAsync(
+ const skills::SkillExecutionRequest& executionRequest)
{
- addPlugin(plugin);
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+
+ skills::SkillExecutionID executionId{executionRequest.skillId,
+ executionRequest.executorName,
+ armarx::core::time::DateTime::Now()};
+
+ skills::detail::SkillRuntime* wrapper;
+ {
+ auto l1 = std::unique_lock{skillFactoriesMutex};
+
+ const auto& fac = getSkillFactory(executionId.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
+
+ {
+ const std::unique_lock l2{skillExecutionsMutex};
+ auto it =
+ skillExecutions.emplace(std::piecewise_construct,
+ std::make_tuple(executionId),
+ std::make_tuple(*fac,
+ executionId,
+ executionRequest.parameters,
+ executionRequest.callbackInterface));
+ wrapper = &it.first->second;
+ }
+
+ wrapper->execution = std::thread(
+ [&]()
+ {
+ // execute waits until the previous execution finishes.
+ auto x = wrapper->executeSkill();
+ });
+ }
+
+ // wait until skill is constructed. This assures, that a status update exists.
+ while (true)
+ {
+ {
+ std::scoped_lock l(wrapper->skillStatusesMutex);
+
+ if (wrapper->statusUpdate.hasBeenConstructed())
+ {
+ break;
+ }
+ }
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ }
+
+ return executionId;
}
- void
- SkillProviderComponentPluginUser::addSkill(std::unique_ptr<skills::Skill>&& skill)
+ bool
+ SkillProviderComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const armarx::aron::data::DictPtr& input)
{
- plugin->addSkill(std::move(skill));
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ const std::scoped_lock l{skillExecutionsMutex};
+ auto it = skillExecutions.find(executionId);
+ if (it == skillExecutions.end())
+ {
+ ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
+ "' found! Ignoring prepareSkill request.";
+ return false;
+ }
+
+ std::scoped_lock l2{it->second.skillStatusesMutex};
+ if (it->second.statusUpdate.status != skills::SkillStatus::Preparing)
+ {
+ ARMARX_INFO << "Could not prepare the skill '" + executionId.skillId.toString() +
+ "' because its not in preparing phase.";
+ return false;
+ }
+
+ it->second.updateSkillParameters(input);
+ return true;
}
- void
- SkillProviderComponentPluginUser::addSkill(const skills::LambdaSkill::FunT& f,
- const skills::SkillDescription& desc)
+ bool
+ SkillProviderComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- plugin->addSkill(f, desc);
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ const std::unique_lock l(skillExecutionsMutex);
+ auto it = skillExecutions.find(executionId);
+ if (it == skillExecutions.end())
+ {
+ ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
+ "' found! Ignoring abortSkill request.";
+ return false;
+ }
+
+ auto& runtime = it->second;
+ runtime.stopSkill();
+
+ while (true)
+ {
+ {
+ std::scoped_lock l(runtime.skillStatusesMutex);
+ auto status = runtime.statusUpdate;
+
+ if (status.hasBeenTerminated())
+ {
+ break;
+ }
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ }
+
+ return true;
}
- skills::provider::dto::SkillDescription
- SkillProviderComponentPluginUser::getSkillDescription(const std::string& name,
- const Ice::Current& /*unused*/)
+ bool
+ SkillProviderComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ const std::unique_lock l(skillExecutionsMutex);
+ auto it = skillExecutions.find(executionId);
+ if (it == skillExecutions.end())
+ {
+ ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
+ "' found! Ignoring abortSkill request.";
+ return false;
+ }
+
+ it->second.stopSkill();
+ return true;
+ }
+
+} // namespace armarx::plugins
+
+namespace armarx
+{
+ SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
{
+ addPlugin(plugin);
+ }
- const auto& skillWrapper = plugin->getSkill(name);
- return skillWrapper.skill->description.toIce();
+ IceUtil::Optional<skills::provider::dto::SkillDescription>
+ SkillProviderComponentPluginUser::getSkillDescription(
+ const skills::provider::dto::SkillID& skillId,
+ const Ice::Current& /*unused*/)
+ {
+ auto id = skills::SkillID::FromIce(skillId, skills::ProviderID{.providerName = getName()});
+ auto o = plugin->getSkillDescription(id);
+ if (o.has_value())
+ {
+ return o->toProviderIce();
+ }
+ return {};
}
skills::provider::dto::SkillDescriptionMap
SkillProviderComponentPluginUser::getSkillDescriptions(const Ice::Current& /*unused*/)
{
- return plugin->getSkillDescriptions();
+ skills::provider::dto::SkillDescriptionMap ret;
+ for (const auto& [k, v] : plugin->getSkillDescriptions())
+ {
+ ret.insert({k.toProviderIce(), v.toProviderIce()});
+ }
+ return ret;
}
- skills::provider::dto::SkillStatusUpdate
- SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill,
- const Ice::Current& /*unused*/)
+ IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
+ SkillProviderComponentPluginUser::getSkillExecutionStatus(
+ const skills::provider::dto::SkillExecutionID& executionId,
+ const Ice::Current& /*unused*/)
{
- auto& skillWrapper = plugin->getSkill(skill);
-
- const std::shared_lock l(skillWrapper.skillStatusMutex);
- return skillWrapper.statusUpdate.toIce();
+ auto execId = skills::SkillExecutionID::FromIce(
+ executionId, skills::ProviderID{.providerName = getName()});
+ auto o = plugin->getSkillExecutionStatus(execId);
+ if (o.has_value())
+ {
+ return o->toProviderIce();
+ }
+ return {};
}
skills::provider::dto::SkillStatusUpdateMap
SkillProviderComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& /*unused*/)
{
- return plugin->getSkillExecutionStatuses();
+ skills::provider::dto::SkillStatusUpdateMap ret;
+ for (const auto& [k, v] : plugin->getSkillExecutionStatuses())
+ {
+ ret.insert({k.toProviderIce(), v.toProviderIce()});
+ }
+ return ret;
}
// Please not that this method waits until the skill can be scheduled!
@@ -196,41 +442,59 @@ namespace armarx
const skills::provider::dto::SkillExecutionRequest& info,
const Ice::Current& /*unused*/)
{
- // The skill will be executed in a different thread
- std::thread execution;
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
+ auto up = this->plugin->executeSkill(exec);
+ return up.toProviderIce();
+ }
- // setup input args for skill execution
- skills::SkillParameterization usedParameterization;
- usedParameterization.usedCallbackInterface = info.callbackInterface;
- usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+ skills::provider::dto::SkillExecutionID
+ SkillProviderComponentPluginUser::executeSkillAsync(
+ const skills::provider::dto::SkillExecutionRequest& info,
+ const Ice::Current& current /*unused*/)
+ {
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
+ auto id = this->plugin->executeSkillAsync(exec);
+ return id.toProviderIce();
+ }
- skills::provider::dto::SkillStatusUpdate ret;
- {
- const std::string skillName = info.skillName;
- auto& wrapper = plugin->getSkill(skillName);
+ skills::provider::dto::ParameterUpdateResult
+ SkillProviderComponentPluginUser::updateSkillParameters(
+ const skills::provider::dto::SkillExecutionID& id,
+ const aron::data::dto::DictPtr& input,
+ const Ice::Current& current /*unused*/)
+ {
+ skills::provider::dto::ParameterUpdateResult res;
- // async start execution. But we wait for the execution to finish at the end of this method
- execution = std::thread(
- [&ret, &wrapper, &info, &usedParameterization]()
- {
- // execute waits until the previous execution finishes.
- 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 release the lock for being able to abort the execution
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ auto prep = armarx::aron::data::Dict::FromAronDictDTO(input);
+ res.success = this->plugin->updateSkillParameters(exec, prep);
+ return res;
+ }
- if (execution.joinable())
- {
- execution.join();
- }
- return ret;
+ skills::provider::dto::AbortSkillResult
+ SkillProviderComponentPluginUser::abortSkill(const skills::provider::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
+ {
+ skills::provider::dto::AbortSkillResult res;
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ res.success = this->plugin->abortSkill(exec);
+ return res;
}
- void
- SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current& /*unused*/)
+ skills::provider::dto::AbortSkillResult
+ SkillProviderComponentPluginUser::abortSkillAsync(
+ const skills::provider::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
{
- auto& wrapper = plugin->getSkill(skillName);
- wrapper.skill->notifySkillToStopASAP();
+ skills::provider::dto::AbortSkillResult res;
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ res.success = this->plugin->abortSkillAsync(exec);
+ return res;
}
const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index ab6c8c4662ce303f532e94c6ebddc2d39dba11e3..812cc0a8814db843793038ebdd16a3a927c912cf 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,11 +1,11 @@
#pragma once
+#include <experimental/memory>
#include <functional>
#include <queue>
#include <shared_mutex>
#include <thread>
#include <type_traits>
-#include <experimental/memory>
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -15,18 +15,24 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
// Include all types of skills
+#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
+
#include "LambdaSkill.h"
#include "PeriodicSkill.h"
#include "PeriodicSpecializedSkill.h"
-#include "Skill.h"
+#include "SkillFactory.h"
#include "SpecializedSkill.h"
// Helper wrapper for execution
#include "detail/SkillImplementationWrapper.h"
+
namespace armarx
{
class SkillProviderComponentPluginUser; // forward declaration
}
+
namespace armarx::plugins
{
class SkillProviderComponentPlugin : public ComponentPlugin
@@ -43,40 +49,73 @@ namespace armarx::plugins
void preOnDisconnectComponent() override;
- void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(std::unique_ptr<skills::Skill>&&);
+ void addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&&);
- template <typename T, typename... Args>
- T*
- addSkill(Args&&... args)
+ skills::SkillBlueprint* addSkillFactory(const skills::SkillDescription&,
+ const skills::LambdaSkill::FunctionType&);
+
+ template <class SkillT, typename... Args>
+
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ addSkillFactory(Args&&... args)
{
- static_assert(std::is_base_of<skills::Skill, T>::value,
- "T must be derived from skills::Skill!");
+ auto fac = skills::SkillBlueprint::ForSkill<SkillT>(std::forward<Args>(args)...);
+ auto* facPtr = fac.get();
+ addSkillFactory(std::move(fac));
+ return static_cast<skills::SkillBlueprint*>(facPtr);
+ }
+
+ template <typename FactoryT, typename... Args>
- auto skill = std::make_unique<T>(std::forward<Args>(args)...);
- auto* skillPtr = skill.get();
- addSkill(std::move(skill));
- return static_cast<T*>(skillPtr);
+ requires skills::isSkillBlueprint<FactoryT> FactoryT*
+ addCustomSkillFactory(Args&&... args)
+ {
+ auto fac = std::make_unique<FactoryT>(std::forward<Args>(args)...);
+ auto* facPtr = fac.get();
+ addCustomSkillFactory(std::move(fac));
+ return static_cast<FactoryT*>(facPtr);
}
+ // Ice forwards
+ std::optional<skills::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::SkillExecutionID&) const;
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ getSkillExecutionStatuses() const;
+
+ std::optional<skills::SkillDescription> getSkillDescription(const skills::SkillID&) const;
+ std::map<skills::SkillID, skills::SkillDescription> getSkillDescriptions() const;
+
+ skills::SkillStatusUpdate executeSkill(const skills::SkillExecutionRequest& executionInfo);
+
+ skills::SkillExecutionID
+ executeSkillAsync(const skills::SkillExecutionRequest& executionInfo);
+
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const armarx::aron::data::DictPtr& params);
+
+ bool abortSkill(const skills::SkillExecutionID& execId);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& execId);
+
private:
- skills::detail::SkillImplementationWrapper& getSkill(const std::string& name);
- skills::provider::dto::SkillStatusUpdateMap getSkillExecutionStatuses() const;
- skills::provider::dto::SkillDescriptionMap getSkillDescriptions() const;
+ skills::SkillBlueprint* getSkillFactory(const armarx::skills::SkillID& name);
+
skills::manager::dti::SkillManagerInterfacePrx manager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
- mutable std::shared_mutex skillsMutex;
+ mutable std::mutex skillFactoriesMutex;
+ std::map<skills::SkillID, std::unique_ptr<skills::SkillBlueprint>> skillFactories;
- bool connected = false;
- std::map<std::string, skills::detail::SkillImplementationWrapper> skillImplementations;
+ mutable std::mutex skillExecutionsMutex;
+ std::map<skills::SkillExecutionID, skills::detail::SkillRuntime> skillExecutions;
friend class armarx::SkillProviderComponentPluginUser;
};
} // namespace armarx::plugins
namespace armarx
+
{
class SkillProviderComponentPluginUser :
virtual public ManagedIceObject,
@@ -85,35 +124,72 @@ namespace armarx
public:
SkillProviderComponentPluginUser();
- skills::provider::dto::SkillDescription
- getSkillDescription(const std::string&,
+ // Ice Implementations
+ IceUtil::Optional<skills::provider::dto::SkillDescription>
+ getSkillDescription(const skills::provider::dto::SkillID& skill,
const Ice::Current& current = Ice::Current()) override;
+
skills::provider::dto::SkillDescriptionMap
getSkillDescriptions(const Ice::Current& current = Ice::Current()) override;
- skills::provider::dto::SkillStatusUpdate
- getSkillExecutionStatus(const std::string& skill,
+
+ IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::provider::dto::SkillExecutionID& executionId,
const Ice::Current& current = Ice::Current()) override;
+
skills::provider::dto::SkillStatusUpdateMap
getSkillExecutionStatuses(const Ice::Current& current = Ice::Current()) override;
skills::provider::dto::SkillStatusUpdate
executeSkill(const skills::provider::dto::SkillExecutionRequest& executionInfo,
const Ice::Current& current = Ice::Current()) override;
- void abortSkill(const std::string& name,
+
+ skills::provider::dto::SkillExecutionID
+ executeSkillAsync(const skills::provider::dto::SkillExecutionRequest& executionInfo,
+ const Ice::Current& current = Ice::Current()) override;
+
+ skills::provider::dto::ParameterUpdateResult
+ updateSkillParameters(const skills::provider::dto::SkillExecutionID& executionId,
+ const armarx::aron::data::dto::DictPtr& parameters,
+ const Ice::Current& current = Ice::Current()) override;
+
+ skills::provider::dto::AbortSkillResult
+ abortSkill(const skills::provider::dto::SkillExecutionID& skill,
+ const Ice::Current& current = Ice::Current()) override;
+
+ skills::provider::dto::AbortSkillResult
+ abortSkillAsync(const skills::provider::dto::SkillExecutionID& skill,
const Ice::Current& current = Ice::Current()) override;
+ // Plugin
const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
getSkillProviderPlugin() const;
protected:
- void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(std::unique_ptr<skills::Skill>&&);
+ // -----------------------------------------------------------------------------------------
+ // New
+ // -----------------------------------------------------------------------------------------
+ skills::SkillBlueprint*
+ addSkillFactory(const skills::SkillDescription& desc,
+ const skills::LambdaSkill::FunctionType& f)
+ {
+ auto ret = plugin->addSkillFactory(desc, f);
+ return ret;
+ }
+
+ template <class SkillT, typename... Args>
+
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ addSkillFactory(Args&&... args)
+ {
+ return plugin->addSkillFactory<SkillT>(std::forward<Args>(args)...);
+ }
+
+ template <typename FacT, typename... Args>
- template <typename T, typename... Args>
- T*
- addSkill(Args&&... args)
+ requires skills::isSkillBlueprint<FacT> FacT*
+ addCustomSkillFactory(Args&&... args)
{
- return plugin->addSkill<T>(std::forward<Args>(args)...);
+ return plugin->addSkillFactory<FacT>(std::forward<Args>(args)...);
}
private:
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
deleted file mode 100644
index 93e18a137f3f927017426aab55ae6b2c05507830..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#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;
- }
-
- 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
deleted file mode 100644
index 6fb15914686e6d9f01d0387719b6d5729b98c495..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#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);
-
- void abortSkill(const std::string& executorName);
-
- 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
deleted file mode 100644
index 18c4b8647bf3390a64343546b278155ab6a9ce30..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-#include "SkillStatusUpdate.h"
-
-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.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);
- skills::toIce(ret.header.status, status);
- ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
- 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
deleted file mode 100644
index 6e1ec3c169c01d337ddb94f337414f2443d11ff8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#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
- {
- 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
- 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 3f2b9371e2b087b4a5da77fb06e898bca525c320..d551bd88df7a19144dac07930233dc5293ffc089 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -1,8 +1,7 @@
#pragma once
-#include "Skill.h"
-
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
// Debug
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
@@ -15,127 +14,39 @@ namespace armarx
class SpecializedSkill : public Skill
{
public:
+ using Base = Skill;
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;
- /// returns the accepted type of the skill
- static armarx::aron::type::ObjectPtr GetAcceptedType()
- {
- return AronT::ToAronType();
- }
-
- bool isSkillAvailable(const SpecializedInitInput& in) const
+ void
+ setParameters(const AronT& d)
{
- return this->isAvailable();
- }
+ std::scoped_lock l(this->parametersMutex);
+ this->parameters = d;
- 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);
+ Base::setParameters(d.toAron());
}
- Skill::MainResult executeFullSkill(const SpecializedMainInput& in)
+ AronT
+ getParameters() const
{
- 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;
+ std::scoped_lock l(this->parametersMutex);
+ return this->parameters;
}
- private:
- /// Override this method if you want to disable a skill based on certain conditions
- 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 Skill::InitResult init(const SpecializedInitInput&)
- {
- return InitResult{.status = TerminatedSkillStatus::Succeeded};
- }
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Skill::MainResult main(const SpecializedMainInput& in)
- {
- ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
- return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
- }
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Skill::ExitResult exit(const SpecializedExitInput&)
- {
- return ExitResult{.status = TerminatedSkillStatus::Succeeded};
- }
-
- /// Do not use anymore
- bool isAvailable(const InitInput& in) const final
- {
- AronT p;
- p.fromAron(in.params);
-
- return isAvailable(SpecializedInitInput({.executorName = in.executorName, .params = p}));
- }
-
- /// Do not use anymore
- Skill::InitResult init(const InitInput& in) final
- {
- AronT p;
- p.fromAron(in.params);
-
- return init(SpecializedInitInput({.executorName = in.executorName, .params = p}));
- }
-
- /// Do not use anymore
- Skill::MainResult main(const MainInput& in) final
+ /// returns the accepted type of the skill
+ static armarx::aron::type::ObjectPtr
+ GetAcceptedType()
{
- AronT p;
- p.fromAron(in.params);
-
- return main(SpecializedMainInput({.executorName = in.executorName, .params = p, .callback = in.callback}));
+ return AronT::ToAronType();
}
- /// Do not use anymore
- Skill::ExitResult exit(const ExitInput& in) final
- {
- AronT p;
- p.fromAron(in.params);
- return exit(SpecializedExitInput({.executorName = in.executorName, .params = p}));
- }
+ protected:
+ mutable std::mutex parametersMutex;
+ AronT parameters;
};
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f372c5f55247795f91d956f92d84eb6ac35ee77
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp
@@ -0,0 +1,8 @@
+#include "SpecializedSkillProxy.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h
new file mode 100644
index 0000000000000000000000000000000000000000..4dd53171c4cf17c7298cf9ba59c364ddcf81eb8e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/SkillProxy.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SpecializedSkillProxy : public SkillProxy
+ {
+ public:
+ using SkillProxy::SkillProxy;
+
+ // Provide a similar API as the skillprovider
+ TerminatedSkillStatusUpdate
+ executeSkill(const std::string& executorName, const AronT& params)
+ {
+ return SkillProxy::executeSkill(executorName, params.toAron());
+ }
+
+ SkillExecutionID
+ executeSkillAsync(const std::string& executorName, const AronT& params = nullptr)
+ {
+ return SkillProxy::executeSkillAsync(executorName, params.toAron());
+ }
+
+ // Utiliy methods
+ AronT
+ getRootProfileParameters()
+ {
+ auto dict = SkillProxy::getRootProfileParameters();
+ if (dict)
+ {
+ return AronT::FromAron(dict);
+ }
+ return {};
+ }
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 81e91b131294d542c286659e0691056ea0bc0894..b9529ee7ff2225785ffa8df4a4a2dc55be92076f 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,219 +4,346 @@ namespace armarx
{
namespace skills::detail
{
- SkillImplementationWrapper::SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
- skill(std::move(skill))
+ SkillRuntime::SkillRuntime(
+ const skills::SkillBlueprint& fac,
+ const skills::SkillExecutionID& execId,
+ const aron::data::DictPtr& initial_parameters,
+ const skills::callback::dti::SkillProviderCallbackInterfacePrx& callbackInterface) :
+ factory(fac), statusUpdate{{execId, initial_parameters, callbackInterface}}
{
- ARMARX_CHECK_NOT_NULL(this->skill);
}
- TerminatedSkillStatusUpdate SkillImplementationWrapper::setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization parameterization)
+ // ask a skill to stop
+ void
+ SkillRuntime::stopSkill()
{
- std::unique_lock l(executingMutex);
+ while (this->skill == nullptr)
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to stop a skill that has not been constructed yet... "
+ "Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
- const std::string skillName = skill->description.skillName;
- ARMARX_INFO_S << "Executing skill: " << skillName;
+ this->skill->notifySkillToStop();
+ }
- // 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; I decided to not update the status to idle every time the skill stops.
- statusUpdate.data = nullptr;
- statusUpdate.executorName = "";
- };
+ void
+ SkillRuntime::updateSkillParameters(const aron::data::DictPtr& i)
+ {
+ while (this->skill == nullptr)
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to add params to a skill that has not been constructed "
+ "yet... Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
- // set params and setup variables
- auto setExecParams = [&](){
- std::lock_guard l(skillStatusMutex);
- statusUpdate.usedParameterization = parameterization;
- statusUpdate.executorName = executorName;
- };
+ this->skill->updateParameters(i);
+ }
- resetExecParam();
- setExecParams();
+ TerminatedSkillStatusUpdate
+ SkillRuntime::executeSkill()
+ {
+ std::scoped_lock l(this->executionMutex);
+ // -------------------------------------------------------------------------------------
+ // sanity check
+ // -------------------------------------------------------------------------------------
+ ARMARX_CHECK(this->skill == nullptr)
+ << "A SKILL HAS ALREADY BEEN CONSTRUCTED. THIS SHOULD NOT HAPPEN!!";
+
+
+ // -------------------------------------------------------------------------------------
+ // setup basic vars and lambdas
+ // -------------------------------------------------------------------------------------
+ // actually we should lock... however this is only read access and the members are const throughout the execution...
+ ARMARX_CHECK(statusUpdate.executionId.skillId.isFullySpecified());
+
+ const auto& initial_aron_params = statusUpdate.parameters;
+ const auto& callback_interface = statusUpdate.callbackInterface;
+ const auto& skillId = statusUpdate.executionId.skillId;
+ const auto& skillName = skillId.skillName;
+ const auto& providerId = *skillId.providerId;
+ const auto& executorName = statusUpdate.executionId.executorName;
+ const auto& manager =
+ skills::manager::dti::SkillManagerInterfacePrx::checkedCast(callback_interface);
- auto& aron_params = parameterization.usedInputParams;
- auto updateStatus = [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr){
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = status;
- statusUpdate.data = data;
+ ARMARX_INFO_S << "Executing skill: " << skillName;
- auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
+ auto updateStatus =
+ [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr)
+ {
+ std::unique_lock l(skillStatusesMutex);
+ statusUpdate.status = status;
+ statusUpdate.result = data;
+ if (skill) // if skill has been constructed
+ {
+ // update parameterization
+ statusUpdate.parameters = skill->getParameters();
+ }
- if (callbackInterface)
+ if (callback_interface) // if callback interface is used
{
- callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ auto pid = providerId.toCallbackIce();
+ callback_interface->updateStatusForSkill(statusUpdate.toProviderIce(), pid);
}
};
- auto createErrorMessage = [](const std::string& message){
+ auto createErrorMessageData = [](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});
+ std::unique_lock l(skillStatusesMutex);
+
+ // ---------------------------------------------------------------------------------
+ // check times
+ // ---------------------------------------------------------------------------------
+ if ((armarx::core::time::DateTime::Now() -
+ statusUpdate.executionId.executionStartedTime) >
+ armarx::core::time::Duration::Seconds(1))
+ {
+ ARMARX_WARNING_S << "SkillError 001: For some reason the skill '"
+ << skillId.toString()
+ << "' has been scheduled > 1s ago. The execution should start "
+ "much faster! Continue, but this is bad behavior..";
+ }
}
- if (skill->description.acceptedType && aron_params && not(aron_params->fullfillsType(skill->description.acceptedType)))
+ // -------------------------------------------------------------------------------------
+ // construct skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Construct skill: " << skillName;
+
+ updateStatus(SkillStatus::Constructing);
+ this->skill = this->factory.createSkill(providerId);
+ this->skill->setExecutorName(executorName);
+ this->skill->setManager(manager);
+ this->skill->setCallback([&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
+ { updateStatus(s, d); });
+
+ // set initial parameters that were attached to the execution request (only add as we are not sure whether some updates already arrived)
+ skill->updateParameters(initial_aron_params);
+
+ auto makeAbortedResult = [&](const std::string& message)
{
- 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});
- }
+ updateStatus(SkillStatus::Aborted, createErrorMessageData(message));
+
+ std::unique_lock l(skillStatusesMutex);
+ auto terminated = TerminatedSkillStatusUpdate{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ terminated.status = TerminatedSkillStatus::Aborted;
+ return terminated;
+ };
- // Check if skill is available with the given parameterization
- try
+
+ auto makeFailedResult = [&](const std::string& message)
{
- if (not skill->isSkillAvailable(Skill::InitInput{.executorName = executorName, .params = aron_params}))
- {
- 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)
+ updateStatus(SkillStatus::Failed, createErrorMessageData(message));
+
+ std::unique_lock l(skillStatusesMutex);
+ auto terminated = TerminatedSkillStatusUpdate{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ terminated.status = TerminatedSkillStatus::Failed;
+ return terminated;
+ };
+
+ // -------------------------------------------------------------------------------------
+ // Check params
+ // -------------------------------------------------------------------------------------
+ // NOT RELEVANT ANYMORE!!
+ // if (skill->getSkillDescription().acceptedType && initial_aron_params &&
+ // not(initial_aron_params->fullfillsType(skill->getSkillDescription().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;
+ // return makeTerminationResult(message);
+ // }
+
+
+ auto exitAndMakeFailedResult = [&](const std::string& message)
{
- std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
- ARMARX_ERROR_S << message;
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
- }
+ skill->exitSkill(); // try to exit skill. Ignore return value
+ return makeFailedResult(message);
+ };
+ auto exitAndMakeAbortedResult = [&](const std::string& message)
+ {
+ skill->exitSkill(); // try to exit skill. Ignore return value
+ return makeAbortedResult(message);
+ };
- // set scheduled
- updateStatus(SkillStatus::Scheduled);
+ // Construction succeeded!
+ // -------------------------------------------------------------------------------------
+ // Init skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Init skill: " << skillName;
+ updateStatus(SkillStatus::Initializing);
- // reset skill and perhaps wait for dependencies
try
{
- skill->resetSkill();
+ Skill::InitResult initRet = skill->initSkill();
+ if (initRet.status != TerminatedSkillStatus::Succeeded)
+ {
+ std::string message = "SkillError 101: The initialization of skill '" +
+ skillName + "' did not succeed.";
+ return exitAndMakeFailedResult(message);
+ }
}
- catch (const std::exception& ex)
+ catch (const error::SkillAbortedException& ex)
{
- std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
- ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
}
-
- try
+ catch (const error::SkillFailedException& ex)
{
- skill->waitForDependenciesOfSkill();
+ return exitAndMakeFailedResult(GetHandledExceptionString());
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message =
+ "SkillError 101e: An error occured during the initialization of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeFailedResult(message);
}
- // execute. If the skill fails for some reason, from this point it will always execute its exit function.
- updateStatus(SkillStatus::Running);
-
+ // Init succeeded!
+ // -------------------------------------------------------------------------------------
+ // Prepare skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Prepare skill: " << skillName;
+ updateStatus(SkillStatus::Preparing);
try
{
- Skill::InitResult initRet = skill->initSkill({executorName, aron_params});
- if (initRet.status != TerminatedSkillStatus::Succeeded)
+ auto prepareRet = skill->prepareSkill();
+ while (prepareRet.status == ActiveOrTerminatedSkillStatus::Running)
{
- 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
+ ARMARX_INFO << deactivateSpam() << "Not all requirements for skill "
+ << skillName << " fulfilled. Waiting...";
- updateStatus(skills::toSkillStatus(initRet.status));
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, initRet.status});
+ // wait...
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ prepareRet = skill->prepareSkill();
+ }
+
+ if (prepareRet.status != ActiveOrTerminatedSkillStatus::Succeeded)
+ {
+ std::string message = "SkillError 201: The prepare method of skill '" +
+ skillName + "' did not succeed.";
+ return exitAndMakeFailedResult(message);
}
}
+ catch (const error::SkillAbortedException& ex)
+ {
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return exitAndMakeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
- std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message = "SkillError 201e: An error occured during waiting for skill "
+ "dependencies of skill '" +
+ skillName +
+ "'. The error was: " + GetHandledExceptionString();
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});
+ return exitAndMakeFailedResult(message);
}
- // Init succeeded!
+ // Prepare succeeded!
+ // -------------------------------------------------------------------------------------
+ // Main of skill
+ // -------------------------------------------------------------------------------------
+ // execute. If the skill fails for some reason, from this point it will always execute its exit function.
+ ARMARX_INFO_S << "Main skill: " << skillName;
+ updateStatus(SkillStatus::Running);
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);
- }});
+ mainRet = skill->mainOfSkill();
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});
+ std::string message = "SkillError 501: The main method of skill '" + skillName +
+ "' did not succeed.";
+ return exitAndMakeFailedResult(message);
}
}
+ catch (const error::SkillAbortedException& ex)
+ {
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return exitAndMakeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
- std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message =
+ "SkillError 501e: An error occured during the main method of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
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});
+ return exitAndMakeFailedResult(message);
}
- // Main succeeded!
+ // Main succeeded!
+ // -------------------------------------------------------------------------------------
+ // Exit of skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Exit skill: " << skillName;
try
{
- Skill::ExitResult exitRet = skill->exitSkill({executorName, aron_params});
+ Skill::ExitResult exitRet = skill->exitSkill();
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});
+ std::string message = "SkillError 601: The exit method of skill '" + skillName +
+ "' did not succeed.";
+ return makeFailedResult(message);
}
}
+ catch (const error::SkillAbortedException& ex)
+ {
+ return makeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return makeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
- std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message =
+ "SkillError 601e: An error occured during the exit method of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return makeFailedResult(message);
}
- // Exit succeeded!
-
+ // Exit succeeded!
// All succeeded!
- updateStatus(SkillStatus::Succeeded);
-
- // Tidy up
- resetExecParam();
-
- // return result of main method
- return {{skill->getSkillId(), executorName, parameterization, mainRet.data}, TerminatedSkillStatus::Succeeded};
+ {
+ updateStatus(SkillStatus::Succeeded);
+
+ // return result of main method
+ std::unique_lock l(skillStatusesMutex);
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ ret.result = mainRet.data;
+ ret.status = TerminatedSkillStatus::Succeeded;
+ return ret;
+ }
}
- }
-}
+ } // namespace skills::detail
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 982bcc366dd415ceb85ce055a35992ae99a8165f..e5a5b1f82c3db4e5939fd4c880a738373967402b 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -2,11 +2,12 @@
#include <shared_mutex>
-#include "../SkillDescription.h"
-#include "../SkillStatusUpdate.h"
-#include "../Skill.h"
-
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
namespace armarx
{
@@ -14,28 +15,39 @@ namespace armarx
{
namespace detail
{
- class SkillImplementationWrapper
+ class SkillRuntime
{
- public:
- // fixed values. Do not change after skill instantiation
- const std::unique_ptr<Skill> skill;
+ private:
+ const skills::SkillBlueprint factory;
+
+ std::unique_ptr<Skill> skill;
+ mutable std::mutex executionMutex;
- // Current execution status. Changes during execution
+ public:
+ // Current execution statuses. Changes during execution
// The status also holds the used parameterization
- // skillName and providerName are const after registering the skill in a provider
- mutable std::shared_mutex skillStatusMutex;
+ mutable std::mutex skillStatusesMutex;
SkillStatusUpdate statusUpdate;
- // Task information
- mutable std::shared_mutex executingMutex;
+ // Set exteranally to store the execution somewhere
+ std::thread execution;
// ctor
- SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill);
+ SkillRuntime(const skills::SkillBlueprint& fac,
+ const skills::SkillExecutionID&,
+ const aron::data::DictPtr&,
+ const skills::callback::dti::SkillProviderCallbackInterfacePrx&);
// 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);
+ TerminatedSkillStatusUpdate executeSkill();
+
+ // ask a skill to stop. Must be concurrent to execute skill
+ void stopSkill();
+
+ // add parameters to a skill. Must be concurrent to execute skill
+ void updateSkillParameters(const aron::data::DictPtr& i);
};
- }
- }
-}
+ } // namespace detail
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/skills.cpp b/source/RobotAPI/libraries/skills/skills.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/skills/skills.h b/source/RobotAPI/libraries/skills/skills.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
index 562ddabd56b6d07428f8ce2b386fa682ac95dd1f..56dc86697810e10870f6b409b3f6f810ee9b0729 100644
--- a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
@@ -3,30 +3,34 @@
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
#include "ObjectMemoryGroupStatechartContext.generated.h"
namespace armarx::ObjectMemoryGroup
{
// DO NOT EDIT NEXT LINE
- RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(), &RequestObjects::CreateInstance);
+ RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(),
+ &RequestObjects::CreateInstance);
- RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData):
- XMLStateTemplate<RequestObjects>(stateData), RequestObjectsGeneratedBase<RequestObjects>(stateData)
+ RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<RequestObjects>(stateData),
+ RequestObjectsGeneratedBase<RequestObjects>(stateData)
{
}
- void RequestObjects::onEnter()
+ void
+ RequestObjects::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
}
- void RequestObjects::run()
+ void
+ RequestObjects::run()
{
if (not in.getEnable())
{
@@ -39,7 +43,8 @@ namespace armarx::ObjectMemoryGroup
const std::string provider = in.isProviderSet() ? in.getProvider() : "";
const std::vector<std::string> objectIdsString = in.getObjectIds();
- const armarx::Duration relativeTimeout = armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
+ const armarx::Duration relativeTimeout =
+ armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
std::stringstream info;
std::stringstream warn;
@@ -49,21 +54,22 @@ namespace armarx::ObjectMemoryGroup
{
try
{
- armarx::ObjectID id = armarx::ObjectID::FromString(idString);
- objectIds.push_back(id);
- info << "Requesting object " << id << "\n";
+ armarx::ObjectID id = armarx::ObjectID::FromString(idString);
+ objectIds.push_back(id);
+ info << "Requesting object " << id << "\n";
}
catch (const armarx::LocalException& e)
{
- warn << "\nGiven object ID '" << idString << "' could not parsed as ObjectID: " << e.what();
+ warn << "\nGiven object ID '" << idString
+ << "' could not parsed as ObjectID: " << e.what();
}
}
auto context = getContext<ObjectMemoryGroupStatechartContext>();
armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context);
- Reader reader{mns};
- reader.connect();
+ Reader reader;
+ reader.connect(mns);
objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage();
armarx::objpose::observer::RequestObjectsInput input;
@@ -91,7 +97,8 @@ namespace armarx::ObjectMemoryGroup
{
if (result.result.success)
{
- info << "Requested object " << id << " via provider '" << result.providerName << "'.\n";
+ info << "Requested object " << id << " via provider '" << result.providerName
+ << "'.\n";
}
else
{
@@ -105,8 +112,9 @@ namespace armarx::ObjectMemoryGroup
}
if (not warn.str().empty())
{
- ARMARX_WARNING << "The following issues occurred whhen requesting objects for localization:"
- << warn.str();
+ ARMARX_WARNING
+ << "The following issues occurred whhen requesting objects for localization:"
+ << warn.str();
}
emitSuccess();
@@ -118,16 +126,17 @@ namespace armarx::ObjectMemoryGroup
// // execution time should be short (<100ms)
//}
- void RequestObjects::onExit()
+ void
+ RequestObjects::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
- XMLStateFactoryBasePtr RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
+ XMLStateFactoryBasePtr
+ RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new RequestObjects(stateData));
}
-}
+} // namespace armarx::ObjectMemoryGroup