diff --git a/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx b/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx
new file mode 100644
index 0000000000000000000000000000000000000000..7dee5334bc635e927fe9d020c16b0363dfb53a68
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx
@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArmemGraspMemory" creation="2021-05-07.14:40:52" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="GraspMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="GraspProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArmemGraspMemory/config/DebugObserver.cfg b/scenarios/ArmemGraspMemory/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArmemGraspMemory/config/GraspMemory.cfg b/scenarios/ArmemGraspMemory/config/GraspMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7bd56e9fa827996e3c78f217b2421489af0a748d
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/GraspMemory.cfg
@@ -0,0 +1,263 @@
+# ==================================================================
+# GraspMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.GraspMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.GraspMemory.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.GraspMemory.EnableProfiling = false
+
+
+# ArmarX.GraspMemory.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.GraspMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.GraspMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.ObjectName = ""
+
+
+# ArmarX.GraspMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.GraspMemory.mem.ltm.00_enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.GraspMemory.mem.ltm.00_enabled = true
+
+
+# ArmarX.GraspMemory.memory.Name: Name of this memory server.
+# Attributes:
+# - Default: Grasp
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.memory.Name = Grasp
+
+
+# ArmarX.GraspMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.GraspMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.GraspMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.GraspMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.GraspMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.GraspMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# 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/ArmemGraspMemory/config/GraspProviderExample.cfg b/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..9df508c17697c16e196f1194f847d1f532ee61b9
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg
@@ -0,0 +1,222 @@
+# ==================================================================
+# GraspProviderExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.GraspProviderExample.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.GraspProviderExample.EnableProfiling = false
+
+
+# ArmarX.GraspProviderExample.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.GraspProviderExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.GraspProviderExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspProviderExample.ObjectName = ""
+
+
+# ArmarX.GraspProviderExample.mem.MemoryName: Name of the memory to use.
+# Attributes:
+# - Default: Grasp
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspProviderExample.mem.MemoryName = Grasp
+
+
+# ArmarX.GraspProviderExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.GraspProviderExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.GraspProviderExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspProviderExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.GraspProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspProviderExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# 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/ArmemGraspMemory/config/MemoryNameSystem.cfg b/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg b/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteGuiProvider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArmemGraspMemory/config/global.cfg b/scenarios/ArmemGraspMemory/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..39c0afa137c7362bbb2a5f82234b53de3744ec8c
--- /dev/null
+++ b/scenarios/ArmemGraspMemory/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArmemGraspMemory
+# ==================================================================
+
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 858bff26243d0580d88d158adddb3fc6222e696f..7b65e66ff7b273110366efa1da21ea7eff3412d5 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -296,7 +296,7 @@ namespace armarx::viz
return *this;
}
- Arrow& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ Arrow& fromTo(const Eigen::Vector3f& from, const Eigen::Vector3f& to)
{
position(from);
direction((to - from).normalized());
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index 837e3903004375e41ba0268ea1712682604a151f..39200985c55a98e19f119c6e08e0d45565ca2f9a 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -3,6 +3,7 @@
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
#include "json_base.h"
@@ -194,6 +195,18 @@ namespace armarx::viz
json::from_json_base(j, pose);
}
+ void data::to_json(nlohmann::json& j, const ElementPath& path)
+ {
+ json::to_json_base(j, path);
+ j["points"] = path.points;
+ }
+
+ void data::from_json(const nlohmann::json& j, ElementPath& path)
+ {
+ json::from_json_base(j, path);
+ path.points = j.at("points").get<armarx::Vector3fSeq>();
+ }
+
void data::to_json(nlohmann::json& j, const ElementSphere& sphere)
{
@@ -301,4 +314,3 @@ namespace armarx::viz
}
}
-
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.h b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
index 160948cb459fa8531f005743700c6a44de3fabb0..5e41dc0df1c60d71ec11dafed535e3ec35126603 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
@@ -70,6 +70,10 @@ namespace armarx::viz::data
void from_json(const nlohmann::json& j, ElementPose& pose);
+ void to_json(nlohmann::json& j, const ElementPath& path);
+ void from_json(const nlohmann::json& j, ElementPath& path);
+
+
void to_json(nlohmann::json& j, const ElementRobot& robot);
void from_json(const nlohmann::json& j, ElementRobot& robot);
diff --git a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
index 5dd43185aecb485da85a7372f096b62c79bef10f..48228263d42465bf3a219488ba012437aae93001 100644
--- a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
@@ -1,3 +1,4 @@
+#include <RobotAPI/interface/ArViz/Elements.h>
#include "ElementJsonSerializers.h"
#include "json_elements.h"
@@ -15,6 +16,7 @@ void armarx::viz::json::ElementJsonSerializers::registerElements()
registerSerializer<data::ElementPointCloud>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementPolygon>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementPose>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementPath>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementRobot>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementSphere>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementEllipsoid>(viz::data::to_json, viz::data::from_json);
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index a87d465a301b423defc89260a52cb15164bcd37d..7bd52aec0ffcf6e423cd959f1a8399ba107045ac 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -61,7 +61,7 @@ namespace armarx
}
{
- providerInfo.objectType = objpose::ObjectTypeEnum::KnownObject;
+ providerInfo.objectType = objpose::ObjectType::KnownObject;
std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
for (const auto& obj : objects)
{
@@ -154,7 +154,7 @@ namespace armarx
armarx::objpose::data::ProvidedObjectPose& pose = poses.emplace_back();
pose.providerName = getName();
- pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.objectType = objpose::ObjectType::KnownObject;
pose.objectID.dataset = info.id().dataset();
pose.objectID.className = info.id().className();
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index de02c9f4e7ef45534c2675db6173205436b4475c..c09dd6f9784a4fa0c05812a0e84d22abe4984e1a 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,2 +1,6 @@
add_subdirectory(ExampleMemoryClient)
+
+add_subdirectory(GraspProviderExample)
+
add_subdirectory(VirtualRobotReaderExampleClient)
+
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..18ea059105a516dbafffaba07b3d4c322dabdc11
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/CMakeLists.txt
@@ -0,0 +1,32 @@
+armarx_component_set_name("GraspProviderExample")
+
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem
+ GraspingUtility
+ ${IVT_LIBRARIES}
+)
+
+set(SOURCES
+ GraspProviderExample.cpp
+)
+
+set(HEADERS
+ GraspProviderExample.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+if (IVT_FOUND)
+ target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
+endif()
+
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f98b2383da3556f459ae6fa01a7f3ec190b29e2
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -0,0 +1,170 @@
+#include "GraspProviderExample.h"
+
+#include <SimoxUtility/color/cmaps.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.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>
+
+
+namespace armarx
+{
+ GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr GraspProviderExample::createPropertyDefinitions()
+ {
+ ARMARX_IMPORTANT << "Prperty defs";
+ 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
+ {
+ return "GraspProviderExample";
+ }
+
+ GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem()), reader(memoryNameSystem())
+ {
+
+ }
+
+
+ void GraspProviderExample::onInitComponent()
+ {
+ ARMARX_IMPORTANT << "Init";
+ }
+
+
+ void GraspProviderExample::onConnectComponent()
+ {
+ writer.connect();
+ reader.connect();
+ task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
+ task->start();
+ }
+
+
+ void GraspProviderExample::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+
+ void GraspProviderExample::onExitComponent()
+ {
+ }
+
+
+ void GraspProviderExample::run()
+ {
+ ARMARX_IMPORTANT << "Running example.";
+
+ CycleUtil c(1000);
+ int i = 0;
+
+
+ while (!task->isStopped() && i++ < 100)
+ {
+ // initialize all necessary fields of a grasp candidate and use writer to commit it to memory
+
+ armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
+ candidate.groupNr = i; //non-necessary field, but used to commit different candidates
+ candidate.approachVector = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ candidate.graspPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ candidate.providerName = "Example";
+ candidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ candidate.tcpPoseInHandRoot = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ // source Info is also not necessary, but reference object name is used as entity name
+ // "UnknownObject" if none is provided
+ candidate.sourceInfo = new grasping::GraspCandidateSourceInfo();
+ candidate.sourceInfo->referenceObjectName = "Box";
+ candidate.sourceInfo->bbox = new grasping::BoundingBox();
+ candidate.sourceInfo->bbox->center = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ candidate.sourceInfo->bbox->ha1 = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ candidate.sourceInfo->bbox->ha2 = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ candidate.sourceInfo->bbox->ha3 = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ candidate.sourceInfo->referenceObjectPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
+
+ writer.commitGraspCandidate(candidate, armem::Time::now(), "provider1");
+ // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
+ armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+ bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+ bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ bimanualCandidate.graspPoseLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.graspPoseRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.providerName = "BimanualExample";
+ bimanualCandidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.tcpPoseInHandRootRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.tcpPoseInHandRootLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.inwardsVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
+
+ writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::now(), "provider2");
+
+ //test for writing Seqs, candidates from the same object appear as instances of the same snapshot
+ grasping::GraspCandidateSeq candidatesToWrite;
+ candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
+ candidate.side = "Left";
+
+ candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
+
+ writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::now(), "provider1");
+
+ // test reader and debug by logging the group number of the candidate
+
+ std::map<std::string, grasping::GraspCandidatePtr> candidates;
+
+ try
+ {
+ candidates = reader.queryLatestGraspCandidates();
+ }
+ catch (armem::error::QueryFailed &e)
+ {
+ ARMARX_ERROR << e.makeMsg(memoryName);
+ }
+
+
+ for (auto [id, ca] : candidates)
+ {
+ ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr ;
+ }
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
+
+ try
+ {
+ bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
+ }
+ catch (armem::error::QueryFailed &e)
+ {
+ ARMARX_ERROR << e.makeMsg(memoryName);
+ }
+
+ for (auto [id, ca] : bimanualCandidates)
+ {
+ ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " << ca->groupNr ;
+ }
+
+ c.waitForCycleDuration();
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..efab1f49318c722eaa52302ae662814dc888addc
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
@@ -0,0 +1,56 @@
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
+#pragma once
+
+
+namespace armarx
+{
+
+
+ class GraspProviderExamplePropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ GraspProviderExamplePropertyDefinitions(std::string prefix);
+ };
+
+
+ class GraspProviderExample :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ClientPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+ GraspProviderExample();
+
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void run();
+
+
+ private:
+
+ armarx::RunningTask<GraspProviderExample>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ std::string memoryName = "Grasp";
+ armarx::armem::GraspCandidateWriter writer;
+ armarx::armem::GraspCandidateReader reader;
+ };
+}
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 824eaa4937df6f7f06fa2d5ce7144020feb6097f..4fd2a92cb219cd738cc7e4b33d946e88a5a1ad74 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -3,5 +3,7 @@ add_subdirectory(GeneralPurposeMemory)
add_subdirectory(ObjectMemory)
add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
+add_subdirectory(GraspMemory)
#add_subdirectory(SubjectMemory)
add_subdirectory(MotionMemory)
+
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7bac572cd11bae463d2349529691f2ff5d6b79b2
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name("GraspMemory")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem
+ RobotAPIComponentPlugins # for ArViz and other plugins
+
+ ${IVT_LIBRARIES}
+)
+
+set(SOURCES
+ GraspMemory.cpp
+)
+set(HEADERS
+ GraspMemory.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server::grasp
+)
+
+
+
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b50b214509fb791a4b9f0394bd2c82867ee192ff
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -0,0 +1,101 @@
+
+#include "GraspMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+
+namespace armarx::armem::server::grasp
+{
+ armarx::PropertyDefinitionsPtr GraspMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(memoryName, "memory.Name", "Name of this memory server.");
+
+ return defs;
+ }
+
+
+ std::string GraspMemory::getDefaultName() const
+ {
+ return "GraspMemory";
+ }
+
+ void GraspMemory::onInitComponent()
+ {
+ workingMemory().name() = memoryName;
+
+ workingMemory().addCoreSegment("GraspCandidate",
+ armarx::grasping::arondto::GraspCandidate::toAronType());
+ workingMemory().addCoreSegment("BimanualGraspCandidate",
+ armarx::grasping::arondto::BimanualGraspCandidate::toAronType());
+ }
+
+ void GraspMemory::onConnectComponent()
+ {
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+
+ }
+
+ void GraspMemory::onDisconnectComponent()
+ {
+ }
+
+ void GraspMemory::onExitComponent()
+ {
+ }
+
+
+
+ // WRITING
+
+ armem::data::CommitResult GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ {
+ // This function is overloaded to trigger the remote gui rebuild.
+ armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
+ tab.rebuild = true;
+ return result;
+ }
+
+
+ // READING
+
+ // Inherited from Plugin
+
+
+
+ // REMOTE GUI
+
+ void GraspMemory::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ {
+
+ tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
+ }
+
+ VBoxLayout root = {tab.memoryGroup, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void GraspMemory::RemoteGui_update()
+ {
+ if (tab.rebuild.exchange(false))
+ {
+ createRemoteGuiTab();
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6cf94b3a961e53c179c14132e5d3aaa3a11ba5b
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -0,0 +1,73 @@
+#pragma once
+
+
+#include <memory>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+
+namespace armarx::armem::server::grasp
+{
+ /**
+ * @defgroup Component-GraspMemory GraspMemory
+ * @ingroup RobotAPI-Components
+ * A description of the component GraspMemory.
+ *
+ * @class GraspMemory
+ * @ingroup Component-GraspMemory
+ * @brief Brief description of class GraspMemory.
+ *
+ * Detailed description of class GraspMemory.
+ */
+ class GraspMemory :
+ virtual public armarx::Component
+ , virtual public armem::server::ReadWritePluginUser
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ public:
+ armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+
+
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+
+ DebugObserverInterfacePrx debugObserver;
+
+
+ std::string memoryName = "Grasp";
+
+
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ std::atomic_bool rebuild = false;
+
+ RemoteGui::Client::GroupBox memoryGroup;
+ };
+ RemoteGuiTab tab;
+
+ };
+}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 8e0bf788792e67933850982bb8f51817d9d791eb..8b3f0299ee9732265b757d6c5b375a08e72d554b 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -79,27 +79,28 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
{
return false;
}
- if (filter->objectType != AnyObject && filter->objectType != candidate->objectType)
+ if (filter->objectType != objpose::AnyObject && filter->objectType != candidate->objectType)
{
return false;
}
return true;
}
-std::string GraspCandidateObserver::ObjectTypeToString(ObjectTypeEnum type)
+std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
{
switch (type)
{
- case grasping::AnyObject:
+ case objpose::AnyObject:
return "AnyObject";
- case grasping::KnownObject:
+ case objpose::KnownObject:
return "KnownObject";
- case grasping::UnknownObject:
+ case objpose::UnknownObject:
return "UnknownObject";
default:
return "ERROR";
}
}
+
void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
{
if (updateCounters.count(providerName) == 0)
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index 67adec25eb5d4ad23b59d8833a5836a4ce65edda..f4bb0ba3e7886d970cd33a82a03fe9525057b6c9 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -72,7 +72,7 @@ namespace armarx
public:
static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter, const std::string& providerName, const grasping::GraspCandidatePtr& candidate);
- static std::string ObjectTypeToString(grasping::ObjectTypeEnum type);
+ static std::string ObjectTypeToString(objpose::ObjectType type);
// GraspCandidateProviderListener interface
public:
diff --git a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
index a46e191dcc34615698951568a86ceddac3231ac3..0cddd8405f669bf3b65ddd1e3c56d40317a4ff5d 100644
--- a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
@@ -260,13 +260,13 @@ namespace armarx
_ui.labelGraspgroupNr ->setText(QString::number(e->gc->groupNr));
switch (e->gc->objectType)
{
- case grasping::ObjectTypeEnum::AnyObject :
+ case objpose::ObjectType::AnyObject :
_ui.labelGraspObjType->setText("AnyObject");
break;
- case grasping::ObjectTypeEnum::KnownObject :
+ case objpose::ObjectType::KnownObject :
_ui.labelGraspObjType->setText("KnownObject");
break;
- case grasping::ObjectTypeEnum::UnknownObject :
+ case objpose::ObjectType::UnknownObject :
_ui.labelGraspObjType->setText("UnknownObject");
break;
}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index adb7c77bd1e04491b7e16ecb1599119df0ee2ce6..c5f588adf750b8b2eda12375c798b7d80c741dfa 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -216,7 +216,7 @@ namespace armarx
int col = 0;
item->setText(col++, QString::fromStdString(pose.objectID.str()));
item->setText(col++, QString::fromStdString(pose.providerName));
- item->setText(col++, QString::fromStdString(objpose::ObjectTypeEnumNames.to_name(pose.objectType)));
+ item->setText(col++, QString::fromStdString(objpose::ObjectTypeNames.to_name(pose.objectType)));
{
std::stringstream ss;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a026e17c6b4a4f036aa600c4b37d6f4e2aadd1c4..a7cc9ca79c5ecc5f99f294065709cc3daa0e2790 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -26,6 +26,7 @@ set(SLICE_FILES
core/CartesianNaturalPositionControllerConfig.ice
core/TopicTimingTest.ice
core/FTSensorValue.ice
+ core/NameValueMap.ice
selflocalisation/SelfLocalisationProcess.ice
diff --git a/source/RobotAPI/interface/core/NameValueMap.ice b/source/RobotAPI/interface/core/NameValueMap.ice
new file mode 100644
index 0000000000000000000000000000000000000000..b15090a586f47458bc3ed9a158a107dbfd258810
--- /dev/null
+++ b/source/RobotAPI/interface/core/NameValueMap.ice
@@ -0,0 +1,32 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI
+* @author Christoph Pohl
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+ /**
+ * [NameValueMap] defined. This data container is mostly used to assign values to e.g. joints which are identified by name.
+ **/
+ dictionary<string, float> NameValueMap;
+}
\ No newline at end of file
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 18201e65d3c652bd02c6957bbaff50cb9e8f46cc..7eef5984e466173433e340cf45711be4fd187c95 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -37,7 +37,7 @@ module armarx
struct ProviderInfo
{
ObjectPoseProvider* proxy;
- ObjectTypeEnum objectType = AnyObject;
+ ObjectType objectType = AnyObject;
armarx::data::ObjectIDSeq supportedObjects;
};
dictionary<string, ProviderInfo> ProviderInfoMap;
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 0dc650c9b7bb6bf9b139714015ea55120a409f0d..7ce071730ed3f35b8f2e526c6bf585d000af73f8 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -24,23 +24,23 @@
#pragma once
#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice>
module armarx
{
- // Originally defined in <RobotAPI/interface/units/KinematicUnitInterface.ice>
- dictionary<string, float> NameValueMap;
-
-
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
- enum ObjectTypeEnum
+ // ObjectTypeEnum is now renamed to ObjectType
+ enum ObjectType
{
AnyObject, KnownObject, UnknownObject
};
+
+
class AABB
{
Vector3Base center;
@@ -67,7 +67,7 @@ module armarx
/// Name of the providing component.
string providerName;
/// Known or unknown object.
- ObjectTypeEnum objectType = AnyObject;
+ ObjectType objectType = AnyObject;
/// Whether object is static. Static objects don't decay.
bool isStatic = false;
@@ -100,7 +100,7 @@ module armarx
/// Name of the providing component.
string providerName;
/// Known or unknown object.
- ObjectTypeEnum objectType = AnyObject;
+ ObjectType objectType = AnyObject;
/// Whether object is static. Static objects don't decay.
bool isStatic = false;
diff --git a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
index 5a59f5b4432e83a28977ca234b98bb9a5e07ddd6..b60aa19c3db3bd5d7f1a68937758e401745f3ef0 100644
--- a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
@@ -35,7 +35,7 @@ module armarx
{
string providerName = "*";
float minimumSuccessProbability = 0;
- ObjectTypeEnum objectType = AnyObject;
+ armarx::objpose::ObjectType objectType = AnyObject;
ApertureType preshape = AnyAperture;
ApproachType approach = AnyApproach;
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index ac4da1da07d1ac2c75526a2f98c32c547aa3d88d..6b45362c42cdc04ae6b02715112a2173483d91af 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -27,14 +27,17 @@
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/RequestableService.ice>
+#include <RobotAPI/interface/objectpose/object_pose_types.ice>
module armarx
{
module grasping
{
- enum ObjectTypeEnum {
+ // Grasping will now use ObjectType from armarx::objpose in <RobotAPI/interface/objectpose/object_pose_types.ice>
+ /*enum ObjectTypeEnum {
AnyObject, KnownObject, UnknownObject
- };
+ };*/
+
enum ApproachType {
AnyApproach, TopApproach, SideApproach
};
@@ -79,6 +82,7 @@ module armarx
{
PoseBase graspPose;
PoseBase robotPose;
+ PoseBase tcpPoseInHandRoot;
Vector3Base approachVector;
string sourceFrame; // frame where graspPose is located
@@ -87,7 +91,7 @@ module armarx
float graspSuccessProbability;
- ObjectTypeEnum objectType = AnyObject;
+ armarx::objpose::ObjectType objectType = AnyObject;
int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
string providerName;
@@ -101,6 +105,8 @@ module armarx
PoseBase graspPoseRight;
PoseBase graspPoseLeft;
PoseBase robotPose;
+ PoseBase tcpPoseInHandRootRight;
+ PoseBase tcpPoseInHandRootLeft;
Vector3Base approachVectorRight;
Vector3Base approachVectorLeft;
@@ -113,7 +119,7 @@ module armarx
//float graspSuccessProbability;
- ObjectTypeEnum objectType = AnyObject;
+ armarx::objpose::ObjectType objectType = AnyObject;
int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
string providerName;
@@ -130,7 +136,7 @@ module armarx
class ProviderInfo
{
- ObjectTypeEnum objectType = AnyObject;
+ armarx::objpose::ObjectType objectType = AnyObject;
StringVariantBaseMap currentConfig;
};
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index 85edf827c17242010d6747f383a5aaac6d7eccc8..5321b009b55f944df51ba9d2e0b826eacd15e903 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -29,6 +29,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/skills/SkillObserverInterface.ice>
module armarx
@@ -124,10 +125,7 @@ module armarx
{
Ice::StringSeq nodes;
};
- /**
- * [NameValueMap] defined. This data container is mostly used to assign values to e.g. joints which are identified by name.
- **/
- dictionary<string, float> NameValueMap;
+
/**
* [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
**/
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 23d54901fbf7fa5db2c222a09c268da111a22512..ca5754aa0dff87780e913f7c255bf2b9067e2258 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -50,7 +50,7 @@ namespace armarx::objpose
/// Name of the providing component.
std::string providerName;
/// Known or unknown object.
- ObjectTypeEnum objectType = AnyObject;
+ ObjectType objectType = AnyObject;
/// Whether object is static. Static objects don't decay.
bool isStatic = false;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index a639223a050b5473c69302816ee1a46adceb505a..5fc8ebdea395c6b8bcd149c14fcb2fe567a4ace8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -24,33 +24,33 @@ void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAtt
dto.poseInFrame = bo.poseInFrame;
}
-void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo)
+void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
{
switch (dto.value)
{
case arondto::ObjectType::AnyObject:
- bo = ObjectTypeEnum::AnyObject;
+ bo = ObjectType::AnyObject;
return;
case arondto::ObjectType::KnownObject:
- bo = ObjectTypeEnum::KnownObject;
+ bo = ObjectType::KnownObject;
return;
case arondto::ObjectType::UnknownObject:
- bo = ObjectTypeEnum::UnknownObject;
+ bo = ObjectType::UnknownObject;
return;
}
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo)
+void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
{
switch (bo)
{
- case ObjectTypeEnum::AnyObject:
+ case ObjectType::AnyObject:
dto.value = arondto::ObjectType::AnyObject;
return;
- case ObjectTypeEnum::KnownObject:
+ case ObjectType::KnownObject:
dto.value = arondto::ObjectType::KnownObject;
return;
- case ObjectTypeEnum::UnknownObject:
+ case ObjectType::UnknownObject:
dto.value = arondto::ObjectType::UnknownObject;
return;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
index 2e3f5901bfaf45f0cf131f609d46a18d67ace532..6117009c5ca366ad0fa0082161f3ee67d39c2892 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -18,8 +18,8 @@ namespace armarx::objpose
void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
- void fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo);
- void toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo);
+ void fromAron(const arondto::ObjectType& dto, ObjectType& bo);
+ void toAron(arondto::ObjectType& dto, const ObjectType& bo);
void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
index 6715760410010d3817c52d677a0a64036fb828c6..ccbae7e21c426f92d946dd6744168a04f2c70e7c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
@@ -82,11 +82,11 @@ armarx::data::ObjectIDSeq armarx::toIce(const std::vector<ObjectID>& ids)
namespace armarx
{
- const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
+ const simox::meta::EnumNames<objpose::ObjectType> objpose::ObjectTypeNames =
{
- { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
- { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
- { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
+ { objpose::ObjectType::AnyObject, "AnyObject" },
+ { objpose::ObjectType::KnownObject, "KnownObject" },
+ { objpose::ObjectType::UnknownObject, "UnknownObject" }
};
objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
index 39ebc20500722518472c9c755a3238d581916fd0..429d6246c18f7a36769d6c3d686c12c175cd8b8f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
@@ -41,7 +41,7 @@ namespace armarx
namespace armarx::objpose
{
- extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
+ extern const simox::meta::EnumNames<objpose::ObjectType> ObjectTypeNames;
objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 389aae5e2ddc59fdf533bf0a43309f89536c1b6e..3888855892a95cecb6ea5fecc55e63763c894f6a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -26,7 +26,7 @@ void armarx::from_json(const nlohmann::json& j, ObjectID& id)
void armarx::objpose::to_json(nlohmann::json& j, const ObjectPose& op)
{
j["providerName"] = op.providerName;
- j["objectType"] = ObjectTypeEnumNames.to_name(op.objectType);
+ j["objectType"] = ObjectTypeNames.to_name(op.objectType);
j["objectID"] = op.objectID;
@@ -51,7 +51,7 @@ void armarx::objpose::to_json(nlohmann::json& j, const ObjectPose& op)
void armarx::objpose::from_json(const nlohmann::json& j, ObjectPose& op)
{
op.providerName = j.at("providerName");
- op.objectType = ObjectTypeEnumNames.from_name(j.at("objectType"));
+ op.objectType = ObjectTypeNames.from_name(j.at("objectType"));
op.objectID = j.at("objectID");
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index e88102be2b34c52d63b73ff876ab6ba8e778f1f5..cce3efe6d385799d6496d39d0d0ca4362177403e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -6,15 +6,30 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS RobotAPICore
RobotStatechartHelpers # Contains RobotNameHelper
+ RobotAPI::armem
+ RobotAPI::ArmarXObjects
SOURCES box_to_grasp_candidates.cpp
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp
BimanualGraspCandidateHelper.cpp
+ aron_conversions.cpp
+ GraspCandidateWriter.cpp
+ GraspCandidateReader.cpp
HEADERS box_to_grasp_candidates.h
box_to_grasp_candidates.ipp
grasp_candidate_drawer.h
GraspCandidateHelper.h
BimanualGraspCandidateHelper.h
+ aron_conversions.h
+ GraspCandidateWriter.h
+ GraspCandidateReader.h
)
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/GraspCandidate.xml
+)
+
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ff7402b4b2db5b833adabcb48f6d5ffb554ec39
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -0,0 +1,301 @@
+#include "GraspCandidateReader.h"
+
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+namespace armarx::armem
+{
+ GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem)
+ : memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+ void GraspCandidateReader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
+ << properties.memoryName << "' ...";
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
+ << properties.memoryName;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+
+ }
+
+
+
+
+ armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
+ {
+ armarx::grasping::GraspCandidate candidate;
+
+ grasping::arondto::GraspCandidate aronTransform;
+
+ aronTransform.fromAron(instance.data());
+
+ fromAron(aronTransform, candidate);
+
+ return candidate;
+ }
+
+ armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
+ {
+ armarx::grasping::BimanualGraspCandidate candidate;
+
+ grasping::arondto::BimanualGraspCandidate aronTransform;
+ aronTransform.fromAron(instance.data());
+
+ fromAron(aronTransform, candidate);
+
+ return candidate;
+ }
+
+
+ grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+ qb.singleEntitySnapshot(id.getEntitySnapshotID());
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ if (!qResult.success)
+ {
+ // todo catch in provider
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
+ }
+
+
+ armarx::grasping::GraspCandidatePtr candidate;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ {
+ if (instance.id() == id)
+ {
+ candidate = new grasping::GraspCandidate(asGraspCandidate(instance));
+ }
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
+ return candidate;
+ }
+
+ grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
+ const armem::MemoryID& id) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+ qb.singleEntitySnapshot(id.getEntitySnapshotID());
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ if (!qResult.success)
+ {
+ // todo catch in provider
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
+ }
+
+
+ armarx::grasping::BimanualGraspCandidatePtr candidate;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ {
+ if (instance.id() == id)
+ {
+ candidate = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ }
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
+ return candidate;
+ }
+
+ std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidateEntity(
+ const std::string& provider, const std::string& entity) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().withName(entity)
+ .snapshots().latest();
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ return getGraspCandidatesFromResultSet(qResult);
+ }
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(
+ const std::string& provider, const std::string& entity) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+ qb
+ .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().withName(entity)
+ .snapshots().latest();
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ return getBimanualGraspCandidatesFromResultSet(qResult);
+ }
+
+ std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidates(
+ const std::string& provider) const
+ {
+
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+
+ if (provider != "")
+ {
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().all()
+ .snapshots().latest();
+ }
+ else
+ {
+ qb
+ .coreSegments().withName(properties.graspCandidateMemoryName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ return getGraspCandidatesFromResultSet(qResult);
+ }
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidates(
+ const std::string& provider) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+
+ if (provider != "")
+ {
+ qb
+ .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments().withName(provider)
+ .entities().all()
+ .snapshots().latest();
+ }
+ else
+ {
+ qb
+ .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ return getBimanualGraspCandidatesFromResultSet(qResult);
+ }
+
+
+ void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
+ registerPropertyDefinitions(def);
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.graspCandidateMemoryName, prefix + "GraspCandidateMemoryName",
+ "Name of the grasping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::getGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
+ {
+ if (!qResult.success)
+ {
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
+ }
+
+
+ std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ {
+ candidates[instance.id().str()] = new grasping::GraspCandidate(asGraspCandidate(instance));
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
+
+ return candidates;
+ }
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
+ {
+ if (!qResult.success)
+ {
+ throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
+ }
+
+
+ std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
+
+ armem::wm::FunctionalVisitor visitor;
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ {
+ candidates[instance.id().str()] = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ return true;
+ };
+
+ visitor.applyTo(qResult.memory);
+
+
+ return candidates;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..0a1604afdb9ca58c93ce52628cee61e7ee647dcc
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+namespace armarx::armem
+{
+
+ class GraspCandidateReader
+ {
+ public:
+ GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
+
+ void connect();
+
+ grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+
+ grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+
+ std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidateEntity(
+ std::string const& provider, std::string const& entity) const;
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
+ std::string const& provider, std::string const& entity) const;
+
+ std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidates(
+ std::string const& provider = "") const;
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
+ std::string const& provider = "") const;
+
+
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+
+ private:
+
+ std::map<std::string, grasping::GraspCandidatePtr> getGraspCandidatesFromResultSet(
+ armem::client::QueryResult const& qResult) const;
+
+ std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
+ armem::client::QueryResult const& qResult) const;
+
+ armem::client::Reader memoryReader;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "Grasp";
+ std::string graspCandidateMemoryName = "GraspCandidate";
+ std::string bimanualGraspCandidateMemoryName = "BimanualGraspCandidate";
+ } properties;
+
+
+ const std::string propertyPrefix = "mem.grasping.";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c998ec1bcf43e235a8eee2706205ea15efe9964
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -0,0 +1,190 @@
+#include "GraspCandidateWriter.h"
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+namespace armarx::armem
+{
+
+
+ GraspCandidateWriter::GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem)
+ : memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+ void GraspCandidateWriter::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(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)
+ {
+ ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.graspMemoryName, prefix + "GraspMemoryName",
+ "Name of the grasping memory core segment to use.");
+ 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)
+ {
+ armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
+ std::string objectName = "UnknownObject";
+ if (candidate.sourceInfo)
+ {
+ objectName = candidate.sourceInfo->referenceObjectName;
+ }
+
+
+ toAron(aronGraspCandidate, candidate);
+ auto dict = aronGraspCandidate.toAron();
+
+ return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
+ }
+
+ bool GraspCandidateWriter::commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
+ const armem::Time& timestamp, const std::string& provider)
+ {
+ armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
+
+ std::string objectName = "UnknownObject";
+ if (candidate.sourceInfo)
+ {
+ objectName = candidate.sourceInfo->referenceObjectName;
+ }
+
+ toAron(aronGraspCandidate, candidate);
+
+ auto dict = aronGraspCandidate.toAron();
+
+ 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 success = true;
+
+ std::map<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> updates = {};
+ armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
+
+ for (armarx::grasping::GraspCandidatePtr candidate : candidates)
+ {
+ std::string objectName = "UnknownObject";
+ if (candidate->sourceInfo)
+ {
+ objectName = candidate->sourceInfo->referenceObjectName;
+ }
+ toAron(aronGraspCandidate, *candidate);
+
+ updates[objectName].push_back(aronGraspCandidate.toAron());
+ }
+
+ for (std::pair<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> update : updates)
+ {
+
+ if (! commitToMemory(update.second, provider, update.first, timestamp, properties.graspMemoryName))
+ {
+ success = false;
+ }
+ }
+ return success;
+ }
+
+ bool GraspCandidateWriter::commitBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates, const armem::Time& timestamp,
+ const std::string& provider)
+ {
+ bool success = true;
+
+ std::map<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> updates = {};
+ armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
+
+ for (armarx::grasping::BimanualGraspCandidatePtr candidate : candidates)
+ {
+ std::string objectName = "UnknownObject";
+ if (candidate->sourceInfo)
+ {
+ objectName = candidate->sourceInfo->referenceObjectName;
+ }
+ toAron(aronGraspCandidate, *candidate);
+ updates[objectName].push_back(aronGraspCandidate.toAron());
+ }
+
+ for (std::pair<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> update : updates)
+ {
+
+ if (! commitToMemory(update.second, provider, update.first, timestamp, properties.bimanualGraspMemoryName))
+ {
+ success = false;
+ }
+ }
+ return success;
+ }
+
+ bool GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::datanavigator::DictNavigatorPtr>& 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);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp.toMicroSeconds());
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId.withEntityName(entityName).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+
+
+ update.instancesData = instances;
+ update.timeCreated = iceTimestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..10dc74821d92d7f7d8005e5ad347df67be9da777
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
@@ -0,0 +1,58 @@
+#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>
+
+namespace armarx::armem
+{
+ class GraspCandidateWriter
+ {
+ public:
+ GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+
+ void connect();
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+ bool commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
+ const armem::Time& timestamp, const std::string& provider);
+ bool commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
+ 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);
+
+
+
+ private:
+
+ bool commitToMemory(const std::vector<armarx::aron::datanavigator::DictNavigatorPtr>& instances,
+ 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 bimanualGraspMemoryName = "BimanualGraspCandidate";
+ } properties;
+
+
+ std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.grasping.";
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
new file mode 100644
index 0000000000000000000000000000000000000000..592c9fe677ecb0e8ad9b4e55c59a9560f336f439
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -0,0 +1,258 @@
+<!-- ARON DTO of armarx::objpose::ObjectPose. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <IntEnum name="armarx::grasping::arondto::ApproachType">
+ <EnumValue key="AnyApproach" value="0" />
+ <EnumValue key="TopApproach" value="1" />
+ <EnumValue key="SideApproach" value="2" />
+ </IntEnum>
+
+ <IntEnum name="armarx::grasping::arondto::ApertureType">
+ <EnumValue key="AnyAperture" value="0" />
+ <EnumValue key="OpenAperture" value="1" />
+ <EnumValue key="PreshapedAperture" value="2" />
+ </IntEnum>
+
+
+ <Object name="armarx::grasping::arondto::BoundingBox">
+ <ObjectChild key='center'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='ha1'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='ha2'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='ha3'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::grasping::arondto::GraspCandidateSourceInfo">
+ <ObjectChild key='referenceObjectPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='referenceObjectName'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='segmentationLabelID'>
+ <int />
+ </ObjectChild>
+ <ObjectChild key='bbox'>
+ <armarx::grasping::arondto::BoundingBox />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::grasping::arondto::GraspCandidateExecutionHints">
+ <ObjectChild key='preshape'>
+ <armarx::grasping::arondto::ApertureType />
+ </ObjectChild>
+ <ObjectChild key='approach'>
+ <armarx::grasping::arondto::ApproachType />
+ </ObjectChild>
+ <ObjectChild key='graspTrajectoryName'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::grasping::arondto::GraspCandidateReachabilityInfo">
+ <ObjectChild key='reachable'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='minimumJointLimitMargin'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='jointLimitMargins'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='maxPosError'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='maxOriError'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::grasping::arondto::GraspCandidate'>
+
+ <ObjectChild key='graspPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='robotPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='tcpPoseInHandRoot'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='approachVector'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='sourceFrame'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='targetFrame'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='side'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='graspSuccessProbability'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='objectType'>
+ <armarx::objpose::arondto::ObjectType />
+ </ObjectChild>
+
+ <ObjectChild key='groupNr'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='providerName'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='sourceInfoValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='sourceInfo'>
+ <armarx::grasping::arondto::GraspCandidateSourceInfo />
+ </ObjectChild>
+
+ <ObjectChild key='executionHintsValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='executionHints'>
+ <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ </ObjectChild>
+
+ <ObjectChild key='reachabilityInfoValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='reachabilityInfo'>
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name='armarx::grasping::arondto::BimanualGraspCandidate'>
+
+ <ObjectChild key='graspPoseRight'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='graspPoseLeft'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='robotPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='tcpPoseInHandRootRight'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='tcpPoseInHandRootLeft'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='approachVectorRight'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='approachVectorLeft'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='inwardsVectorRight'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='inwardsVectorLeft'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='sourceFrame'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='targetFrame'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='objectType'>
+ <armarx::objpose::arondto::ObjectType />
+ </ObjectChild>
+
+ <ObjectChild key='groupNr'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='providerName'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='sourceInfoValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='sourceInfo'>
+ <armarx::grasping::arondto::GraspCandidateSourceInfo />
+ </ObjectChild>
+
+ <ObjectChild key='executionHintsRightValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='executionHintsRight'>
+ <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ </ObjectChild>
+
+ <ObjectChild key='executionHintsLeftValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='executionHintsLeft'>
+ <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ </ObjectChild>
+
+ <ObjectChild key='reachabilityInfoRightValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='reachabilityInfoRight'>
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ </ObjectChild>
+
+ <ObjectChild key='reachabilityInfoLeftValid'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='reachabilityInfoLeft'>
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ </ObjectChild>
+
+ <ObjectChild key='graspName'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01f6adc1a9fe479580104e9e7de110de6b09aaef
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -0,0 +1,377 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
+ armarx::grasping::BoundingBox& bo)
+{
+ bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
+ const armarx::grasping::BoundingBox& bo)
+{
+ dto.center = fromIce(bo.center);
+ dto.ha1 = fromIce(bo.ha1);
+ dto.ha2 = fromIce(bo.ha2);
+ dto.ha3 = fromIce(bo.ha3);
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ armarx::grasping::GraspCandidateSourceInfo& bo)
+{
+ bo.bbox = new BoundingBox();
+ fromAron(dto.bbox, *bo.bbox);
+ bo.referenceObjectName = dto.referenceObjectName;
+ bo.referenceObjectPose = toIce(dto.referenceObjectPose);
+ bo.segmentationLabelID = dto.segmentationLabelID;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ const armarx::grasping::GraspCandidateSourceInfo& bo)
+{
+ toAron(dto.bbox, *bo.bbox);
+ dto.referenceObjectName = bo.referenceObjectName;
+ dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ dto.segmentationLabelID = bo.segmentationLabelID;
+
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ armarx::grasping::GraspCandidateReachabilityInfo& bo)
+{
+ bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
+ dto.maxPosError, dto.maxOriError);
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ const armarx::grasping::GraspCandidateReachabilityInfo& bo)
+{
+ dto.jointLimitMargins = bo.jointLimitMargins;
+ dto.maxOriError = bo.maxOriError;
+ dto.maxPosError = bo.maxPosError;
+ dto.minimumJointLimitMargin = bo.minimumJointLimitMargin;
+ dto.reachable = bo.reachable;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
+ armarx::grasping::GraspCandidate& bo)
+{
+ bo = GraspCandidate();
+ bo.graspPose = toIce(dto.graspPose);
+ bo.robotPose = toIce(dto.robotPose);
+ bo.tcpPoseInHandRoot = toIce(dto.tcpPoseInHandRoot);
+ bo.approachVector = toIce(dto.approachVector);
+ bo.sourceFrame = dto.sourceFrame;
+ bo.targetFrame = dto.targetFrame;
+ bo.side = dto.side;
+ bo.graspSuccessProbability = dto.graspSuccessProbability;
+ fromAron(dto.objectType, bo.objectType);
+ if (dto.executionHintsValid)
+ {
+ bo.executionHints = new GraspCandidateExecutionHints();
+ fromAron(dto.executionHints, *bo.executionHints);
+ }
+ else
+ {
+ bo.executionHints = nullptr;
+ }
+ bo.groupNr = dto.groupNr;
+ bo.providerName = dto.providerName;
+ if (dto.reachabilityInfoValid)
+ {
+ bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
+ fromAron(dto.reachabilityInfo, *bo.reachabilityInfo);
+ }
+ else
+ {
+ bo.reachabilityInfo = nullptr;
+ }
+ if (dto.sourceInfoValid)
+ {
+ bo.sourceInfo = new GraspCandidateSourceInfo();
+ fromAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ bo.sourceInfo = nullptr;
+ }
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
+ const armarx::grasping::GraspCandidate& bo)
+{
+ dto.approachVector = fromIce(bo.approachVector);
+ if (bo.executionHints)
+ {
+ dto.executionHintsValid = true;
+ toAron(dto.executionHints, *bo.executionHints);
+ }
+ else
+ {
+ dto.executionHintsValid = false;
+ dto.executionHints.toAron();
+ }
+ dto.graspPose = fromIce(bo.graspPose);
+ dto.graspSuccessProbability = bo.graspSuccessProbability;
+ dto.groupNr = bo.groupNr;
+ toAron(dto.objectType, bo.objectType);
+ dto.providerName = bo.providerName;
+ if (bo.reachabilityInfo)
+ {
+ dto.reachabilityInfoValid = true;
+ toAron(dto.reachabilityInfo, *bo.reachabilityInfo);
+ }
+ else
+ {
+ dto.reachabilityInfoValid = false;
+ dto.reachabilityInfo.toAron();
+ }
+ dto.robotPose = fromIce(bo.robotPose);
+ dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
+ dto.side = bo.side;
+ dto.sourceFrame = bo.sourceFrame;
+ if (bo.sourceInfo)
+ {
+ dto.sourceInfoValid = true;
+ toAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ dto.sourceInfoValid = false;
+ dto.sourceInfo.toAron();
+ }
+ dto.targetFrame = bo.targetFrame;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ armarx::grasping::BimanualGraspCandidate& bo)
+{
+ bo = BimanualGraspCandidate();
+ bo.graspPoseRight = toIce(dto.graspPoseRight);
+ bo.graspPoseLeft = toIce(dto.graspPoseLeft);
+ bo.robotPose = toIce(dto.robotPose);
+ bo.tcpPoseInHandRootRight = toIce(dto.tcpPoseInHandRootRight);
+ bo.tcpPoseInHandRootLeft = toIce(dto.tcpPoseInHandRootLeft);
+ bo.approachVectorRight = toIce(dto.approachVectorRight);
+ bo.approachVectorLeft = toIce(dto.approachVectorLeft);
+ bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
+ bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
+ bo.sourceFrame = dto.sourceFrame;
+ bo.targetFrame = dto.targetFrame;
+ fromAron(dto.objectType, bo.objectType);
+ if (dto.executionHintsRightValid)
+ {
+ bo.executionHintsRight = new GraspCandidateExecutionHints();
+ fromAron(dto.executionHintsRight, *bo.executionHintsRight);
+ }
+ else
+ {
+ bo.executionHintsRight = nullptr;
+ }
+ if (dto.executionHintsLeftValid)
+ {
+ bo.executionHintsLeft = new GraspCandidateExecutionHints();
+ fromAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ }
+ else
+ {
+ bo.executionHintsLeft = nullptr;
+ }
+ bo.groupNr = dto.groupNr;
+ bo.providerName = dto.providerName;
+ if (dto.reachabilityInfoRightValid)
+ {
+ bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
+ fromAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ }
+ else
+ {
+ bo.reachabilityInfoRight = nullptr;
+ }
+ if (dto.reachabilityInfoLeftValid)
+ {
+ bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
+ fromAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ }
+ else
+ {
+ bo.reachabilityInfoLeft = nullptr;
+ }
+ if (dto.sourceInfoValid)
+ {
+ bo.sourceInfo = new GraspCandidateSourceInfo();
+ fromAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ bo.sourceInfo = nullptr;
+ }
+ bo.graspName = dto.graspName;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ const armarx::grasping::BimanualGraspCandidate& bo)
+{
+ dto.approachVectorRight = fromIce(bo.approachVectorRight);
+ dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
+ if (bo.executionHintsRight)
+ {
+ dto.executionHintsRightValid = true;
+ toAron(dto.executionHintsRight, *bo.executionHintsRight);
+ }
+ else
+ {
+ dto.executionHintsRightValid = false;
+ dto.executionHintsRight.toAron();
+ }
+ if (bo.executionHintsLeft)
+ {
+ dto.executionHintsLeftValid = true;
+ toAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ }
+ else
+ {
+ dto.executionHintsLeftValid = false;
+ dto.executionHintsLeft.toAron();
+
+ }
+ dto.graspPoseRight = fromIce(bo.graspPoseRight);
+ dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
+ dto.groupNr = bo.groupNr;
+ toAron(dto.objectType, bo.objectType);
+ dto.providerName = bo.providerName;
+ if (bo.reachabilityInfoRight)
+ {
+ dto.reachabilityInfoRightValid = true;
+ toAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ }
+ else
+ {
+ dto.reachabilityInfoRightValid = false;
+ dto.reachabilityInfoRight.toAron();
+ }
+ if (bo.reachabilityInfoLeft)
+ {
+ dto.reachabilityInfoLeftValid = true;
+ toAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ }
+ else
+ {
+ dto.reachabilityInfoLeftValid = false;
+ dto.reachabilityInfoLeft.toAron();
+ }
+ dto.robotPose = fromIce(bo.robotPose);
+ dto.sourceFrame = bo.sourceFrame;
+ if (bo.sourceInfo)
+ {
+ dto.sourceInfoValid = true;
+ toAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ dto.sourceInfoValid = false;
+ dto.sourceInfo.toAron();
+ }
+ dto.targetFrame = bo.targetFrame;
+ dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
+ dto.inwardsVectorLeft = fromIce(bo.inwardsVectorLeft);
+ dto.graspName = bo.graspName;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ armarx::grasping::GraspCandidateExecutionHints& bo)
+{
+ bo = GraspCandidateExecutionHints();
+ fromAron(dto.approach, bo.approach);
+ fromAron(dto.preshape, bo.preshape);
+ bo.graspTrajectoryName = dto.graspTrajectoryName;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ const armarx::grasping::GraspCandidateExecutionHints& bo)
+{
+ toAron(dto.approach, bo.approach);
+ toAron(dto.preshape, bo.preshape);
+ dto.graspTrajectoryName = bo.graspTrajectoryName;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
+ armarx::grasping::ApproachType& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ApproachType::AnyApproach:
+ bo = ApproachType::AnyApproach;
+ return;
+ case arondto::ApproachType::TopApproach:
+ bo = ApproachType::TopApproach;
+ return;
+ case arondto::ApproachType::SideApproach:
+ bo = ApproachType::SideApproach;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
+ const armarx::grasping::ApproachType& bo)
+{
+ switch (bo)
+ {
+ case ApproachType::AnyApproach:
+ dto.value = arondto::ApproachType::AnyApproach;
+ return;
+ case ApproachType::TopApproach:
+ dto.value = arondto::ApproachType::TopApproach;
+ return;
+ case ApproachType::SideApproach:
+ dto.value = arondto::ApproachType::SideApproach;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
+ armarx::grasping::ApertureType& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ApertureType::AnyAperture:
+ bo = ApertureType::AnyAperture;
+ return;
+ case arondto::ApertureType::OpenAperture:
+ bo = ApertureType::OpenAperture;
+ return;
+ case arondto::ApertureType::PreshapedAperture:
+ bo = ApertureType::PreshapedAperture;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
+ const armarx::grasping::ApertureType& bo)
+{
+ switch (bo)
+ {
+ case ApertureType::AnyAperture:
+ dto.value = arondto::ApertureType::AnyAperture;
+ return;
+ case ApertureType::OpenAperture:
+ dto.value = arondto::ApertureType::OpenAperture;
+ return;
+ case ApertureType::PreshapedAperture:
+ dto.value = arondto::ApertureType::PreshapedAperture;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e1ac4ca2f78637660c12250a0dfd4dd4b8250b5
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+
+namespace armarx
+{
+ namespace grasping
+ {
+ void fromAron(const arondto::ApproachType& dto, ApproachType& bo);
+ void toAron(arondto::ApproachType& dto, const ApproachType& bo);
+
+ void fromAron(const arondto::ApertureType& dto, ApertureType& bo);
+ void toAron(arondto::ApertureType& dto, const ApertureType& bo);
+
+ void fromAron(const arondto::BoundingBox& dto, BoundingBox& bo);
+ void toAron(arondto::BoundingBox& dto, const BoundingBox& bo);
+
+ void fromAron(const arondto::GraspCandidateSourceInfo& dto, GraspCandidateSourceInfo& bo);
+ void toAron(arondto::GraspCandidateSourceInfo& dto, const GraspCandidateSourceInfo& bo);
+
+ void fromAron(const arondto::GraspCandidateExecutionHints& dto, GraspCandidateExecutionHints& bo);
+ void toAron(arondto::GraspCandidateExecutionHints& dto, const GraspCandidateExecutionHints& bo);
+
+ void fromAron(const arondto::GraspCandidateReachabilityInfo& dto, GraspCandidateReachabilityInfo& bo);
+ void toAron(arondto::GraspCandidateReachabilityInfo& dto, const GraspCandidateReachabilityInfo& bo);
+
+ void fromAron(const arondto::GraspCandidate& dto, GraspCandidate& bo);
+ void toAron(arondto::GraspCandidate& dto, const GraspCandidate& bo);
+
+ void fromAron(const arondto::BimanualGraspCandidate& dto, BimanualGraspCandidate& bo);
+ void toAron(arondto::BimanualGraspCandidate& dto, const BimanualGraspCandidate& bo);
+ }
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
index 3082a9367101850c603aa710581e9bbc7e5c6032..628824cb3bfdbd750939721c1cba92f21967dc68 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
@@ -292,7 +292,7 @@ namespace armarx
gc->providerName = provider_name;
gc->side = side_name;
gc->approachVector = new armarx::Vector3(approach);
- gc->objectType = armarx::grasping::UnknownObject;
+ gc->objectType = armarx::objpose::UnknownObject;
gc->sourceFrame = "root";
gc->targetFrame = "root";
return gc;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index 510ff8933b6e045ec96e1020e6914f2b064636f2..743d8a9bc2904a707f42633567f9b59ef184c3af 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -67,6 +67,7 @@ namespace armarx::plugins
HeartbeatComponentPlugin::preOnConnectComponent()
{
robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
+ componentName = parent<Component>().getName();
}
void
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c524ecd93da7721ee95cc111beb518cafd848de5..cf8ce72dfd79c7148548940fc954b4ea10ad1fda 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -84,6 +84,7 @@ set(LIB_FILES
server/RemoteGuiAronDataVisitor.cpp
server/ltm/LongtermMemoryBase.cpp
+ server/ltm/disk/operations.cpp
server/ltm/disk/MemoryManager.cpp
server/ltm/mongodb/MemoryManager.cpp
server/ltm/mongodb/ConnectionManager.cpp
@@ -196,6 +197,7 @@ set(LIB_HEADERS
server/RemoteGuiAronDataVisitor.h
server/ltm/LongtermMemoryBase.h
+ server/ltm/disk/operations.h
server/ltm/disk/MemoryManager.h
server/ltm/mongodb/MemoryManager.h
server/ltm/mongodb/ConnectionManager.h
diff --git a/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h b/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
index 219a08e47a6c0cf6fd2cb8463c8e1923997c77af..ee004f1b819769df41615b2bfa0706e68772d5ca 100644
--- a/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
+++ b/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
@@ -12,6 +12,9 @@ namespace armarx::armem::client::query::detail
{
public:
+ virtual ~NameSelectorOps() = default;
+
+
virtual DerivedT& withName(const std::string& name) = 0;
virtual DerivedT& withNamesMatching(const std::string& regex) = 0;
diff --git a/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h b/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
index c6e153f0d7358b318c6966f2c9d5c9c2c35579a4..aa354b42ff993f2e669b9fada2441a788b938081 100644
--- a/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
+++ b/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
@@ -19,6 +19,7 @@ namespace armarx::armem::client::query::detail
using QueryT = _QueryT;
ChildSelectorOps() = delete;
+ virtual ~ChildSelectorOps() = default;
ChildSelectorOps(const armem::query::data::QueryTargets& p) :
_parentTargets(p)
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index a78db401d3f578d7be9f9c86542a37f72326aabc..e567b76b8d36d8cf41c00118a49d3e041d0607bc 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -44,7 +44,7 @@ namespace armarx::armem::client::util
virtual ~SimpleReaderBase() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ virtual void connect();
protected:
@@ -65,6 +65,8 @@ namespace armarx::armem::client::util
const armem::client::Reader& memoryReader() const;
+ MemoryNameSystem& memoryNameSystem;
+
private:
Properties props;
@@ -72,7 +74,6 @@ namespace armarx::armem::client::util
armem::client::Reader memoryReaderClient;
std::mutex memoryMutex;
- MemoryNameSystem& memoryNameSystem;
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index f4d0b530f43afcfa7f7aeba95389bdb8a5371918..4f63c6f3a5cb14ad61f4d4664ead485c78345938 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -247,11 +247,10 @@ namespace armarx::armem::base
}
- void append(const _Derived& m)
+ template <class OtherDerivedT>
+ void append(const OtherDerivedT& other)
{
- // ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
-
- m.forEachProviderSegment([this](const ProviderSegmentT & provSeg)
+ other.forEachProviderSegment([this](const auto& provSeg)
{
auto it = this->_container.find(provSeg.name());
if (it == this->_container.end())
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 6fa26a5e40ae0ba23ea7e281ec336590dea483cb..9b34e11bd3d53e896f38cd2c3301c2c35dcdffd9 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -538,20 +538,19 @@ namespace armarx::armem::base
}
- void append(const DerivedT& m)
+ template <class OtherDerivedT>
+ void append(const OtherDerivedT& other)
{
- // ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
-
- m.forEachSnapshot([this](const EntitySnapshotT & snapshot)
+ other.forEachSnapshot([this](const auto& snapshot)
{
auto it = this->_container.find(snapshot.time());
if (it == this->_container.end())
{
EntitySnapshotT copy { snapshot };
- copy.id() = this->id().withTimestamp(snapshot.time()); // update id (e.g. memory name) if necessary
+ copy.id() = this->id().withTimestamp(snapshot.time()); // update id (e.g. memory name) if necessary
this->_container.emplace(snapshot.time(), copy);
}
- // else: segment already exists
+ // else: snapshot already exists
// We assume that a snapshot does not change, so ignore
return true;
});
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index e1729bbcd90cc6062e93b8e62ed8db29e3bb4387..2d28474e232281c085cad9a88cd0dcd85fec70d0 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -281,11 +281,10 @@ namespace armarx::armem::base
* @brief Merge another memory into this one. Append all data
* @param m The other memory
*/
- void append(const _Derived& m)
+ template <class OtherDerivedT>
+ void append(const OtherDerivedT& other)
{
- // ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
-
- m.forEachCoreSegment([this](const CoreSegmentT & coreSeg)
+ other.forEachCoreSegment([this](const auto& coreSeg)
{
auto it = this->_container.find(coreSeg.name());
if (it == this->_container.end())
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 6a5497444c9e5959ac7d59300e1bc99e0fac2018..a2a9f2a8c8ae6b57ff33ce1ecf8d1d1b5710ff55 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -229,9 +229,11 @@ namespace armarx::armem::base
return ret;
}
- void append(const DerivedT& m)
+
+ template <class OtherDerivedT>
+ void append(const OtherDerivedT& other)
{
- m.forEachEntity([this](const EntityT & entity)
+ other.forEachEntity([this](const auto& entity)
{
auto it = this->_container.find(entity.name());
if (it == this->_container.end())
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 003ef9fb5496b92814ad68cd9904987498460c46..ac8be17b51fd8f67d90de93c77513d15b6ff8320 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -16,12 +16,17 @@ namespace armarx::armem::error
}
- InvalidArgument::InvalidArgument(const std::string& argument, const std::string& function, const std::string& message) :
+ InvalidArgument::InvalidArgument(const std::string& argument,
+ const std::string& function,
+ const std::string& message) :
ArMemError(makeMsg(argument, function, message))
{
}
- std::string InvalidArgument::makeMsg(const std::string& argument, const std::string& function, const std::string& message)
+ std::string
+ InvalidArgument::makeMsg(const std::string& argument,
+ const std::string& function,
+ const std::string& message)
{
std::stringstream ss;
ss << "Invalid value for argument '" << argument << "' in function " << function << "()";
@@ -38,24 +43,37 @@ namespace armarx::armem::error
ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
- const std::string& ownTerm, const std::string& containerName) :
+ const std::string& ownTerm,
+ const std::string& containerName) :
ArMemError(makeMsg(gottenName, ownTerm, containerName))
- {}
+ {
+ }
- std::string ContainerNameMismatch::makeMsg(const std::string& gottenName, const std::string& containerTerm, const std::string& containerName)
+ std::string
+ ContainerNameMismatch::makeMsg(const std::string& gottenName,
+ const std::string& containerTerm,
+ const std::string& containerName)
{
std::stringstream ss;
- ss << "Name '" << gottenName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
+ ss << "Name '" << gottenName << "' does not match name of " << containerTerm << " '"
+ << containerName << "'.";
return ss.str();
}
- ContainerEntryAlreadyExists::ContainerEntryAlreadyExists(const std::string& existingTerm, const std::string& existingName, const std::string& ownTerm, const std::string& ownName) :
+ ContainerEntryAlreadyExists::ContainerEntryAlreadyExists(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName) :
ArMemError(makeMsg(existingTerm, existingName, ownTerm, ownName))
{
}
- std::string ContainerEntryAlreadyExists::makeMsg(const std::string& existingTerm, const std::string& existingName, const std::string& ownTerm, const std::string& ownName)
+ std::string
+ ContainerEntryAlreadyExists::makeMsg(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName)
{
std::stringstream ss;
ss << simox::alg::capitalize_words(existingTerm) << " with name '" << existingName << "' "
@@ -64,16 +82,21 @@ namespace armarx::armem::error
}
- MissingEntry::MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& containerTerm, const std::string& containerName,
+ MissingEntry::MissingEntry(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& containerTerm,
+ const std::string& containerName,
size_t size) :
ArMemError(makeMsg(missingTerm, missingName, containerTerm, containerName, size))
{
}
- std::string MissingEntry::makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& containerTerm, const std::string& containerName,
- size_t size)
+ std::string
+ MissingEntry::makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& containerTerm,
+ const std::string& containerName,
+ size_t size)
{
std::stringstream ss;
ss << "No " << missingTerm << " with name '" << missingName << "' "
@@ -82,15 +105,19 @@ namespace armarx::armem::error
}
-
- MissingData::MissingData(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName) :
+ MissingData::MissingData(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName) :
ArMemError(makeMsg(missingTerm, missingName, ownTerm, ownName))
{
}
- std::string MissingData::makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName)
+ std::string
+ MissingData::makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName)
{
std::stringstream ss;
ss << "No " << missingTerm << " data at '" << missingName << "' "
@@ -104,7 +131,8 @@ namespace armarx::armem::error
{
}
- std::string ParseIntegerError::makeMsg(std::string string, std::string semanticName)
+ std::string
+ ParseIntegerError::makeMsg(std::string string, std::string semanticName)
{
std::stringstream ss;
ss << "Failed to parse " << semanticName << " '" << string << "' as integer.";
@@ -112,13 +140,13 @@ namespace armarx::armem::error
}
-
InvalidMemoryID::InvalidMemoryID(const MemoryID& id, const std::string& message) :
ArMemError(makeMsg(id, message))
{
}
- std::string InvalidMemoryID::makeMsg(const MemoryID& id, const std::string& message)
+ std::string
+ InvalidMemoryID::makeMsg(const MemoryID& id, const std::string& message)
{
std::stringstream ss;
ss << "Invalid memory ID " << id << ": " << message;
@@ -126,13 +154,14 @@ namespace armarx::armem::error
}
-
- EntityHistoryEmpty::EntityHistoryEmpty(const std::string& entityName, const std::string& message) :
+ EntityHistoryEmpty::EntityHistoryEmpty(const std::string& entityName,
+ const std::string& message) :
ArMemError(makeMsg(entityName, message))
{
}
- std::string EntityHistoryEmpty::makeMsg(const std::string& entityName, const std::string& message)
+ std::string
+ EntityHistoryEmpty::makeMsg(const std::string& entityName, const std::string& message)
{
std::stringstream ss;
ss << "History of entity '" << entityName << "' is empty";
@@ -148,78 +177,47 @@ namespace armarx::armem::error
}
-
UnknownQueryType::UnknownQueryType(const std::string& term, const std::string& typeName) :
ArMemError(makeMsg(term, typeName))
{
}
- std::string UnknownQueryType::makeMsg(const std::string& term, const std::string& typeName)
+ std::string
+ UnknownQueryType::makeMsg(const std::string& term, const std::string& typeName)
{
std::stringstream ss;
ss << "Unknown " << term << " query type '" << typeName << "'.";
return ss.str();
}
-
- PathNotARegularFile::PathNotARegularFile(const std::string& path, const std::string& message) :
- ArMemError(makeMsg(path, message))
+ QueryFailed::QueryFailed(const std::string& memory, const std::string& message) :
+ ArMemError(makeMsg(memory, message))
{
}
- std::string PathNotARegularFile::makeMsg(const std::string& path, const std::string& message)
+ std::string QueryFailed::makeMsg(const std::string& memory, const std::string& message)
{
std::stringstream ss;
- ss << "The path '" << path << "' is not a regular file. Expecting a regular file there.";
- if (message.size() > 0)
- {
- ss << " " << message;
- }
- else
- {
- ss << ".";
- }
+ ss << "Query from memory " << memory << " failed with message: " << message;
return ss.str();
}
- PathNotADirectory::PathNotADirectory(const std::string& path, const std::string& message) :
- ArMemError(makeMsg(path, message))
+ IOError::IOError(const std::string& path, const std::string& message) :
+ ArMemError(makeMsg(path, message)), path(path)
{
}
- std::string PathNotADirectory::makeMsg(const std::string& path, const std::string& message)
- {
- std::stringstream ss;
- ss << "The path '" << path << "' is not a directory. Expecting a directory there.";
- if (message.size() > 0)
- {
- ss << " " << message;
- }
- else
- {
- ss << ".";
- }
- return ss.str();
- }
- PathDoesNotExist::PathDoesNotExist(const std::string& path, const std::string& message) :
- ArMemError(makeMsg(path, message))
- {
- }
-
- std::string PathDoesNotExist::makeMsg(const std::string& path, const std::string& message)
+ std::string
+ IOError::makeMsg(const std::string& path, const std::string& message)
{
std::stringstream ss;
- ss << "The path '" << path << "' does not exist. Expecting a valid path.";
+ ss << "IOError on path \"" << path << "\".";
if (message.size() > 0)
{
- ss << " " << message;
- }
- else
- {
- ss << ".";
+ ss << "\n" << message;
}
return ss.str();
}
-}
+} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 219d489fad038da1098d724bddd9e2bc74b0a5d7..5678ee1a9a8e452309f2127fa41f519858ee233c 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -174,42 +174,34 @@ namespace armarx::armem::error
};
/**
- * @brief Indicates that a path does not point to a directory.
+ * @brief Indicates that a query resulted in an Error.
*/
- class PathNotADirectory : public ArMemError
+ class QueryFailed : public ArMemError
{
public:
- PathNotADirectory(const std::string& path, const std::string& message = "");
+ QueryFailed(const std::string& memory, const std::string& message = "");
- static std::string makeMsg(const std::string& path, const std::string& message = "");
+ static std::string makeMsg(const std::string& memory, const std::string& message = "");
};
+
/**
- * @brief Indicates that a path does not point to a regular file.
+ * @brief Indicates that something went wrong when accessing the filesystem.
*/
- class PathNotARegularFile : public ArMemError
+ class IOError : public ArMemError
{
public:
- PathNotARegularFile(const std::string& path, const std::string& message = "");
+ IOError(const std::string& path, const std::string& message = "");
static std::string makeMsg(const std::string& path, const std::string& message = "");
- };
- /**
- * @brief Indicates that a path does not exist.
- */
- class PathDoesNotExist : public ArMemError
- {
- public:
-
- PathDoesNotExist(const std::string& path, const std::string& message = "");
-
- static std::string makeMsg(const std::string& path, const std::string& message = "");
+ std::string path;
};
+
}
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index a80a189ff585c7610bebe0978468aec82ebe48b9..a0c606a990fd4635a5fa2cded1c0e5be8f7566a8 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -40,6 +40,14 @@ namespace armarx::armem::mns
}
}
}
+
+ dto::RegisterServerResult MemoryNameSystem::registerServer(const dto::RegisterServerInput& input)
+ {
+ const auto result = Registry::registerServer(input);
+ waitForServer_processOnce();
+
+ return result;
+ }
armarx::RemoteGui::Client::GridLayout MemoryNameSystem::RemoteGui_buildInfoGrid()
@@ -92,4 +100,3 @@ namespace armarx::armem::mns
}
}
-
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
index 51e4719f127ee576073e0c9b45cb0635241a185d..e0cf5b88753cb4a96caad2c004a853dcd4d11a34 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
@@ -31,6 +31,8 @@ namespace armarx::armem::mns
void waitForServer_processOnce();
+ dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input) override;
+
/// Builds a RemoteGui grid containing information about registered memories.
armarx::RemoteGui::Client::GridLayout RemoteGui_buildInfoGrid();
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
index bf9348d157ee990f6032c4cf115b9fd56c70f716..c1965bdac24fcdf943ad2b7bc945698358dd3f0c 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.h
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -35,7 +35,7 @@ namespace armarx::armem::mns
* Causes threads waiting in `waitForMemory()` to resume if the respective
* memory server was added.
*/
- dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input);
+ virtual dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input);
/**
* @brief Remove a server entry.
*/
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
index 2e8c64d64121eccc007cad7ba2d55c2e361b94e2..53e6f0169ab5ccb459ff730d67651836e78d5ec1 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
@@ -1,20 +1,23 @@
-// Header
#include "LongtermMemoryBase.h"
-// ArmarX
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
namespace armarx::armem::server::ltm
{
- void LongtermMemoryBase::setName(const std::string& name)
+ void
+ LongtermMemoryBase::setName(const std::string& name)
{
cache.name() = name;
lut.name() = name;
}
- armem::wm::Memory LongtermMemoryBase::getCacheAndLutNotConverted() const
+
+ armem::wm::Memory
+ LongtermMemoryBase::getCacheAndLutNotConverted() const
{
std::lock_guard l(cache_mutex);
std::lock_guard l2(lut_mutex);
@@ -32,23 +35,43 @@ namespace armarx::armem::server::ltm
return m;
}
- void LongtermMemoryBase::append(const armem::wm::Memory& m)
+
+ template <class ...Args>
+ void LongtermMemoryBase::_append(const armem::base::MemoryBase<Args...>& memory)
{
TIMING_START(LTM_Append);
- ARMARX_INFO << "Append memory with name '" << m.name() << "' into the LTM with name '" << cache.name() << "'";
+ ARMARX_INFO << "Append memory with name '" << memory.name() << "' into the LTM with name '"
+ << cache.name() << "'";
std::lock_guard l(cache_mutex);
- cache.append(m);
+ cache.append(memory);
encodeAndStore();
TIMING_END(LTM_Append);
}
- void LongtermMemoryBase::checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot)
+
+ void
+ LongtermMemoryBase::append(const armem::wm::Memory& memory)
+ {
+ this->_append(memory);
+ }
+
+
+ void
+ LongtermMemoryBase::append(const armem::server::wm::Memory& memory)
+ {
+ this->_append(memory);
+ }
+
+
+ void
+ LongtermMemoryBase::checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot)
{
// update map of latestSnapshots
- if (auto it = latestSnapshots.find(newSnapshot.id().getEntityID().str()); it != latestSnapshots.end())
+ if (auto it = latestSnapshots.find(newSnapshot.id().getEntityID().str());
+ it != latestSnapshots.end())
{
auto ptr = it->second;
if (ptr->id().timestamp > newSnapshot.id().timestamp)
@@ -64,7 +87,9 @@ namespace armarx::armem::server::ltm
}
}
- bool LongtermMemoryBase::containsCoreSegment(const MemoryID& coreSegmentID) const
+
+ bool
+ LongtermMemoryBase::containsCoreSegment(const MemoryID& coreSegmentID) const
{
//ARMARX_INFO << "Check if lut has core seg";
if (lut.hasCoreSegment(coreSegmentID.coreSegmentName))
@@ -75,7 +100,9 @@ namespace armarx::armem::server::ltm
return false;
}
- bool LongtermMemoryBase::containsProviderSegment(const MemoryID& providerSegmentID) const
+
+ bool
+ LongtermMemoryBase::containsProviderSegment(const MemoryID& providerSegmentID) const
{
//ARMARX_INFO << "Check if lut has prov seg";
if (lut.hasCoreSegment(providerSegmentID.coreSegmentName))
@@ -90,7 +117,9 @@ namespace armarx::armem::server::ltm
return false;
}
- bool LongtermMemoryBase::containsEntity(const MemoryID& entityID) const
+
+ bool
+ LongtermMemoryBase::containsEntity(const MemoryID& entityID) const
{
//ARMARX_INFO << "Check if lut has entity";
if (lut.hasCoreSegment(entityID.coreSegmentName))
@@ -109,7 +138,9 @@ namespace armarx::armem::server::ltm
return false;
}
- bool LongtermMemoryBase::containsSnapshot(const MemoryID& snapshotID) const
+
+ bool
+ LongtermMemoryBase::containsSnapshot(const MemoryID& snapshotID) const
{
//ARMARX_INFO << "Check if lut has snapshot";
if (lut.hasCoreSegment(snapshotID.coreSegmentName))
@@ -131,4 +162,5 @@ namespace armarx::armem::server::ltm
}
return false;
}
-}
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
index 097f951548cc777fa03cedbc5f425e867439cd48..463c4c37998bd95ce4ec81f2e92eb0c937e2dc7c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
@@ -1,11 +1,14 @@
#pragma once
-// STD / STL
-#include <optional>
+#include <map>
#include <mutex>
+#include <optional>
+#include <string>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-// Memory
-#include "../../core/wm/memory_definitions.h"
namespace armarx::armem::server::ltm
{
@@ -26,10 +29,10 @@ namespace armarx::armem::server::ltm
struct ReloadResult
{
-
};
- void append(const armem::wm::Memory&);
+ void append(const armem::wm::Memory& memory);
+ void append(const armem::server::wm::Memory& memory);
virtual void reload() = 0;
virtual void convert(armem::wm::Memory&) = 0;
@@ -61,5 +64,11 @@ namespace armarx::armem::server::ltm
/// A map from entityID to its latest snapshot stored. When adding a new snapshot we compare it to the last one stored.
std::map<std::string, const armem::wm::EntitySnapshot*> latestSnapshots;
+
+ private:
+
+ template <class ...Args>
+ void _append(const armem::base::MemoryBase<Args...>& memory);
+
};
-}
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
index 227d419119e3889006a3f0f30a4789e3b9e76dfe..e9eeffb05667a7e4feeb9c638abea41be02201cd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
@@ -1,18 +1,16 @@
#pragma once
-// STD / STL
+#include <filesystem>
#include <mutex>
#include <optional>
-#include <filesystem>
-// Base Class
-#include "../LongtermMemoryBase.h"
+#include <RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h>
+
namespace armarx::armem::server::ltm::disk
{
/// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
- class MemoryManager :
- public LongtermMemoryBase
+ class MemoryManager : public LongtermMemoryBase
{
using Base = LongtermMemoryBase;
@@ -23,14 +21,13 @@ namespace armarx::armem::server::ltm::disk
void convert(armem::wm::Memory&) override;
void encodeAndStore() override;
- void setBasePath(const std::filesystem::path& p)
+ void
+ setBasePath(const std::filesystem::path& p)
{
basePathToMemory = p;
}
protected:
-
-
private:
bool checkPath() const;
@@ -41,4 +38,5 @@ namespace armarx::armem::server::ltm::disk
static const constexpr char* TYPE_FILENAME = "type.aron.ltm.json";
static const constexpr char* DATA_FILENAME = "data.aron.ltm.json";
};
-}
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..83d2b379d85225e6e369e8711c04ef068e5016e0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp
@@ -0,0 +1,76 @@
+#include "operations.h"
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
+namespace armarx::armem::server::ltm
+{
+
+ armem::wm::Memory
+ disk::load(const std::filesystem::path& directory)
+ {
+ const std::string key = directory.filename();
+ armem::server::ltm::disk::MemoryManager manager;
+ manager.setName(key);
+ manager.setBasePath(directory);
+ manager.reload();
+
+ armem::client::QueryBuilder builder;
+ builder.all();
+ armem::client::QueryInput queryInput = builder.buildQueryInput();
+ queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
+
+ armem::server::query_proc::ltm::MemoryQueryProcessor processor;
+ armem::wm::Memory memory =
+ processor.process(queryInput.toIce(), manager.getCacheAndLutNotConverted());
+ manager.convert(memory);
+
+ return memory;
+ }
+
+
+ namespace detail
+ {
+ template <class MemoryT>
+ void
+ store(const std::filesystem::path& directory, const MemoryT& memory)
+ {
+ const std::string& name = memory.name();
+ if (std::filesystem::is_regular_file(directory / name))
+ {
+ std::stringstream ss;
+ ss << "Could not export memory '" << name << "' to " << directory << ": "
+ << "Cannot overwrite existing file.\n";
+ throw error::IOError(directory, ss.str());
+ }
+ else
+ {
+ std::filesystem::create_directories(directory / name);
+
+ armem::server::ltm::disk::MemoryManager manager;
+ manager.setName(name);
+ manager.setBasePath(directory / name);
+ manager.reload();
+ manager.append(memory);
+ }
+ }
+ } // namespace detail
+
+
+ void
+ disk::store(const std::filesystem::path& directory, const armem::wm::Memory& memory)
+ {
+ detail::store(directory, memory);
+ }
+
+
+ void
+ disk::store(const std::filesystem::path& directory, const armem::server::wm::Memory& memory)
+ {
+ detail::store(directory, memory);
+ }
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h
new file mode 100644
index 0000000000000000000000000000000000000000..b18575996f0ba38b0251844b5b9f18a2c98d0752
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <filesystem>
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::ltm::disk
+{
+
+ armem::wm::Memory load(const std::filesystem::path& directory);
+
+ void store(const std::filesystem::path& directory, const armem::wm::Memory& memory);
+ void store(const std::filesystem::path& directory, const armem::server::wm::Memory& memory);
+
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index e361438099e5e2a34b80d5f64f810a23fb3270d2..166256ba2e20bc72b560f481069b4fb9a927a825 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -1,6 +1,7 @@
#include "ControlWidget.h"
#include <RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/operations.h>
#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -148,18 +149,9 @@ namespace armarx::armem::gui::disk
{
if (dir.is_directory())
{
- const std::string key = dir.path().filename();
- armem::server::ltm::disk::MemoryManager manager;
- manager.setName(key);
- manager.setBasePath(path / key);
-
- manager.reload();
-
- armem::server::query_proc::ltm::MemoryQueryProcessor ltm_processor;
- armem::wm::Memory query_res = ltm_processor.process(queryIce, manager.getCacheAndLutNotConverted());
-
- manager.convert(query_res);
- memoryData[key] = std::move(query_res);
+ std::string memoryName = dir.path().filename();
+ armem::wm::Memory memory = armem::server::ltm::disk::load(dir.path());
+ memoryData[memoryName] = std::move(memory);
numLoaded++;
}
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 202c4d6c4cc5b94e9a912ee9a24a3387b8542906..c554afd52d27cd534b4015eb2c5114ddd8955181 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -2,15 +2,16 @@
#include <mutex>
#include <optional>
-#include <Eigen/src/Geometry/Transform.h>
-#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Geometry>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.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/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -32,7 +33,8 @@ namespace armarx::armem::articulated_object
{
}
- void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
@@ -49,7 +51,8 @@ namespace armarx::armem::articulated_object
def->optional(properties.providerName, prefix + "read.ProviderName");
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
@@ -65,14 +68,15 @@ namespace armarx::armem::articulated_object
}
armem::MemoryID id = armem::MemoryID();
- id.memoryName = properties.memoryName;
+ id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
}
- void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+ void
+ Reader::updateKnownObject(const armem::MemoryID& snapshotId)
{
// const std::string& nameWithDataset = snapshotId.providerSegmentName;
@@ -82,8 +86,9 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
+ void
+ Reader::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
@@ -106,9 +111,15 @@ namespace armarx::armem::articulated_object
// });
}
- std::optional<ArticulatedObject> Reader::get(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<ArticulatedObject>
+ Reader::get(const std::string& name, const armem::Time& timestamp)
{
+ const auto splits = simox::alg::split(name, "/");
+ ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
+
+ const std::string className = splits.at(0) + "/" + splits.at(1); // `DATASET/NAME`
+ const std::string instanceName = splits.at(2);
+
const auto description = queryDescription(name, timestamp);
if (not description)
@@ -117,36 +128,42 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- return get(*description, timestamp);
+ return get(*description, timestamp, instanceName);
}
- ArticulatedObject Reader::get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp)
+ ArticulatedObject
+ Reader::get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp,
+ const std::string& instanceName)
{
ArticulatedObject obj{.description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp};
+ .instance = instanceName,
+ .config = {}, // will be populated by `synchronize()`
+ .timestamp = timestamp};
synchronize(obj, timestamp);
return obj;
}
- void Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
+ void
+ Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
{
- auto state = queryState(obj.description, timestamp);
+ ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
+
+ auto state = queryState(obj.name(), timestamp);
if (not state) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ ARMARX_WARNING << "Could not synchronize object " << obj.instance;
return;
}
obj.config = std::move(*state);
}
- std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+ std::vector<robot::RobotDescription>
+ Reader::queryDescriptions(const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -171,19 +188,23 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- std::string Reader::getProviderName() const
+ std::string
+ Reader::getProviderName() const
{
return properties.providerName;
}
- void Reader::setProviderName(const std::string& providerName)
+ void
+ Reader::setProviderName(const std::string& providerName)
{
this->properties.providerName = providerName;
}
- std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<robot::RobotDescription>
+ Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
+ // FIXME: why is `name` unused?
+
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -208,8 +229,8 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ std::optional<robot::RobotState>
+ Reader::queryState(const std::string& instanceName, const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -220,7 +241,7 @@ namespace armarx::armem::articulated_object
qb
.coreSegments().withName(properties.coreInstanceSegmentName)
.providerSegments().all() // withName(properties.providerName) // agent
- .entities().withName(description.name)
+ .entities().withName(instanceName)
.snapshots().latest();
// clang-format on
@@ -237,7 +258,8 @@ namespace armarx::armem::articulated_object
}
- std::optional<robot::RobotState> convertToRobotState(const armem::wm::EntityInstance& instance)
+ std::optional<robot::RobotState>
+ convertToRobotState(const armem::wm::EntityInstance& instance)
{
armarx::armem::arondto::ObjectInstance aronObjectInstance;
try
@@ -253,12 +275,9 @@ namespace armarx::armem::articulated_object
objpose::ObjectPose objectPose;
objpose::fromAron(aronObjectInstance.pose, objectPose);
- robot::RobotState robotState
- {
- .timestamp = objectPose.timestamp,
- .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues
- };
+ robot::RobotState robotState{.timestamp = objectPose.timestamp,
+ .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
+ .jointMap = objectPose.objectJointValues};
return robotState;
}
@@ -272,20 +291,16 @@ namespace armarx::armem::articulated_object
// clang-format on
std::optional<wm::EntityInstance> instance;
- coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = i;
- });
-
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; });
+
if (instance.has_value())
{
return convertToRobotState(instance.value());
// return robot::convertRobotState(instance.value());
}
-
+
ARMARX_FATAL << "Failed to obtain robot state";
return std::nullopt;
-
}
@@ -297,10 +312,7 @@ namespace armarx::armem::articulated_object
// clang-format on
std::optional<wm::EntityInstance> instance;
- coreSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = i;
- });
+ coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; });
if (instance.has_value())
{
@@ -309,7 +321,6 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "No robot description";
return std::nullopt;
-
}
@@ -320,17 +331,19 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(properties.coreClassSegmentName);
std::vector<robot::RobotDescription> descriptions;
- coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
- {
- if (not entity.empty())
+ coreSegment.forEachEntity(
+ [&descriptions](const wm::Entity& entity)
{
- const auto robotDescription = convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
- if (robotDescription)
+ if (not entity.empty())
{
- descriptions.push_back(*robotDescription);
+ const auto robotDescription =
+ convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
+ if (robotDescription)
+ {
+ descriptions.push_back(*robotDescription);
+ }
}
- }
- });
+ });
return descriptions;
}
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 e0dca8f3ebd68e682f7bff2bcc759c6bb2673759..3108b17f9b7054a61290a4647d5d3965f17d2fbd 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -50,9 +50,9 @@ namespace armarx::armem::articulated_object
std::optional<ArticulatedObject> get(const std::string& name,
const armem::Time& timestamp) override;
ArticulatedObject get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp, const std::string& instanceName) override;
- std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
+ std::optional<robot::RobotState> queryState(const std::string &instanceName,
const armem::Time& timestamp);
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
const armem::Time& timestamp);
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 dcd44d6739bfaa3e211a79721a330cc10b4751fc..05506152012d6527110801adb5bfe5b015a42d1b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -104,6 +104,8 @@ namespace armarx::armem::articulated_object
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
{
+ ARMARX_TRACE;
+
const auto objectId = knownObjects.find(obj.description.name);
// check if exists
@@ -187,7 +189,10 @@ namespace armarx::armem::articulated_object
const auto& timestamp = obj.timestamp;
- ARMARX_DEBUG << "Storing articulated object instance '" << obj.description.name << "' (provider '" << properties.providerName << "')";
+ 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 << "')";
const auto providerId = armem::MemoryID()
.withMemoryName(properties.memoryName)
@@ -195,7 +200,7 @@ namespace armarx::armem::articulated_object
.withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
- update.entityID = providerId.withEntityName(obj.description.name);
+ update.entityID = providerId.withEntityName(entityName);
// .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
// arondto::Robot aronArticulatedObject;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index ced0a76fd07fe4575ea4762ea062a804431fff2a..a3061a553cb545605ce37febf9632d6c73d50cda 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -13,7 +13,7 @@ namespace armarx::armem::articulated_object
virtual void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
- virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) = 0;
+ virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName) = 0;
virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) = 0;
};
@@ -26,4 +26,4 @@ namespace armarx::armem::articulated_object
virtual bool store(const ArticulatedObject& obj) = 0;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 9668e307ba7c7ff3ed1143087bf4aee1adf2608c..1383ae80470a5e8831326b62c2f34fc39f5a1396 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -937,7 +937,7 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPose& pose = objectPoses.emplace_back();
pose.providerName = sceneName;
- pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.objectType = objpose::ObjectType::KnownObject;
pose.isStatic = true; // Objects loaded from prior knowledge are considerd static to exclude them from decay.
pose.objectID = classID.withInstanceName(
object.instanceName.empty()
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index da72cc16f995f239c207dfb38dcaa6cf533fb74b..426511a9a8a59dd5cba9545f08f8479c0104d96c 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,5 +1,6 @@
#include "aron_conversions.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
@@ -107,5 +108,19 @@ namespace armarx::armem
dto.objectPoseGlobal = bo.globalPose.matrix();
dto.objectJointValues = bo.jointMap;
}
+
+ void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 9c124d758a9189b1ec709cca9808b430bbc63a94..e6b462fa866d0c011cb0d899f08ad4e804e3f210 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,6 +8,7 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -26,6 +27,8 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index df39df99e945e4a1ef38c38db6f9428404a35bed..ad7454d6fb4f74b49b6909a3a6d362c302203fc9 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -8,6 +8,7 @@
#include <IceUtil/Time.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include <ArmarXCore/core/PackagePath.h>
@@ -22,6 +23,18 @@ namespace armarx::armem::robot
PackagePath xml{"", std::filesystem::path("")};
};
+
+ struct Twist
+ {
+ Eigen::Vector3f linear;
+ Eigen::Vector3f angular;
+ };
+
+ struct PlatformState
+ {
+ Twist twist;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
@@ -41,6 +54,14 @@ namespace armarx::armem::robot
RobotState config;
IceUtil::Time timestamp;
+
+ std::string name() const
+ {
+ ARMARX_CHECK_NOT_EMPTY(description.name) << "The robot name must be set!";
+ ARMARX_CHECK_NOT_EMPTY(instance) << "The robot instance name must be provided!";
+
+ return description.name + "/" + instance;
+ }
};
using Robots = std::vector<Robot>;
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 57ecaf8de87598880229489908edb300431b96fa..4da91df52e77ecd83fe4c061d0a969bce8bae32f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -4,6 +4,7 @@
#include <optional>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
@@ -23,6 +24,7 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
+
RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
@@ -113,11 +115,11 @@ namespace armarx::armem::robot_state
.snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
// clang-format on
- ARMARX_INFO << "Lookup query in reader";
+ ARMARX_DEBUG << "Lookup query in reader";
if (not memoryReader)
{
- ARMARX_WARNING << "Memory reader is null";
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -125,7 +127,7 @@ namespace armarx::armem::robot_state
{
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_INFO << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
if (not qResult.success) /* c++20 [[unlikely]] */
{
@@ -149,14 +151,14 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state";
+ ARMARX_WARNING << "Failed to query joint state for robot " << description.name;
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
- ARMARX_WARNING << "Failed to query global pose";
+ ARMARX_WARNING << "Failed to query global pose for robot " << description.name;
return std::nullopt;
}
@@ -197,6 +199,40 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotPlatformState(qResult.memory, description.name);
+ }
+
std::optional<robot::RobotState::Pose>
RobotReader::queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -307,6 +343,34 @@ namespace armarx::armem::robot_state
return jointMap;
}
+ std::optional<robot::PlatformState>
+ RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::optional<robot::PlatformState> platformState;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity([&platformState](const wm::Entity & entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ platformState = robot::PlatformState(); // initialize optional
+ robot::fromAron(proprioception->platform, platformState.value());
+
+ });
+
+ return platformState;
+ }
+
+
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
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 b3f71e59ac2b33c19ff97fd0c238f5540503b6ee..e24bc65fa8a9ca14213bfe79007fc8519f02d176 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -72,14 +72,24 @@ namespace armarx::armem::robot_state
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ std::optional<robot::PlatformState>
+ queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
+
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::PlatformState>
+ getRobotPlatformState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+
struct Properties
{
std::string memoryName = "RobotState";
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 10e13611f3fd8608a5d0b5027fd92b960c3a7754..1c0152a463d5c38abb3b1c81f1014e1f74ef4d77 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -42,7 +42,7 @@ namespace armarx::armem::robot_state
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed!";
+ ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / " << robot.getType() << "`!";
return false;
}
@@ -67,7 +67,9 @@ namespace armarx::armem::robot_state
const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
- return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+ auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+ robot->setName(name);
+ return robot;
}
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index 1767c9cebd878b1d15cad6f8043a2bd7cf932814..cc81788baa40cf3089de08f3699ac8e9cf026c36 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,6 +19,7 @@ armarx_add_library(
./client/laser_scans/Writer.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
+ ./OccupancyGridHelper.h
SOURCES
./aron_conversions.cpp
./client/laser_scans/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
index 17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3..4b936b6e5bbc7de8141ea380efdac63f3e8d14ad 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -25,7 +25,7 @@ namespace armarx
public:
using Params = detail::OccupancyGridHelperParams;
- OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+ OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params = Params());
using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index 717a2ec98e5283515a5454bfa31eb7c00636b01c..fe59e94773e8f1a82396705e43be5e51c57c08b0 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -48,6 +48,7 @@
namespace armarx::armem::vision::occupancy_grid::client
{
+ Reader::~Reader() = default;
armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
{
@@ -55,7 +56,7 @@ namespace armarx::armem::vision::occupancy_grid::client
// clang-format off
qb
- .coreSegments().withName(properties().memoryName)
+ .coreSegments().withName(properties().coreSegmentName)
.providerSegments().withName(query.providerName)
.entities().all()
.snapshots().beforeOrAtTime(query.timestamp);
@@ -63,6 +64,20 @@ namespace armarx::armem::vision::occupancy_grid::client
return qb;
}
+
+ std::string Reader::propertyPrefix() const
+ {
+ return "mem.vision.occupancy_grid.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties Reader::defaultProperties() const
+ {
+ return
+ {
+ .memoryName = "Vision",
+ .coreSegmentName = "OccupancyGrid"
+ };
+ }
OccupancyGrid asOccupancyGrid(const wm::ProviderSegment& providerSegment)
{
@@ -116,9 +131,19 @@ namespace armarx::armem::vision::occupancy_grid::client
.errorMessage = qResult.errorMessage};
}
- // now create result from memory
- const wm::ProviderSegment& providerSegment = qResult.memory.getCoreSegment(properties().memoryName)
- .getProviderSegment(query.providerName);
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if(not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_WARNING << "Provider segment `" << query.providerName << "` does not exist (yet).";
+ return
+ {
+ .occupancyGrid = std::nullopt,
+ .status = Result::Status::NoData
+ };
+ }
+
+ const wm::ProviderSegment& providerSegment = coreSegment.getProviderSegment(query.providerName);
if (providerSegment.empty())
{
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
index ce5883afb6eeb72c7ebb9337d0b4980eee7e462e..3275d888db9c8841e1dd7a8baf45582d27ab03ed 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -10,12 +10,12 @@ namespace armarx::armem::vision::occupancy_grid::client
bool Writer::store(const OccupancyGrid& grid,
const std::string& frame,
- const std::string& agentName,
+ const std::string& providerName,
const std::int64_t& timestamp)
{
std::lock_guard g{memoryWriterMutex()};
- const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+ const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName);
if (not result.success)
{
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
index b2a900321224d2c4617cab541c7b1cf77f41d1f1..df5e02ceaa5b2034e0028e7625c616924af8a2bf 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -49,7 +49,7 @@ namespace armarx::armem::vision::occupancy_grid::client
bool store(const OccupancyGrid& grid,
const std::string& frame,
- const std::string& agentName,
+ const std::string& providerName,
const std::int64_t& timestamp);
protected:
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85..00fb545a104afb19c38c9d7b01fd864ed6ba1fe6 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -45,7 +45,7 @@ namespace armarx::armem
// template<typename _ValueT = float>
struct OccupancyGrid
{
- float resolution;
+ float resolution; // [mm]
std::string frame;
Eigen::Affine3f pose;
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 4a5a02e56edc8fae399b2a216b4ee77a6f4536d8..810693b3af482ad0863e6dffd59adc0cf7ce68fe 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -150,11 +150,11 @@ namespace armarx::aron::converter
static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
{
const auto dims = nav.getDimensions();
+ ARMARX_CHECK_EQUAL(dims.size(), 2);
using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
- ArrayT ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ ArrayT ret = Eigen::Map<ArrayT>(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
return ret;
}
diff --git a/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
index 8f5c8006f3b16d7343a2f74b1c6b6562de426165..d044d9dfedd3933df0293e5e0b04cf3dea09e162 100644
--- a/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
@@ -3,8 +3,6 @@ set(LIB_NAME aronjsonconverter)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(IVT COMPONENTS ivt ivtopencv QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
set(LIBS
aron
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
index 790bb1e62538da3cdd0c25908118514f9be3cddd..30f5e54450020945bf1caec30e8be1c153fb17d9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
@@ -47,6 +47,7 @@ namespace armarx::aron::cppserializer
public:
AronCppClass() = default;
+ virtual ~AronCppClass() = default;
/// Reset all member values of this class to default (as stated in the XML). This may mean that maybe types are null or false and images may be created as headers_only
virtual void resetHard() = 0;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index 0db5f2d68e1de29da18c9327a5b25ee5bd834bc5..5f3c8043a69376538f41f8a55313b53f0c331f80 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -59,7 +59,7 @@ namespace armarx::aron::cppserializer::serializer
for (const auto& [key, value] : typenavigator->getAcceptedValueMap())
{
std::string enumKeyWithNamespace = std::string(IMPL_ENUM) + "::" + key;
- fields.push_back(std::make_shared<CppField>("const static " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
+ fields.push_back(std::make_shared<CppField>("static constexpr " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
enum_to_name << "\t\t{" << enumKeyWithNamespace << ", \"" << key << "\"}," << std::endl;
name_to_enum << "\t\t{\"" << key << "\", " << enumKeyWithNamespace << "}," << std::endl;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
index bd4a0f8128b19bbe28b388e3fa97faddf8f743b1..4a73767f0f800f03be6c1d77759ba17993cac1a3 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
@@ -66,6 +66,6 @@ namespace armarx::aron::cppserializer::serializer
private:
// Members
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- static constexpr const char* IMPL_ENUM = "__ImplEnum";
+ static constexpr const char* IMPL_ENUM = "ImplEnum";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
index 754d1566b0281cec57568cadee27d2177f75affb..0cd870ab77edc1a72f6cb5c90d8b820bde30b05e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
@@ -47,6 +47,7 @@ namespace armarx::aron
class Navigator
{
public:
+
// constructors
Navigator() = default;
Navigator(const Descriptor& descriptor, const Path& path) :
@@ -54,6 +55,7 @@ namespace armarx::aron
path(path)
{
}
+ virtual ~Navigator() = default;
// public member functions
Descriptor getDescriptor() const
@@ -76,7 +78,6 @@ namespace armarx::aron
virtual std::string getName() const = 0;
protected:
- virtual ~Navigator() = default;
// members
const Descriptor descriptor;
diff --git a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
index 4ff0724c95ca3c0cbc31dd051b6ba3afdff37fe9..b9c54acda1e8d0e1b70477427a2d8c848bbedb15 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
@@ -384,3 +384,11 @@ BOOST_AUTO_TEST_CASE(test_Optional)
OptionalTest pc2;
runTestWithInstances<OptionalTest>(pc, pc2);
}
+
+
+BOOST_AUTO_TEST_CASE(test_Enum)
+{
+ BOOST_TEST_MESSAGE("Running Optional test");
+ TheIntEnum value = TheIntEnum::INT_ENUM_VALUE_2;
+ BOOST_CHECK_EQUAL(value.toString(), "INT_ENUM_VALUE_2");
+}