diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index fac11b677edbf7fc26cd2538e453981601e94f13..9d9342ab21b51d11fd188a70c5e4cf6066afff24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -43,10 +43,18 @@
<Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
humanName="Platform Unit (obstacle avoiding)"
typeName="PlatformUnitInterfacePrx"
- memberName="obstacleAvoidingPlatformUnit"
- getterName="getObstacleAvoidingPlatformUnit"
- propertyName="ObstacleAvoidingPlatformUnitName"
+ memberName="platformUnit1"
+ getterName="getPlatformUnit1"
+ propertyName="PlatformUnitName1"
propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+ propertyIsOptional="true" />
+ <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+ humanName="Platform Unit (obstacle aware)"
+ typeName="PlatformUnitInterfacePrx"
+ memberName="platformUnit2"
+ getterName="getPlatformUnit2"
+ propertyName="PlatformUnitName2"
+ propertyDefaultValue="ObstacleAwarePlatformUnit"
propertyIsOptional="true" />
<Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
humanName="Platform Unit Observer"
diff --git a/etc/mongocxx/install_mongocxx.sh b/etc/mongocxx/install_mongocxx.sh
index feb912d3b34eda393e95c7c4f9a6cb207961713a..2578dd3ae3a35b331782b2ba75e9b9bb09fa4ba1 100755
--- a/etc/mongocxx/install_mongocxx.sh
+++ b/etc/mongocxx/install_mongocxx.sh
@@ -18,6 +18,7 @@ pwd="$(pwd)"
# Then you have to update the symlinks (not necessary on the lab pcs)
# Update the symlinks because the default mongoc bson cmake configs are shitty
+#sudo mkdir -p /usr/lib/include/
#sudo ln -s /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
#sudo ln -s /usr/include/libbson-1.0/ /usr/lib/include/libbson-1.0
#sudo ln -s /usr/include/libmongoc-1.0/ /usr/lib/include/libmongoc-1.0
diff --git a/scenarios/ArMemCore/ArMemCore.scx b/scenarios/ArMemCore/ArMemCore.scx
new file mode 100644
index 0000000000000000000000000000000000000000..421e3a1277c89e919677097f3841ae90061d8e25
--- /dev/null
+++ b/scenarios/ArMemCore/ArMemCore.scx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemCore" creation="2021-06-21.11:36:43" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemCore/config/DebugObserver.cfg b/scenarios/ArMemCore/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArMemCore/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/ArMemCore/config/MemoryNameSystem.cfg b/scenarios/ArMemCore/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArMemCore/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/ArMemCore/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemCore/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/ArMemCore/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/ArMemCore/config/global.cfg b/scenarios/ArMemCore/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7a27480632a19b090c2a5d877135779187e46554
--- /dev/null
+++ b/scenarios/ArMemCore/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArMemCore
+# ==================================================================
+
diff --git a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index 10325be750b47ccdfa1827265cce4d881d58d2ed..43b1e45191b3eb1df8d39ffb36d8990ec70e3c0e 100644
--- a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -118,12 +118,12 @@ ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemoryClient.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ExampleMemoryClient.mem.MemoryName: Name of the memory to use.
+# ArmarX.ExampleMemoryClient.mem.UsedMemoryName: Name of the memory to use.
# Attributes:
# - Default: Example
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemoryClient.mem.MemoryName = Example
+# ArmarX.ExampleMemoryClient.mem.UsedMemoryName = Example
# ArmarX.ExampleMemoryClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
index 45075e6114eae82fe89f50317c0bced824c96b46..76fbf317102eb56ab01bfd0037544c69f85c6c27 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
@@ -135,6 +135,15 @@
# ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, KIT/YellowSaltCylinder
+# ArmarX.ObjectPoseProviderExample.SingleShot: If true, publishes only one snapshot.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseProviderExample.SingleShot = false
+
+
# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index 2b27537f1a3c0cd903b360b271ad7b02b94459ef..b0b29f25911e2899083979af9b94987dd7b70bc8 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -169,7 +169,7 @@ namespace armarx
- viz::data::LayerUpdates armarx::ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
+ viz::data::LayerUpdates ArVizStorage::pullUpdatesSince(Ice::Long revision, const Ice::Current&)
{
viz::data::LayerUpdates result;
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
index 0a88d089382eebb915104c24ee41339a18dd2f29..cf5e9899e943dd4076c2dd21ca9fe982b3c1a9bc 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -1,27 +1,21 @@
#include "Path.h"
+#include <iterator>
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+
namespace armarx::viz
{
Path& Path::clear()
{
data_->points.clear();
- return *this;
- }
-
- Path& Path::lineColor(Color color)
- {
- data_->lineColor = color;
return *this;
}
- Path& Path::lineColorGlasbeyLUT(std::size_t id, int alpha)
- {
- return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
- }
-
- Path& Path::lineWidth(float w)
+ Path& Path::width(float w)
{
data_->lineWidth = w;
@@ -33,17 +27,21 @@ namespace armarx::viz
auto& points = data_->points;
points.clear();
points.reserve(ps.size());
- for (auto& p : ps)
+
+ std::transform(ps.begin(),
+ ps.end(),
+ std::back_inserter(points),
+ [](const auto & e)
{
- points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
- }
+ return ToBasicVectorType(e);
+ });
return *this;
}
Path& Path::addPoint(Eigen::Vector3f p)
{
- data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ data_->points.emplace_back(ToBasicVectorType(p));
return *this;
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
index 50eec6abe454bbf56bc45053c4d35f5e41ca5bc6..7cfbecbd973a194cef0e0e350a5c7c3c1aedbc13 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -35,17 +35,7 @@ namespace armarx::viz
Path& clear();
- Path& lineColor(Color color);
-
- template<class...Ts>
- Path& lineColor(Ts&& ...ts)
- {
- return lineColor({std::forward<Ts>(ts)...});
- }
-
- Path& lineColorGlasbeyLUT(std::size_t id, int alpha = 255);
-
- Path& lineWidth(float w);
+ Path& width(float w);
Path& points(std::vector<Eigen::Vector3f> const& ps);
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
index 3018a3af51e25222b97c0c60ed06fea65a8c5822..a0190abd75a8438a2371c728a580837d68c3eecc 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -1,9 +1,12 @@
#include "VisualizationPath.h"
+#include <algorithm>
+
#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoPointSet.h>
namespace armarx::viz::coin
{
@@ -27,19 +30,36 @@ namespace armarx::viz::coin
node->addChild(coordinate3);
node->addChild(lineSep);
+
+ // points (use the following, if the points should be drawn in a different color)
+
+ // pclMat = new SoMaterial;
+
+ // SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+ // pclMatBind->value = SoMaterialBinding::PER_PART;
+
+ pclCoords = new SoCoordinate3;
+ pclStyle = new SoDrawStyle;
+
+ // node->addChild(pclMat);
+ // node->addChild(pclMatBind);
+ node->addChild(pclCoords);
+ node->addChild(pclStyle);
+ node->addChild(new SoPointSet);
}
bool VisualizationPath::update(ElementType const& element)
{
// set position
- coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+ coordinate3->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
// set color
- const auto lineColor = element.lineColor;
+ const auto lineColor = element.color;
constexpr float toUnit = 1.0F / 255.0F;
- const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
lineMaterial->diffuseColor.setValue(color);
@@ -58,6 +78,20 @@ namespace armarx::viz::coin
const int pointSize = static_cast<int>(element.points.size());
lineSet->numVertices.set1Value(0, pointSize);
+ // points at each node
+ // const std::vector<SbVec3f> colorData(element.points.size(), color);
+
+ // pclMat->diffuseColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->ambientColor.setValuesPointer(colorData.size(),
+ // reinterpret_cast<const float*>(colorData.data()));
+ // pclMat->transparency = transparency;
+
+ pclCoords->point.setValuesPointer(element.points.size(),
+ reinterpret_cast<const float*>(element.points.data()));
+
+ pclStyle->pointSize = element.lineWidth + 5;
+
return true;
}
} // namespace armarx::viz::coin
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
index e1e446bbe66bc49a6d0c4bdf1999c7f0615f06ec..487ee9df58baeb6b91e91de6fdb3be873511a0af 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -39,9 +39,17 @@ namespace armarx::viz::coin
bool update(ElementType const& element);
+ /// lines
SoCoordinate3* coordinate3;
SoDrawStyle* lineStyle;
SoLineSet* lineSet;
SoMaterial* lineMaterial;
+
+
+ /// points
+ // SoMaterial* pclMat;
+ SoCoordinate3* pclCoords;
+ SoDrawStyle* pclStyle;
+
};
} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index d1e48cdea6aa941033225c291e50a3da312f9475..7009a9267a2828a4cd6ca5cc2238d9e153a7062f 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -29,6 +29,8 @@
#include <IceUtil/Time.h>
+#include <SimoxUtility/math/pose/pose.h>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
@@ -50,8 +52,10 @@
namespace armarx::articulated_object
{
ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
- articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(memoryNameSystem)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem))
+ {
+ }
armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
{
@@ -62,6 +66,7 @@ namespace armarx::articulated_object
defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
+ // Reader will override some properties of writer.
articulatedObjectWriter->registerPropertyDefinitions(defs);
articulatedObjectReader->registerPropertyDefinitions(defs);
@@ -73,7 +78,11 @@ namespace armarx::articulated_object
return "ArticulatedObjectLocalizerExample";
}
- void ArticulatedObjectLocalizerExample::onInitComponent() {}
+ void ArticulatedObjectLocalizerExample::onInitComponent()
+ {
+ // Reader overwrote property registered property of articulatedObjectWriter.
+ articulatedObjectWriter->setProviderName(articulatedObjectReader->getProviderName());
+ }
void ArticulatedObjectLocalizerExample::onConnectComponent()
{
@@ -83,7 +92,7 @@ namespace armarx::articulated_object
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::now();
- task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency);
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
task->start();
}
@@ -152,7 +161,7 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
- const float t = float((now - start).toSecondsDouble());
+ const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
@@ -163,9 +172,11 @@ namespace armarx::articulated_object
{"drawer_joint", 350 * k}
};
+ dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
dishwasher->setJointValues(jointValues);
- armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
+ armarx::armem::articulated_object::ArticulatedObject armemDishwasher =
+ convert(*dishwasher, IceUtil::Time::now());
articulatedObjectWriter->store(armemDishwasher);
}
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e712e30ccb2978fdc7caa0210b919773b7dce46b..41fc3e85f7a6a7250c47fe58c0730973a16480be 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -22,6 +22,8 @@
#include "DynamicObstacleManager.h"
+#include <VirtualRobot/MathTools.h>
+
// STD/STL
#include <string>
#include <vector>
@@ -118,6 +120,7 @@ namespace armarx
void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
{
+ TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
const float length = difference_vec.norm();
const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
@@ -136,10 +139,10 @@ namespace armarx
return;
}
-
{
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ TIMING_START(add_decayable_line_segment_mutex);
// First check own obstacles
for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
{
@@ -155,13 +158,15 @@ namespace armarx
managed_obstacle->line_matches.push_back(line_start);
managed_obstacle->line_matches.push_back(line_end);
managed_obstacle->m_last_matched = IceUtil::Time::now();
+ TIMING_END(add_decayable_line_segment_mutex);
return;
}
}
+ TIMING_END(add_decayable_line_segment_mutex);
}
// Second check the obstacles from the obstacle avoidance
- for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
+ /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
{
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
{published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
@@ -172,7 +177,7 @@ namespace armarx
ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
return;
}
- }
+ }*/
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
@@ -198,6 +203,15 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
m_managed_obstacles.push_back(new_managed_obstacle);
}
+ TIMING_END(add_decayable_line_segment);
+ }
+
+ void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ {
+ for (const auto& line : lines)
+ {
+ add_decayable_line_segment(line.lineStart, line.lineEnd, c);
+ }
}
@@ -218,7 +232,7 @@ namespace armarx
std::lock_guard l(m_managed_obstacles_mutex);
ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
- for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
{
@@ -240,6 +254,36 @@ namespace armarx
}
+ float
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ {
+ std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+ const float sample_step = 5; // in [mm], sample step size towards goal.
+ const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+ float current_distance = safetyRadius;
+
+ while (current_distance < distance_to_goal)
+ {
+ for (const auto man_obstacle : m_managed_obstacles)
+ {
+ Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+ Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ {
+ return current_distance - safetyRadius;
+ }
+ }
+
+ current_distance += sample_step;
+ }
+
+ return std::numeric_limits<float>::infinity();
+ }
+
+
void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
@@ -283,7 +327,6 @@ namespace armarx
}
}
-
// Update obstacle avoidance
int checked_obstacles = 0;
bool updated = false;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 1629b7efe3102dccf85521ac0fac6379e09225cc..5f2d8065a307706ea16789930c12f5165d6044eb 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -69,10 +69,12 @@ namespace armarx
void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index fe21cc4e46558b3a70102592ee77ef56715421f8..f718a45434ec538fbd5d2541fb9413568f3e79f0 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -94,10 +94,25 @@ namespace armarx
float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
- const unsigned int SAMPLES = 40;
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
const float distance = difference_vec.norm();
+ const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
+
+ const Vector2f difference_start_origin = e_origin - line_seg_start;
+ const Vector2f difference_end_origin = e_origin - line_seg_end;
+
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+ if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+
const float step_size = distance / SAMPLES;
const Eigen::Vector2f dir = difference_vec_normed * step_size;
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
index 92bc44f64073b1a26ceb15782207265f7bbdcad0..7becda0cb17dca65fb607f7c518a2d2a12302078 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/KITProstheticHandUnit/CMakeLists.txt
@@ -4,6 +4,7 @@ find_package(Qt5 COMPONENTS Core Bluetooth QUIET)
armarx_build_if(Qt5_FOUND "Qt5 Core or Bluetooth not available")
set(COMPONENT_LIBS
+ RemoteGui
RobotAPIUnits
KITProstheticHandDriver
)
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index c0ef77b2f4b728d245c446b9782336843157d9e6..a87d465a301b423defc89260a52cb15164bcd37d 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -42,6 +42,8 @@ namespace armarx
defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
.map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
+ defs->optional(singleShot, "SingleShot", "If true, publishes only one snapshot.");
+
return defs;
}
@@ -172,6 +174,11 @@ namespace armarx
ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
objectPoseTopic->reportObjectPoses(getName(), poses);
+ if (singleShot)
+ {
+ break;
+ }
+
cycle.waitForCycleDuration();
}
}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index 7ed814eb06c219f3280f1d5d4e9473ef56344fb0..8de0dac4589f9ab3333becd9a464a0ef236231f5 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -102,6 +102,8 @@ namespace armarx
armarx::ObjectFinder objectFinder;
std::vector<std::string> initialObjectIDs = { "KIT/Amicelli", "KIT/YellowSaltCylinder" };
+ bool singleShot = false;
+
armarx::SimpleRunningTask<>::pointer_type poseEstimationTask;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 48be5ead78768c516c63004c0fd0bd24494b5f8f..8ae42e00bd5385886696dd6ead35feb927665947 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -32,6 +32,8 @@
#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/error.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -39,22 +41,20 @@
namespace armarx
{
- ExampleMemoryClientPropertyDefinitions::ExampleMemoryClientPropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr ExampleMemoryClient::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ExampleMemoryClientPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
- defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
+ defs->optional(p.usedMemoryName, "mem.UsedMemoryName", "Name of the memory to use.");
+ defs->optional(p.commitFrequency, "ex.CommitFrequency", "Frequency in which example data is commited.");
return defs;
}
+
std::string ExampleMemoryClient::getDefaultName() const
{
return "ExampleMemoryClient";
@@ -72,27 +72,32 @@ namespace armarx
RemoteGui_startRunningTask();
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Waiting for memory '" << memoryName << "' ...";
- auto result = useMemory(memoryName);
- if (!result.success)
+ ARMARX_IMPORTANT << "Waiting for memory '" << p.usedMemoryName << "' ...";
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(p.usedMemoryName);
+ memoryWriter = memoryNameSystem.useWriter(p.usedMemoryName);
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
}
// Add a provider segment to commit to.
- providerID = addProviderSegment();
+ exampleProviderID = addProviderSegment();
// Construct the entity ID.
- entityID = providerID.withEntityName("example_entity");
+ exampleEntityID = exampleProviderID.withEntityName("example_entity");
// Subscribe to example_entity updates
// Using a lambda:
- memoryReader.subscribe(entityID, [&](const armem::MemoryID & entityID, const std::vector<armem::MemoryID>& snapshotIDs)
+ memoryNameSystem.subscribe(exampleEntityID, [&](const armem::MemoryID & exampleEntityID, const std::vector<armem::MemoryID>& snapshotIDs)
{
- ARMARX_INFO << "Entity " << entityID << " was updated by " << snapshotIDs.size() << " snapshots.";
+ ARMARX_INFO << "Entity " << exampleEntityID << " was updated by " << snapshotIDs.size() << " snapshots.";
});
// Using a member function:
- memoryReader.subscribe(entityID, this, &ExampleMemoryClient::example_entityUpdated);
+ memoryNameSystem.subscribe(exampleEntityID, this, &ExampleMemoryClient::processExampleEntityUpdate);
task = new RunningTask<ExampleMemoryClient>(this, &ExampleMemoryClient::run);
@@ -117,10 +122,10 @@ namespace armarx
run_started = IceUtil::Time::now();
std::srand(std::time(nullptr));
- armem::MemoryID snapshotID = commitSingleSnapshot(entityID);
+ armem::MemoryID snapshotID = commitSingleSnapshot(exampleEntityID);
if (true)
{
- commitMultipleSnapshots(entityID, 3);
+ commitMultipleSnapshots(exampleEntityID, 3);
}
if (true)
{
@@ -135,15 +140,19 @@ namespace armarx
commitExampleData();
queryExampleData();
}
+ if (true)
+ {
+ commitExamplesWithLinks();
+ }
- CycleUtil c(1000);
+ CycleUtil c(static_cast<int>(1000 / p.commitFrequency));
int i = 0;
while (!task->isStopped())
{
commitExampleData();
if (i++ < 10000)
{
- commitSingleSnapshot(entityID);
+ commitSingleSnapshot(exampleEntityID);
}
c.waitForCycleDuration();
}
@@ -163,6 +172,7 @@ namespace armarx
return armem::MemoryID(result.segmentID);
}
+
armem::MemoryID ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
// Prepare the update with some empty instances.
@@ -179,7 +189,7 @@ namespace armarx
auto cos = std::make_shared<aron::datanavigator::FloatNavigator>(std::cos(diff));
auto sqrt = std::make_shared<aron::datanavigator::DoubleNavigator>(std::sqrt(diff));
- auto lin = std::make_shared<aron::datanavigator::LongNavigator>((long)(diff * 1000));
+ auto lin = std::make_shared<aron::datanavigator::LongNavigator>(static_cast<long>(diff * 1000));
auto rand = std::make_shared<aron::datanavigator::IntNavigator>(std::rand());
dict1->addElement("sin", sin);
@@ -206,13 +216,14 @@ namespace armarx
return updateResult.snapshotID;
}
+
void ExampleMemoryClient::commitMultipleSnapshots(const armem::MemoryID& entityID, int num)
{
// Commit a number of updates with different timestamps and number of instances.
armem::Commit commit;
for (int i = 0; i < num; ++i)
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
+ armem::EntityUpdate& update = commit.add();
update.entityID = entityID;
update.timeCreated = armem::Time::now() + armem::Time::seconds(i);
for (int j = 0; j < i; ++j)
@@ -290,6 +301,7 @@ namespace armarx
}
}
+
void ExampleMemoryClient::queryExactSnapshot(const armem::MemoryID& snapshotID)
{
ARMARX_IMPORTANT
@@ -299,11 +311,7 @@ namespace armarx
namespace qf = armem::client::query_fns;
armem::client::query::Builder qb;
- qb
- .coreSegments(qf::withID(snapshotID))
- .providerSegments(qf::withID(snapshotID))
- .entities(qf::withID(snapshotID))
- .snapshots(qf::withID(snapshotID));
+ qb.singleEntitySnapshot(snapshotID);
armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_INFO << qResult;
@@ -335,23 +343,25 @@ namespace armarx
ARMARX_ERROR << addSegmentResult.errorMessage;
return;
}
- armem::MemoryID providerID = armem::MemoryID(addSegmentResult.segmentID);
+ exampleDataProviderID = armem::MemoryID(addSegmentResult.segmentID);
- armem::Time time = armem::Time::now();
+ const armem::Time time = armem::Time::now();
armem::Commit commit;
+
//default
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName("default");
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("default");
update.timeCreated = time;
- armem::example::ExampleData primitive;
- update.instancesData = { primitive.toAron() };
+ armem::example::ExampleData data;
+ toAron(data.memoryLink, armem::MemoryID());
+ update.instancesData = { data.toAron() };
}
//the answer
{
- armem::EntityUpdate& update = commit.updates.emplace_back();
- update.entityID = providerID.withEntityName("the answer");
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("the answer");
update.timeCreated = time;
armem::example::ExampleData data;
@@ -378,6 +388,9 @@ namespace armarx
{ "two", 2 },
{ "three", 3 },
};
+
+ toAron(data.memoryLink, armem::MemoryID());
+
{
data.the_ivt_image = std::make_shared<CByteImage>();
CByteImage& image = *data.the_ivt_image;
@@ -401,24 +414,29 @@ namespace armarx
}
armem::CommitResult commitResult = memoryWriter.commit(commit);
- if (!commitResult.allSuccess())
+ if (commitResult.allSuccess())
+ {
+ armem::fromIce(commitResult.results.at(1).snapshotID, theAnswerSnapshotID);
+ }
+ else
{
ARMARX_WARNING << commitResult.allErrorMessages();
}
}
+
void ExampleMemoryClient::queryExampleData()
{
// Query all entities from provider.
armem::client::query::Builder qb;
qb
- .coreSegments().withID(providerID)
- .providerSegments().withID(providerID)
+ .coreSegments().withID(exampleProviderID)
+ .providerSegments().withID(exampleProviderID)
.entities().all()
.snapshots().all();
armem::client::QueryResult result = memoryReader.query(qb.buildQueryInput());
- if (result)
+ if (result.success)
{
tab.queryResult = std::move(result.memory);
tab.rebuild = true;
@@ -430,8 +448,65 @@ namespace armarx
}
+ void ExampleMemoryClient::commitExamplesWithLinks()
+ {
+ const armem::Time time = armem::Time::now();
+
+ armem::Commit commit;
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to the_answer");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, theAnswerSnapshotID);
+
+ update.instancesData = { data.toAron() };
+ }
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to self");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, update.entityID.withTimestamp(time));
+
+ update.instancesData = { data.toAron() };
+ }
+
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
+ update.timeCreated = time - armem::Time::seconds(1); // 1 sec in the past
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, armem::MemoryID()); // First entry - invalid link
+
+ update.instancesData = { data.toAron() };
+ }
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ armem::toAron(data.memoryLink, update.entityID.withTimestamp(time - armem::Time::seconds(1)));
+
+ update.instancesData = { data.toAron() };
+ }
+
+ ARMARX_CHECK_EQUAL(commit.updates.size(), 4);
+ armem::CommitResult commitResult = memoryWriter.commit(commit);
+
+ if (!commitResult.allSuccess() || commitResult.results.size() != commit.updates.size())
+ {
+ ARMARX_WARNING << commitResult.allErrorMessages();
+ }
+ }
+
- void ExampleMemoryClient::example_entityUpdated(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void ExampleMemoryClient::processExampleEntityUpdate(
+ const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
{
std::stringstream ss;
ss << "example_entity got updated: " << subscriptionID << "\n";
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 774a4cbb84bc06496471e214d1cdb7935af1418f..5680e316a6228021932afea5d73abd02fdcda6d8 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -39,28 +39,18 @@
namespace armarx
{
- /**
- * @class ExampleClientPropertyDefinitions
- * @brief Property definitions of `ExampleClient`.
- */
- class ExampleMemoryClientPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- ExampleMemoryClientPropertyDefinitions(std::string prefix);
- };
-
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
+ *
+ * An example for an ArMem Memory Client.
*
* @class ExampleClient
* @ingroup Component-ExampleClient
* @brief Brief description of class ExampleClient.
*
- * Detailed description of class ExampleClient.
+ * Connects to the example memory, and commits and queries example data.
*/
class ExampleMemoryClient :
virtual public armarx::Component,
@@ -93,35 +83,48 @@ namespace armarx
private:
- armem::MemoryID providerID;
- armem::MemoryID entityID;
-
// Callback for updates on `example_entity`.
- void example_entityUpdated(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
+ void processExampleEntityUpdate(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
// Examples
void waitForMemory();
armem::MemoryID addProviderSegment();
- armem::MemoryID commitSingleSnapshot(const armem::MemoryID& entityID);
- void commitMultipleSnapshots(const armem::MemoryID& entityID, int num = 3);
+ armem::MemoryID commitSingleSnapshot(const armem::MemoryID& exampleEntityID);
+ void commitMultipleSnapshots(const armem::MemoryID& exampleEntityID, int num = 3);
- void queryLatestSnapshot(const armem::MemoryID& entityID);
+ void queryLatestSnapshot(const armem::MemoryID& exampleEntityID);
void queryExactSnapshot(const armem::MemoryID& snapshotID);
void commitExampleData();
void queryExampleData();
+ void commitExamplesWithLinks();
+
+
private:
+ struct Properties
+ {
+ std::string usedMemoryName = "Example";
+ float commitFrequency = 10;
+ };
+ Properties p;
+
+
+ armem::MemoryID exampleProviderID;
+ armem::MemoryID exampleEntityID;
+
+ armem::MemoryID exampleDataProviderID;
+ armem::MemoryID theAnswerSnapshotID;
+
+
IceUtil::Time run_started;
armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
- std::string memoryName = "Example";
-
struct RemoteGuiTab : RemoteGui::Client::Tab
{
std::atomic_bool rebuild = false;
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 85ca85f48ca62c7bc686bd9286d52fae5480ebc7..6650e573785cce732f0f2bbe7c128aa34fc58626 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -31,7 +31,7 @@
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(*this) {}
+ virtualRobotReader(this->memoryNameSystem) {}
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index e2127dedac1fab56502a1d95d535c0454f725a87..edd34d36d5d05c01ce7e40b2f1cbdc9213c2c2f6 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -1,12 +1,20 @@
<!--Some fancy comment -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+
<CodeIncludes>
<Include include="<Eigen/Core>" />
<Include include="<Image/ByteImage.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
</CodeIncludes>
+
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+
<GenerateTypes>
<Object name='armarx::armem::example::ExampleData'>
+
<ObjectChild key='the_int'>
<Int />
</ObjectChild>
@@ -79,6 +87,11 @@
</Dict>
</ObjectChild>
+ <ObjectChild key="memoryLink">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
+
</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index e43cbbc7c728405c745783a231484dd9cd154ebb..8c41ace39ea53283c22b3a191ae9cd9ebba2ce3c 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -70,9 +70,15 @@ namespace armarx::armem::server::robot_state
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ const std::string prefix = "mem.";
+
+ workingMemory.name() = "RobotState";
+ defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
const std::string robotUnitPrefix{sensorValuePrefix};
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
@@ -123,7 +129,7 @@ namespace armarx::armem::server::robot_state
ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
- workingMemory.name() = workingMemoryName;
+ // workingMemory.name() = workingMemoryName;
}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 51d1930b83753c195ff7558154092ea025466e4c..3fa53590170e0ff9d288f7f130adae9814ee7446 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index c1796936e47e97e06b6a923a4ca6832bcdf81f40..1dacd798cc3feb30b0fb04b283b0d9ebb3eb2ae4 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -297,7 +297,7 @@ namespace armarx
if (aValueChanged || newChannel)
{
- boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
+ std::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
if (timestamp < 0)
{
for (const auto& it : nameValueMap)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..524b650bf5a3767a92a457ab97b26ffcdd903a40
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_add_component(
+ COMPONENT_NAME ObstacleAwarePlatformUnit
+ HEADERS ObstacleAwarePlatformUnit.h
+ SOURCES ObstacleAwarePlatformUnit.cpp
+ COMPONENT_LIBS ArmarXCoreComponentPlugins
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2acb4a23b423df17ca7f80ffc76251634bcb933f
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -0,0 +1,728 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <numeric>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+// ArmarX
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+namespace
+{
+
+ void
+ invalidate(float& v)
+ {
+ v = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector2f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector3f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ v.z() = std::numeric_limits<float>::infinity();
+ }
+
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
+
+ std::string
+ to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
+ {
+ switch (mode)
+ {
+ case armarx::ObstacleAwarePlatformUnit::control_mode::position:
+ return "position";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::velocity:
+ return "velocity";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::none:
+ default:
+ return "unset";
+ }
+ }
+
+}
+
+
+const std::string
+armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+
+
+armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
+
+
+armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ if (!hasRobot("robot"))
+ {
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+ }
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+ invalidate(m_viz.start);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAwarePlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveTo(
+ const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
+ m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
+ m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
+
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ schedule_high_level_control_loop(control_mode::velocity);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveRelative(
+ const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
+{
+ using namespace simox::math;
+
+ // Transform relative to global.
+ std::unique_lock lock{m_control_data.mutex};
+ synchronizeLocalClone(m_robot);
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
+ lock.unlock();
+
+ // Use moveTo.
+ moveTo(
+ agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
+ const float max_pos_vel,
+ const float max_rot_vel,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+ m_control_data.max_vel = max_pos_vel;
+ m_control_data.max_rot_vel = max_rot_vel;
+ m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
+{
+ schedule_high_level_control_loop(control_mode::none);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode mode)
+{
+ std::scoped_lock l{m_control_loop.mutex};
+
+ // If the control mode didn't change, there's nothing to do.
+ if (m_control_loop.mode == mode)
+ {
+ return;
+ }
+
+ // If a control loop is runnung, stop it and wait for termination.
+ if (m_control_loop.mode != mode and m_control_loop.task)
+ {
+ ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
+ const bool join = true;
+ m_control_loop.task->stop(join);
+ }
+
+ // If the new control mode is none, no new control loop needs to be created.
+ if (mode == control_mode::none)
+ {
+ ARMARX_VERBOSE << "Going into stand-by.";
+ return;
+ }
+
+ // Start next control loop.
+ ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
+ this,
+ &ObstacleAwarePlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
+{
+ const control_mode mode = m_control_loop.mode;
+ ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
+
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ const velocities vels = get_velocities();
+
+ // Debug.
+ StringVariantBaseMap m;
+ m["err_dist"] = new Variant{vels.err_dist};
+ m["err_angular_dist"] = new Variant{vels.err_angular_dist};
+
+ m["target_global_x"] = new Variant{vels.target_global.x()};
+ m["target_global_y"] = new Variant{vels.target_global.y()};
+ m["target_global_abs"] = new Variant{vels.target_global.norm()};
+
+ m["target_local_x"] = new Variant{vels.target_local.x()};
+ m["target_local_y"] = new Variant{vels.target_local.y()};
+ m["target_local_abs"] = new Variant(vels.target_local.norm());
+ m["target_rot"] = new Variant{vels.target_rot};
+
+ m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
+ m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
+ m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
+
+ m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
+ m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
+ m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
+
+ setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
+
+ m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
+ visualize(vels);
+ cu.waitForCycleDuration();
+ }
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Error occured while running control loop.\n"
+ << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unknown error occured while running control loop.";
+ }
+
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+ m_control_loop.mode = control_mode::none;
+
+ visualize();
+
+ ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
+}
+
+
+armarx::ObstacleAwarePlatformUnit::velocities
+armarx::ObstacleAwarePlatformUnit::get_velocities()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+ // Update agent and get target velocities.
+ update_agent_dependent_values();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
+ // Apply obstacle avoidance and apply post processing to get the modulated velocity.
+ const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
+ {
+ const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
+ ARMARX_DEBUG << "Circle approximation: " << circle.radius;
+ m_viz.boundingCircle = circle;
+ const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+
+ ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+ // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+ float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
+
+ const Eigen::Vector2f vel = modifier * target_vel;
+ return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
+ }();
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+
+ velocities vels;
+ vels.target_local = r * target_vel;
+ vels.target_global = target_vel;
+ vels.modulated_local = r * modulated_vel;
+ vels.modulated_global = modulated_vel;
+ vels.target_rot = target_rot_vel;
+ vels.err_dist = m_control_data.target_dist;
+ vels.err_angular_dist = m_control_data.target_angular_dist;
+
+ return vels;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAwarePlatformUnit::get_target_velocity()
+const
+{
+ using namespace simox::math;
+
+ Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
+
+ if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
+ {
+ uncapped_target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_vel = m_control_data.target_vel;
+ }
+
+ ARMARX_CHECK(uncapped_target_vel.allFinite());
+
+ return norm_max(uncapped_target_vel, m_control_data.max_vel);
+}
+
+
+float
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
+const
+{
+ using namespace simox::math;
+
+ float uncapped_target_rot_vel = 0;
+
+ if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
+ {
+ m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
+ uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_rot_vel = m_control_data.target_rot_vel;
+ }
+
+ ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
+
+ return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ uncapped_target_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
+{
+ using namespace simox::math;
+
+ synchronizeLocalClone(m_robot);
+ m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
+ m_control_data.agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // Update distances in position mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ m_control_data.target_dist =
+ (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist =
+ periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
+ -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
+
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+ }
+ // Otherwise invalidate them.
+ else
+ {
+ invalidate(m_control_data.target_dist);
+ invalidate(m_control_data.target_angular_dist);
+ }
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_position_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_orientation_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return target_position_reached() and target_orientation_reached();
+ }
+
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
+const noexcept
+{
+ if (m_control_data.target_dist < m_control_data.pos_near_threshold)
+ {
+ const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
+
+ const float sim = simox::math::cosine_similarity(target_direction, control_direction);
+
+ // if almost pointing into same direction
+ if (sim > cos(M_PI_4f32))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize()
+{
+ const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+ velocities vels;
+ vels.target_local = zero;
+ vels.target_global = zero;
+ vels.modulated_local = zero;
+ vels.modulated_global = zero;
+ vels.target_rot = 0;
+
+ visualize(vels);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
+{
+ using namespace armarx::viz;
+
+ if (not m_viz.enabled)
+ {
+ return;
+ }
+
+ Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ Layer l_prog = arviz.layer("progress");
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+
+ {
+ l_prog.add(Cylinder{"boundingCylinder"}
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
+ }
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = agent_pos;
+ }
+
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(agent_pos);
+ }
+
+ // Draw path.
+ if (not target_reached())
+ {
+ // Start.
+ l_prog.add(Sphere{"start"}
+ .position(m_viz.start)
+ .color(Color::cyan(255, 64))
+ .radius(40));
+
+ // Path.
+ for (unsigned i = 0; i < m_viz.path.size(); ++i)
+ {
+ l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
+ }
+
+ // Goal.
+ const Eigen::Vector3f target{m_control_data.target_pos.x(),
+ m_control_data.target_pos.y(),
+ 0};
+ l_prog.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+ }
+ else
+ {
+ m_viz.path.clear();
+ invalidate(m_viz.start);
+ }
+ }
+
+ // Velocities.
+ Layer l_vels = arviz.layer("velocities");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ const float min_velocity = 15;
+ const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+ const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
+ const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
+
+ if (original.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"original"}
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (modulated.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+ }
+
+ // Agent.
+ Layer l_agnt = arviz.layer("agent");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ l_agnt.add(Sphere{"agent"}
+ .position(agent_pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_control_data.agent_safety_margin > 0)
+ {
+ l_agnt.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
+ }
+ }
+
+ arviz.commit({l_prog, l_vels, l_agnt});
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+
+ def->component(m_platform, "Platform");
+ def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
+
+ // Settings.
+ def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ "the robot should at least set when near the target");
+ def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ "should at least set on general");
+ def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ "which the robot is considered to be near the target for min velocity, "
+ "smoothing, etc.");
+
+ // Control parameters.
+ def->optional(m_control_data.kp, "control.pos.kp");
+ def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
+ def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
+ def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..274b67ea153eec20473b67fa4f45b8425c7b174d
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -0,0 +1,259 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <tuple>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAwarePlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
+ {
+
+ public:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ private:
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAwarePlatformUnit>::pointer_type task;
+ IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
+ };
+
+ struct control_data
+ {
+ std::mutex mutex;
+ Eigen::Vector2f target_pos;
+ float target_ori;
+ Eigen::Vector2f target_vel;
+ float target_rot_vel;
+ float target_dist;
+ float target_angular_dist;
+ Eigen::Vector2f agent_pos;
+ float agent_ori;
+ double agent_safety_margin = 0;
+ float min_vel_near_target = 50;
+ float min_vel_general = 100;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_near_threshold = 250;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ VirtualRobot::Circle boundingCircle;
+ };
+
+ struct velocities
+ {
+ Eigen::Vector2f target_local;
+ Eigen::Vector2f modulated_local;
+ Eigen::Vector2f target_global;
+ Eigen::Vector2f modulated_global;
+ float target_rot;
+ float err_dist;
+ float err_angular_dist;
+ };
+
+ public:
+
+ ObstacleAwarePlatformUnit();
+
+ ~ObstacleAwarePlatformUnit()
+ override;
+
+ std::string
+ getDefaultName()
+ const override;
+
+ void
+ moveTo(
+ float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ moveRelative(
+ float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ void
+ onInitPlatformUnit()
+ override;
+
+ void
+ onStartPlatformUnit()
+ override;
+
+ void
+ onExitPlatformUnit()
+ override;
+
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ velocities
+ get_velocities();
+
+ void
+ update_agent_dependent_values();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
+
+ bool
+ target_position_reached()
+ const;
+
+ bool
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
+
+ void
+ visualize();
+
+ void
+ visualize(const velocities& vels);
+
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ VirtualRobot::RobotPtr m_robot;
+ DynamicObstacleManagerInterfacePrx m_obsman;
+
+ ObstacleAwarePlatformUnit::control_loop m_control_loop;
+ ObstacleAwarePlatformUnit::control_data m_control_data;
+
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f168208f6734ea228aa848dcf0c5d0976370bae7
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
@@ -0,0 +1,40 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAwarePlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 0ec81ab3895bc1037141ee6798c01effcbee45eb..e106a9dbb943dabd9bd23a05e9d8320b6e415b82 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -1419,7 +1419,7 @@ namespace armarx
auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
{
double D = tf - t0;
- BOOST_ASSERT(D != 0.0);
+ ARMARX_CHECK(D != 0.0);
double D2 = D * D;
double tau = (t - t0) / D;
double b0 = s0;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index bc246422b98fbd89feac994a2ace52645532e22a..77f9c8d8b49f9f192eb63ab64fe9d5b308b9e22e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,6 +23,10 @@
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <cmath>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -34,7 +38,7 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
+ opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
@@ -78,24 +82,14 @@ namespace armarx
return;
}
- float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
- Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
-
- Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
- float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
-
- float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
- relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
-
- float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
- relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
+ const float measuredOrientation = rtGetControlStruct().globalOrientation;
+ pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
- pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
- //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
+ const Eigen::Rotation2Df global_R_local(-measuredOrientation);
- Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
target->velocityX = velocities.x();
target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -112,7 +106,7 @@ namespace armarx
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
getWriterControlStruct().newTargetSet = true;
@@ -125,7 +119,7 @@ namespace armarx
// ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
+ getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation;
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 1c00dd4e630208a79852ea3d18d96da8a501f7d0..bebbb3f414be2184f51fdb8518cdcd368da33ebc 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -104,7 +104,6 @@ module data
{
Vector3fSeq points;
- Color lineColor;
float lineWidth = 10.0f;
};
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index a4c8ba2f7602be3eccf333584cffe7a773486f03..ec83f439d8a91e8e57fe4c1767a8ba27bfab7c5d 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -6,6 +6,8 @@
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
+
module armarx
{
@@ -21,7 +23,10 @@ module armarx
{
};
- interface MemoryInterface extends WorkingMemoryInterface, LongTermMemoryInterface
+ interface MemoryInterface extends
+ WorkingMemoryInterface,
+ LongTermMemoryInterface,
+ client::MemoryListenerInterface
{
};
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index d21a3c55b7574a459f0be3e2577870cf7640f5e3..5ff2649fae12cb906f5821f73ba1fb1fd21647fe 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -30,6 +30,18 @@
module armarx
{
+ module dynamicobstaclemanager
+ {
+ struct LineSegment
+ {
+ Eigen::Vector2f lineStart;
+ Eigen::Vector2f lineEnd;
+ };
+
+ sequence<LineSegment> LineSegments;
+ }
+
+
interface DynamicObstacleManagerInterface
{
void
@@ -38,6 +50,9 @@ module armarx
void
add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void
+ add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+
void
remove_all_decayable_obstacles();
@@ -48,6 +63,8 @@ module armarx
remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
+
+ float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 0af92b11dcd764b7ab376f07a5869cb1962e5f4f..28a302b49f98aec283958bdb368cc9e3a164818b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -29,8 +29,38 @@
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+BOOST_AUTO_TEST_CASE(test_ObjectType_copy_assignment)
+{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectType lhs, rhs;
+
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
+
+ BOOST_TEST_MESSAGE("Done");
+}
+
+BOOST_AUTO_TEST_CASE(test_ObjectAttachmentInfo_copy_assignment)
{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectAttachmentInfo lhs, rhs;
+
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
+
+ BOOST_TEST_MESSAGE("Done");
+}
+
+BOOST_AUTO_TEST_CASE(test_ObjectPose_copy_assignment)
+{
+ BOOST_TEST_MESSAGE("Constructor");
+ armarx::objpose::arondto::ObjectPose lhs, rhs;
+
+ BOOST_TEST_MESSAGE("Assignment");
+ BOOST_CHECK_NO_THROW(lhs = rhs);
- BOOST_CHECK_EQUAL(true, true);
+ BOOST_TEST_MESSAGE("Done");
}
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 5df6b028f0272c51a0538392375c27421d19dc06..02e5c638bfdfc145c271a995e016ce0c5f15ba01 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,7 +22,7 @@ add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
-add_subdirectory(armem_robot_mapping)
+add_subdirectory(armem_vision)
add_subdirectory(armem_skills)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
index 92005dea9a32122b5e447d60436332286e1625dd..a568893eb8448825f471ceb44f31944e3bdab56b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
@@ -155,28 +155,25 @@ namespace armarx::RemoteGui
.addChild(new RemoteGui::HSpacer)
);
}
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- void GetValueOutputParameter <CartesianWaypointControllerConfig, void>::call(
- CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
+
+ void getValueFromMap(CartesianWaypointControllerConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name)
{
- cfg.maxPositionAcceleration = getValue<float>(values, name + "_maxAcc_Pos");
- cfg.maxOrientationAcceleration = getValue<float>(values, name + "_maxAcc_Ori");
- cfg.maxNullspaceAcceleration = getValue<float>(values, name + "_maxAcc_Null");
+ getValueFromMap(cfg.maxPositionAcceleration, values, name + "_maxAcc_Pos");
+ getValueFromMap(cfg.maxOrientationAcceleration, values, name + "_maxAcc_Ori");
+ getValueFromMap(cfg.maxNullspaceAcceleration, values, name + "_maxAcc_Null");
- cfg.kpJointLimitAvoidance = getValue<float>(values, name + "_JointLimitAvoidance_KP");
- cfg.jointLimitAvoidanceScale = getValue<float>(values, name + "_JointLimitAvoidance_Scale");
+ getValueFromMap(cfg.kpJointLimitAvoidance, values, name + "_JointLimitAvoidance_KP");
+ getValueFromMap(cfg.jointLimitAvoidanceScale, values, name + "_JointLimitAvoidance_Scale");
- cfg.thresholdOrientationNear = getValue<float>(values, name + "_Thresholds_Ori_Near");
- cfg.thresholdOrientationReached = getValue<float>(values, name + "_Thresholds_Ori_Reached");
- cfg.thresholdPositionNear = getValue<float>(values, name + "_Thresholds_Pos_Near");
- cfg.thresholdPositionReached = getValue<float>(values, name + "_Thresholds_Pos_Reached");
+ getValueFromMap(cfg.thresholdOrientationNear, values, name + "_Thresholds_Ori_Near");
+ getValueFromMap(cfg.thresholdOrientationReached, values, name + "_Thresholds_Ori_Reached");
+ getValueFromMap(cfg.thresholdPositionNear, values, name + "_Thresholds_Pos_Near");
+ getValueFromMap(cfg.thresholdPositionReached, values, name + "_Thresholds_Pos_Reached");
- cfg.maxOriVel = getValue<float>(values, name + "_Max_vel_ori");
- cfg.maxPosVel = getValue<float>(values, name + "_Max_vel_pos");
- cfg.kpOri = getValue<float>(values, name + "_KP_ori");
- cfg.kpPos = getValue<float>(values, name + "_KP_pos");
+ getValueFromMap(cfg.maxOriVel, values, name + "_Max_vel_ori");
+ getValueFromMap(cfg.maxPosVel, values, name + "_Max_vel_pos");
+ getValueFromMap(cfg.kpOri, values, name + "_KP_ori");
+ getValueFromMap(cfg.kpPos, values, name + "_KP_pos");
}
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
index ca43720fa07154d3067941a7f9ec496f2e43bd26..238ba2ae38f1f20aeba20604c62c86f1e432488b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
@@ -31,15 +31,7 @@ namespace armarx::RemoteGui
detail::GroupBoxBuilder makeConfigGui(
const std::string& name,
const CartesianWaypointControllerConfig& val);
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- template<>
- struct GetValueOutputParameter <CartesianWaypointControllerConfig, void> : std::true_type
- {
- static void call(
- CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
- };
+ void getValueFromMap(CartesianWaypointControllerConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name);
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
index fece7f9254cc646114adec271f5c931a92b47432..dc6aadb5ad0b72d8ebe19ad8e8f1b21b3a8b7d9b 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
@@ -49,16 +49,13 @@ namespace armarx::RemoteGui
return builder;
}
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- void GetValueOutputParameter <NJointCartesianWaypointControllerRuntimeConfig, void>::call(
- NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
+
+ void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name)
{
- getValue(cfg.wpCfg, values, name);
- getValue(cfg.forceThreshold, values, name + "_forceThreshold");
- getValue(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
- getValue(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
+ getValueFromMap(cfg.wpCfg, values, name);
+ getValueFromMap(cfg.forceThreshold, values, name + "_forceThreshold");
+ getValueFromMap(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
+ getValueFromMap(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
}
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
index f06371d4fe96ec51fcafbf80fb9a999e86f697f3..f8b06a1a00c52963ff590e196ba95c97bdb3cefb 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
@@ -32,19 +32,7 @@ namespace armarx::RemoteGui
const std::string& name,
const NJointCartesianWaypointControllerRuntimeConfig& val);
- template <>
- NJointCartesianWaypointControllerRuntimeConfig getValue<NJointCartesianWaypointControllerRuntimeConfig>(
- ValueMap const& values,
- std::string const& name);
-}
-namespace armarx::RemoteGui::detail::_getValue
-{
- template<>
- struct GetValueOutputParameter <NJointCartesianWaypointControllerRuntimeConfig, void> : std::true_type
- {
- static void call(
- NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
- };
+ void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ RemoteGui::ValueMap const& values, std::string const& name);
}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 477b0abf97f9521ab52e679622b514c9c3bce3cf..a374c1ac1540763305b5000cc71ec9fe6cec4435 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -26,6 +26,7 @@ set(LIBS
set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
+ core/MemoryID_operators.cpp
core/SuccessHeader.cpp
core/Time.cpp
core/ice_conversions.cpp
@@ -52,10 +53,11 @@ set(LIB_FILES
core/workingmemory/EntitySnapshot.cpp
core/workingmemory/Memory.cpp
core/workingmemory/ProviderSegment.cpp
- core/workingmemory/Visitor.cpp
core/workingmemory/ice_conversions.cpp
core/workingmemory/json_conversions.cpp
core/workingmemory/entityInstance_conversions.cpp
+ core/workingmemory/visitor/Visitor.cpp
+ core/workingmemory/visitor/FunctionalVisitor.cpp
core/longtermmemory/CoreSegment.cpp
core/longtermmemory/Entity.cpp
@@ -74,13 +76,20 @@ set(LIB_FILES
core/diskmemory/ProviderSegment.cpp
core/error/ArMemError.cpp
+ core/error/mns.cpp
client/ComponentPlugin.cpp
+ client/MemoryNameSystem.cpp
+ client/MemoryNameSystemComponentPlugin.cpp
client/Reader.cpp
client/ReaderComponentPlugin.cpp
client/Writer.cpp
client/WriterComponentPlugin.cpp
+ client/util/MemoryListener.cpp
+ client/util/SimpleReaderBase.cpp
+ client/util/SimpleWriterBase.cpp
+
client/Query.cpp
client/query/Builder.cpp
client/query/selectors.cpp
@@ -109,7 +118,6 @@ set(LIB_FILES
server/query_proc/longtermmemory/MemoryQueryProcessor.cpp
mns/MemoryNameSystem.cpp
- mns/ClientPlugin.cpp
mns/ComponentPlugin.cpp
util/util.cpp
@@ -120,6 +128,7 @@ set(LIB_HEADERS
core/Commit.h
core/DataMode.h
core/MemoryID.h
+ core/MemoryID_operators.h
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
@@ -128,6 +137,7 @@ set(LIB_HEADERS
core/error.h
core/error/ArMemError.h
+ core/error/mns.h
core/base/detail/MemoryItem.h
core/base/detail/MaxHistorySize.h
@@ -149,10 +159,12 @@ set(LIB_HEADERS
core/workingmemory/EntitySnapshot.h
core/workingmemory/Memory.h
core/workingmemory/ProviderSegment.h
- core/workingmemory/Visitor.h
core/workingmemory/ice_conversions.h
core/workingmemory/json_conversions.h
core/workingmemory/entityInstance_conversions.h
+ core/workingmemory/visitor.h
+ core/workingmemory/visitor/Visitor.h
+ core/workingmemory/visitor/FunctionalVisitor.h
core/longtermmemory/CoreSegment.h
core/longtermmemory/Entity.h
@@ -175,11 +187,14 @@ set(LIB_HEADERS
client.h
client/ComponentPlugin.h
+ client/MemoryNameSystem.h
+ client/MemoryNameSystemComponentPlugin.h
client/Reader.h
client/ReaderComponentPlugin.h
client/Writer.h
client/WriterComponentPlugin.h
+ client/query.h
client/Query.h
client/query/Builder.h
client/query/query_fns.h
@@ -187,6 +202,10 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
+ client/util/MemoryListener.h
+ client/util/SimpleReaderBase.h
+ client/util/SimpleWriterBase.h
+
server.h
server/ComponentPlugin.h
server/MemoryToIceAdapter.h
@@ -214,7 +233,6 @@ set(LIB_HEADERS
mns.h
mns/MemoryNameSystem.h
- mns/ClientPlugin.h
mns/ComponentPlugin.h
util/util.h
diff --git a/source/RobotAPI/libraries/armem/client.h b/source/RobotAPI/libraries/armem/client.h
index baf0ca232e5447048939aa062ee122a1d4f936b2..834d62ab45d88d18e4277929c47acd55c5825263 100644
--- a/source/RobotAPI/libraries/armem/client.h
+++ b/source/RobotAPI/libraries/armem/client.h
@@ -1,6 +1,8 @@
#pragma once
#include "client/ComponentPlugin.h"
+#include "client/MemoryNameSystem.h"
+#include "client/MemoryNameSystemComponentPlugin.h"
#include "client/Query.h"
#include "client/query/Builder.h"
#include "client/query/query_fns.h"
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
index ba836f47f31f1dd5e21cb5101fbd7c4997ca4836..a68e742cd24489694da91ef73ff557a6e8be1294 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.cpp
@@ -21,20 +21,20 @@ armarx::armem::client::ComponentPluginUser::~ComponentPluginUser()
void
-armarx::armem::client::ComponentPluginUser::setMemory(server::MemoryInterfacePrx memory)
+armarx::armem::client::ComponentPluginUser::setMemoryServer(server::MemoryInterfacePrx memory)
{
- setReadingMemory(memory);
- setWritingMemory(memory);
+ setReadingMemoryServer(memory);
+ setWritingMemoryServer(memory);
}
armarx::armem::data::WaitForMemoryResult
-armarx::armem::client::ComponentPluginUser::useMemory(const std::string& memoryName)
+armarx::armem::client::ComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setMemory(result.proxy);
+ setMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index 7fc432536b62411a912d7829bc40fd82efa5a855..3bdbde93bd046bcceb361d94183be5fd1c09db23 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -32,14 +32,15 @@ namespace armarx::armem::client
* @brief Resolve the given memory name, add it as dependency and update the readers and writers.
* @param The memory's name.
* @return The result of `waitForMemory()`.
- * @see `armem::mns::plugins::ClientPluginUserBase::useMemory()`
+ * @see `armem::MemoryNameSystemComponentPluginUser::useMemory()`
*/
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
+
protected:
- void setMemory(server::MemoryInterfacePrx memory);
+ void setMemoryServer(server::MemoryInterfacePrx memory);
};
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..826a567ab68378d29ba600bfab1801b5a0013a40
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -0,0 +1,366 @@
+#include "MemoryNameSystem.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::client
+{
+
+ MemoryNameSystem::MemoryNameSystem()
+ {
+ }
+
+
+ MemoryNameSystem::MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component) :
+ mns(mns), component(component)
+ {
+ }
+
+
+ void MemoryNameSystem::update()
+ {
+ ARMARX_CHECK_NOT_NULL(mns);
+
+ data::GetAllRegisteredMemoriesResult result;
+ try
+ {
+ result = mns->getAllRegisteredMemories();
+ }
+ catch (const Ice::NotRegisteredException& e)
+ {
+ throw error::MemoryNameSystemQueryFailed(e.what());
+ }
+
+ if (result.success)
+ {
+ this->servers = result.proxies;
+ }
+ else
+ {
+ throw error::MemoryNameSystemQueryFailed(result.errorMessage);
+ }
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::resolveServer(const MemoryID& memoryID)
+ {
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ update();
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ throw error::CouldNotResolveMemoryServer(memoryID);
+ }
+ }
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::waitForServer(const MemoryID& memoryID, Time timeout)
+ {
+ if (auto it = servers.find(memoryID.memoryName); it != servers.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ armem::data::WaitForMemoryInput input;
+ input.name = memoryID.memoryName;
+ input.timeoutMilliSeconds = timeout.toMilliSeconds();
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ armem::data::WaitForMemoryResult result = mns->waitForMemory(input);
+ if (result.success)
+ {
+ return result.proxy;
+ }
+ else
+ {
+ throw error::CouldNotResolveMemoryServer(memoryID, result.errorMessage);
+ }
+ }
+ }
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID)
+ {
+ ARMARX_CHECK_NOT_NULL(component)
+ << "Owning component not set when using a memory server. \n"
+ << "When calling `armem::mns::MemoryNameSystem::useServer()`, the owning component which should "
+ << "receive the dependency to the memory server must be set beforehand. \n\n"
+ << "Use `armem::mns::MemoryNameSystem::setComponent()` or pass the component on construction "
+ << "before calling useServer().";
+ return useServer(memoryID, *component);
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ server::MemoryInterfacePrx server = waitForServer(memoryID);
+ // Add dependency.
+ component.usingProxy(server->ice_getIdentity().name);
+ return server;
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName)
+ {
+ return useServer(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ server::MemoryInterfacePrx MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useServer(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
+ {
+ return Reader(resolveServer(memoryID));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
+ {
+ return Reader(useServer(memoryID));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ return Reader(useServer(memoryID, component));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const std::string& memoryName)
+ {
+ return useReader(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ Reader MemoryNameSystem::useReader(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useReader(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ template <class ClientT>
+ std::map<std::string, ClientT> MemoryNameSystem::_getAllClients() const
+ {
+ std::map<std::string, ClientT> result;
+ for (const auto& [name, server] : servers)
+ {
+ result[name] = ClientT(server);
+ }
+ return result;
+ }
+
+
+ template <class ClientT>
+ std::map<std::string, ClientT> MemoryNameSystem::_getAllClients(bool update)
+ {
+ if (update)
+ {
+ this->update();
+ }
+ return const_cast<const MemoryNameSystem&>(*this)._getAllClients<ClientT>();
+ }
+
+
+ std::map<std::string, Reader> MemoryNameSystem::getAllReaders(bool update)
+ {
+ return _getAllClients<Reader>(update);
+ }
+
+
+ std::map<std::string, Reader> MemoryNameSystem::getAllReaders() const
+ {
+ return _getAllClients<Reader>();
+ }
+
+
+ Writer MemoryNameSystem::getWriter(const MemoryID& memoryID)
+ {
+ return Writer(resolveServer(memoryID));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const MemoryID& memoryID)
+ {
+ return Writer(useServer(memoryID));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
+ {
+ return Writer(useServer(memoryID, component));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const std::string& memoryName)
+ {
+ return useWriter(MemoryID().withMemoryName(memoryName));
+ }
+
+
+ Writer MemoryNameSystem::useWriter(const std::string& memoryName, ManagedIceObject& component)
+ {
+ return useWriter(MemoryID().withMemoryName(memoryName), component);
+ }
+
+
+ std::map<std::string, Writer> MemoryNameSystem::getAllWriters(bool update)
+ {
+ return _getAllClients<Writer>(update);
+ }
+
+
+ std::map<std::string, Writer> MemoryNameSystem::getAllWriters() const
+ {
+ return _getAllClients<Writer>();
+ }
+
+
+ std::optional<wm::EntityInstance> MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
+ {
+ auto result = resolveEntityInstances({id});
+ if (result.size() > 0)
+ {
+ return result.begin()->second;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::map<MemoryID, wm::EntityInstance> MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
+ {
+ std::stringstream errors;
+ int errorCounter = 0;
+
+ std::map<std::string, std::vector<MemoryID>> idsPerMemory;
+ for (const auto& id : ids)
+ {
+ idsPerMemory[id.memoryName].push_back(id);
+ }
+
+ std::map<MemoryID, wm::EntityInstance> result;
+ for (const auto& [memoryName, ids] : idsPerMemory)
+ {
+ Reader reader = getReader(MemoryID().withMemoryName(memoryName));
+ QueryResult queryResult = reader.queryMemoryIDs(ids);
+ if (queryResult.success)
+ {
+ for (const MemoryID& id : ids)
+ {
+ try
+ {
+ if (id.hasInstanceIndex())
+ {
+ result[id] = queryResult.memory.getEntityInstance(id);
+ }
+ else if (id.hasTimestamp())
+ {
+ result[id] = queryResult.memory.getEntitySnapshot(id).getInstance(0);
+ }
+ else if (id.hasEntityName())
+ {
+ result[id] = queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
+ }
+ else
+ {
+ std::stringstream ss;
+ ss << "MemoryNameSystem::" << __FUNCTION__ << "requires IDs to be entity, snapshot or instance IDs,"
+ << "but ID has no entity name.";
+ throw error::InvalidMemoryID(id, ss.str());
+ }
+ }
+ catch (const error::ArMemError& e)
+ {
+ errors << "#" << ++errorCounter << "\n"
+ << "Failed to retrieve " << id << " from query result: \n" << e.what() << "\n";
+ }
+ }
+ }
+ else
+ {
+ errors << "# Failed to query '" << memoryName << "': \n" << queryResult.errorMessage << "\n";
+ }
+ }
+
+ if (errors.str().size() > 0)
+ {
+ ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__ << ": The following errors may affect your result: "
+ << "\n\n" << errors.str() << "\n\n"
+ << "When querying entity instances: \n- "
+ << simox::alg::join(simox::alg::multi_to_string(ids), "\n- ");
+ }
+
+ return result;
+ }
+
+
+ void MemoryNameSystem::registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx proxy)
+ {
+ data::RegisterMemoryInput input;
+ input.name = memoryID.memoryName;
+ input.proxy = proxy;
+ ARMARX_CHECK_NOT_NULL(input.proxy);
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ data::RegisterMemoryResult result = mns->registerMemory(input);
+ if (!result.success)
+ {
+ throw error::ServerRegistrationOrRemovalFailed("register", memoryID, result.errorMessage);
+ }
+ }
+
+
+ void MemoryNameSystem::removeServer(const MemoryID& memoryID)
+ {
+ data::RemoveMemoryInput input;
+ input.name = memoryID.memoryName;
+
+ ARMARX_CHECK_NOT_NULL(mns);
+ data::RemoveMemoryResult result = mns->removeMemory(input);
+ if (!result.success)
+ {
+ throw error::ServerRegistrationOrRemovalFailed("remove", memoryID, result.errorMessage);
+ }
+ }
+
+
+ mns::MemoryNameSystemInterfacePrx MemoryNameSystem::getMemoryNameSystem() const
+ {
+ return mns;
+ }
+
+
+ void MemoryNameSystem::getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns)
+ {
+ this->mns = mns;
+ }
+
+ void MemoryNameSystem::setComponent(ManagedIceObject* component)
+ {
+ this->component = component;
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
new file mode 100644
index 0000000000000000000000000000000000000000..d981bab0383f0d04fdc221fbbd2cbd062d061c53
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -0,0 +1,270 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::libraries::armem
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <optional>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+namespace armarx::armem::client
+{
+ class Reader;
+ class Writer;
+}
+
+namespace armarx::armem::wm
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem::client
+{
+
+ /**
+ * @brief The memory name system (MNS) client.
+ *
+ * This client class serves provides the MNS interface and is a local cache
+ * of the MNS registry. It can be used to resolve memory servers by their
+ * memory ID and to construct `client::Readers` and `client::Writers`.
+ *
+ * During server resolution, it first consults the locally cached registry.
+ * If the memory server is not known locally, the local registry is
+ * updated to that of the remote MNS before trying again.
+ *
+ * In addition, the MNS client can be used to send queries over multiple
+ * memory servers, as well as retrieving the data for arbitrary entity or
+ * entity snapshot IDs.
+ */
+ class MemoryNameSystem : public util::MemoryListener
+ {
+ public:
+
+ MemoryNameSystem();
+
+ /**
+ * @brief Construct an MNS client.
+ *
+ * @param mns The MNS proxy.
+ * @param component The owning component. When `using` a memory server,
+ * dependencies will be added to this component.
+ */
+ MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
+ ManagedIceObject* component = nullptr);
+
+
+ mns::MemoryNameSystemInterfacePrx getMemoryNameSystem() const;
+ void getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns);
+
+ void setComponent(ManagedIceObject* component);
+
+
+ // Name Resolution
+
+ /**
+ * @brief Update the internal registry to the data in the MNS.
+ *
+ * @throw `error::MemoryNameSystemQueryFailed` If the call to the MNS failed.
+ */
+ void update();
+
+ /**
+ * @brief Resolve the given memory server for the given memory ID.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx resolveServer(const MemoryID& memoryID);
+
+ /**
+ * @brief Wait for the given memory server.
+ *
+ * @param memoryID The memory ID.
+ * @param timeout How long to wait at maximum. Negative values indicate infinite wait.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx waitForServer(const MemoryID& memoryID, Time timeout = Time::milliSeconds(-1));
+
+ /**
+ * @brief Wait for the given memory server and add a dependency to the memory server.
+ *
+ * @param memoryID The memory ID.
+ * @param component The component that should depend on the memory server.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ server::MemoryInterfacePrx useServer(const MemoryID& memoryID);
+ server::MemoryInterfacePrx useServer(const MemoryID& memoryID, ManagedIceObject& component);
+ server::MemoryInterfacePrx useServer(const std::string& memoryName);
+ server::MemoryInterfacePrx useServer(const std::string& memoryName, ManagedIceObject& component);
+
+
+ // Reader/Writer construction
+
+ /**
+ * @brief Get a reader to the given memory name.
+ *
+ * @param memoryID The memory ID.
+ * @return The reader.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ Reader getReader(const MemoryID& memoryID);
+
+ /// Use a memory server and get a reader for it.
+ Reader useReader(const MemoryID& memoryID);
+ Reader useReader(const MemoryID& memoryID, ManagedIceObject& component);
+ Reader useReader(const std::string& memoryName);
+ Reader useReader(const std::string& memoryName, ManagedIceObject& component);
+
+ /**
+ * @brief Get Readers for all registered servers.
+ *
+ * @param update If true, perform an update first.
+ * @return The readers.
+ */
+ std::map<std::string, Reader> getAllReaders(bool update = true);
+ /**
+ * @brief Get Readers for all registered servers (without updating).
+ *
+ * @return The readers.
+ */
+ std::map<std::string, Reader> getAllReaders() const;
+
+
+ /**
+ * @brief Get a writer to the given memory name.
+ *
+ * @param memoryID The memory ID.
+ * @return The writer.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ Writer getWriter(const MemoryID& memoryID);
+
+ /// Use a memory server and get a writer for it.
+ Writer useWriter(const MemoryID& memoryID);
+ Writer useWriter(const MemoryID& memoryID, ManagedIceObject& component);
+ Writer useWriter(const std::string& memoryName);
+ Writer useWriter(const std::string& memoryName, ManagedIceObject& component);
+
+ /**
+ * @brief Get Writers for all registered servers.
+ *
+ * @param update If true, perform an update first.
+ * @return The readers.
+ */
+ std::map<std::string, Writer> getAllWriters(bool update = true);
+ /**
+ * @brief Get Writers for all registered servers (without updating).
+ *
+ * @return The readers.
+ */
+ std::map<std::string, Writer> getAllWriters() const;
+
+
+ // ToDo: commit() and query()
+
+ /**
+ * @brief Resolve a memory ID to an EntityInstance.
+ *
+ * The ID can refer to an entity, an entity snapshot, or an entity
+ * instance. When not referring to an entity instance, the latest
+ * snapshot and first instance will be queried.
+ *
+ * @param id The entity, snapshot or instance ID.
+ * @return
+ */
+ std::optional<wm::EntityInstance> resolveEntityInstance(const MemoryID& id);
+
+ std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
+
+
+
+ // Registration - only for memory servers
+
+ /**
+ * @brief Register a memory server in the MNS.
+ *
+ * @param memoryID The memoryID.
+ * @param server The memory server proxy.
+ *
+ * @throw `error::ServerRegistrationOrRemovalFailed` If the registration failed.
+ */
+ void registerServer(const MemoryID& memoryID, server::MemoryInterfacePrx server);
+
+ /**
+ * @brief Remove a memory server from the MNS.
+ *
+ * @param memoryID The memoryID.
+ *
+ * @throw `error::ServerRegistrationOrRemovalFailed` If the removal failed.
+ */
+ void removeServer(const MemoryID& memoryID);
+
+
+
+ // Operators
+
+ /// Indicate whether the proxy is set.
+ inline operator bool() const
+ {
+ return bool(mns);
+ }
+
+
+ private:
+
+ template <class ClientT>
+ std::map<std::string, ClientT> _getAllClients(bool update);
+ template <class ClientT>
+ std::map<std::string, ClientT> _getAllClients() const;
+
+
+ /// The MNS proxy.
+ mns::MemoryNameSystemInterfacePrx mns = nullptr;
+
+ /// The component to which dependencies will be added.
+ ManagedIceObject* component = nullptr;
+
+ /// The registered memory servers.
+ std::map<std::string, server::MemoryInterfacePrx> servers;
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d24d974e97932be9a56dd8693b1f74ef8a604f1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
@@ -0,0 +1,153 @@
+#include "MemoryNameSystemComponentPlugin.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+
+
+
+namespace armarx::armem::client::plugins
+{
+
+ MemoryNameSystemComponentPlugin::~MemoryNameSystemComponentPlugin()
+ {}
+
+
+ void MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(PROPERTY_MNS_NAME_NAME),
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName,
+ "Name of the Memory Name System (MNS) component.");
+ }
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
+ {
+ properties->defineOptionalProperty<bool>(
+ makePropertyName(PROPERTY_MNS_ENABLED_NAME),
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled,
+ "Whether to use (and depend on) the Memory Name System (MNS)."
+ "\nSet to false to use this memory as a stand-alone.");
+ }
+
+ // Subscribe topics by single servers, use this as a prefix.
+ properties->topic<MemoryListenerInterface>("MemoryUpdates");
+ }
+
+ void MemoryNameSystemComponentPlugin::preOnInitComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ parent().usingProxy(getMemoryNameSystemName());
+ }
+ }
+
+ void MemoryNameSystemComponentPlugin::preOnConnectComponent()
+ {
+ if (isMemoryNameSystemEnabled())
+ {
+ ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystem =
+ MemoryNameSystem(getMemoryNameSystem(), &parent());
+ }
+ }
+
+ bool MemoryNameSystemComponentPlugin::isMemoryNameSystemEnabled()
+ {
+ return parentDerives<Component>() ?
+ parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
+ parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemEnabled;
+ }
+
+ std::string MemoryNameSystemComponentPlugin::getMemoryNameSystemName()
+ {
+ return parentDerives<Component>() ?
+ parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
+ std::string{parent<MemoryNameSystemComponentPluginUser>().memoryNameSystemName};
+ }
+
+ mns::MemoryNameSystemInterfacePrx MemoryNameSystemComponentPlugin::getMemoryNameSystem()
+ {
+ return isMemoryNameSystemEnabled() && parentDerives<Component>()
+ ? parent<Component>().getProxy<mns::MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
+ : nullptr;
+ }
+
+}
+
+namespace armarx::armem::client
+{
+
+ MemoryNameSystemComponentPluginUser::MemoryNameSystemComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ MemoryNameSystemComponentPluginUser::~MemoryNameSystemComponentPluginUser()
+ {
+ }
+
+ void MemoryNameSystemComponentPluginUser::memoryUpdated(
+ const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current&)
+ {
+ memoryNameSystem.updated(updatedSnapshotIDs);
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const MemoryID& id)
+ {
+ armem::data::WaitForMemoryResult result;
+ try
+ {
+ // Add dependency.
+ result.proxy = memoryNameSystem.useServer(id);
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::useMemoryServer(const std::string& memoryName)
+ {
+ return useMemoryServer(MemoryID().withMemoryName(memoryName));
+ }
+
+ armem::data::WaitForMemoryResult MemoryNameSystemComponentPluginUser::waitForMemoryServer(const std::string& memoryName)
+ {
+ armem::data::WaitForMemoryResult result;
+ try
+ {
+ result.proxy = memoryNameSystem.waitForServer(MemoryID().withMemoryName(memoryName));
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+ armem::data::ResolveMemoryNameResult MemoryNameSystemComponentPluginUser::resolveMemoryServer(const std::string& memoryName)
+ {
+ armem::data::ResolveMemoryNameResult result;
+ try
+ {
+ result.proxy = memoryNameSystem.resolveServer(MemoryID().withMemoryName(memoryName));
+ result.success = true;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ return result;
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e71cd33c126a572e74441fdcb42cc219bee10fe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
@@ -0,0 +1,99 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::mns
+{
+ class MemoryNameSystemComponentPluginUser;
+}
+
+
+namespace armarx::armem::client::plugins
+{
+ /**
+ * @brief A base plugin offering optional access and dependency
+ * to the Memory Name System (MNS).
+ */
+ class MemoryNameSystemComponentPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+ virtual ~MemoryNameSystemComponentPlugin() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ /**
+ * @brief Indicate whether the Memory Name System (MNS) is enabled.
+ */
+ bool isMemoryNameSystemEnabled();
+ /**
+ * @brief Get the name of the MNS component.
+ */
+ std::string getMemoryNameSystemName();
+
+ /**
+ * @brief Get the MNS proxy.
+ * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
+ */
+ mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
+
+ public:
+ static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
+ static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
+ };
+
+}
+
+
+namespace armarx::armem::client
+{
+
+ class MemoryNameSystemComponentPluginUser :
+ virtual public ManagedIceObject,
+ virtual public MemoryListenerInterface
+ {
+ protected:
+
+ MemoryNameSystemComponentPluginUser();
+ virtual ~MemoryNameSystemComponentPluginUser() override;
+
+
+ virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
+
+
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::WaitForMemoryResult waitForMemoryServer(const std::string& memoryName);
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::ResolveMemoryNameResult resolveMemoryServer(const std::string& memoryName);
+
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ armem::data::WaitForMemoryResult useMemoryServer(const MemoryID& id);
+ [[deprecated("Use memoryNameSystem member instead of function inherited by MemoryNameSystemComponentPluginUser.")]]
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName);
+
+
+ public:
+
+ /// Only valid when enabled.
+ MemoryNameSystem memoryNameSystem;
+
+ bool memoryNameSystemEnabled = true;
+ std::string memoryNameSystemName = "MemoryNameSystem";
+
+
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index b22c880013ad594acad439f3941eaff324763287..4f5a87bdccfc84458210d0811b02d99275b54ae4 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -9,8 +9,16 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+namespace armarx::armem::client::query
+{
+ // #include <RobotAPI/libraries/armem/client/query/Builder.h>
+ class Builder;
+}
+
namespace armarx::armem::client
{
+ using QueryBuilder = query::Builder;
+
/**
* @brief An update of an entity for a specific point in time.
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 011948cc29860ed26b63dfdf93e59358c8180e08..a589512f13bcdaaa2536f597a54474eb72f96574 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -1,7 +1,16 @@
#include "Reader.h"
+#include <sstream>
+
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
#include "query/Builder.h"
#include "query/query_fns.h"
@@ -20,6 +29,7 @@ namespace armarx::armem::client
return QueryResult::fromIce(query(input.toIce()));
}
+
armem::query::data::Result
Reader::query(const armem::query::data::Input& input) const
{
@@ -50,11 +60,13 @@ namespace armarx::armem::client
return result;
}
+
QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, DataMode dataMode) const
{
return this->query(armem::query::data::MemoryQuerySeq{query}, dataMode);
}
+
QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode) const
{
QueryInput input;
@@ -63,79 +75,161 @@ namespace armarx::armem::client
return this->query(input);
}
- QueryResult Reader::getAll(DataMode dataMode) const
- {
- using namespace client::query_fns;
-
- query::Builder qb(dataMode);
- qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
- return this->query(qb.buildQueryInput());
+ QueryResult Reader::query(const QueryBuilder& queryBuilder) const
+ {
+ return this->query(queryBuilder.buildQueryInput());
}
- QueryResult Reader::getLatestSnapshots(DataMode dataMode) const
+
+ QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode) const
{
using namespace client::query_fns;
query::Builder qb(dataMode);
- qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
+ for (const MemoryID& id : ids)
+ {
+ query::EntitySelector& entity = qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
- return this->query(qb.buildQueryInput());
+ if (id.hasTimestamp())
+ {
+ entity.snapshots(withID(id));
+ }
+ else
+ {
+ entity.snapshots(latest());
+ }
+ }
+ return query(qb);
}
- void
- Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
+
+
+ std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
{
- for (const auto& [subscription, callbacks] : this->callbacks)
- {
- std::vector<MemoryID> matchingSnapshotIDs;
+ std::vector<MemoryID> snapshotIDs = _snapshotIDs;
- for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+ client::QueryResult result = this->queryMemoryIDs(snapshotIDs);
+ if (result.success)
+ {
+ std::sort(snapshotIDs.begin(), snapshotIDs.end(), compareTimestampDecreasing);
+ for (const MemoryID& snapshotID : snapshotIDs)
{
- if (contains(subscription, updatedSnapshotID))
+ try
{
- matchingSnapshotIDs.push_back(updatedSnapshotID);
+ wm::EntitySnapshot& snapshot = result.memory.getEntitySnapshot(snapshotID);
+ return snapshot;
}
- }
-
- if (not matchingSnapshotIDs.empty())
- {
- for (auto& callback : callbacks)
+ catch (const armem::error::ArMemError&)
{
- callback(subscription, matchingSnapshotIDs);
}
}
+ return std::nullopt;
+ }
+ else
+ {
+ ARMARX_INFO << "Error querying " << snapshotIDs.size() << " STT snapshots:\n"
+ << result.errorMessage;
+ return std::nullopt;
}
}
- data::StoreResult
- Reader::readAndStore(const data::StoreInput& input) const
+
+ QueryResult Reader::getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode) const
{
- server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
- if (storingMemoryPrx)
+ using namespace client::query_fns;
+ if (!id.isWellDefined())
{
- return storingMemoryPrx->store(input);
+ throw armem::error::InvalidMemoryID(id, "ID must be well defined, but was not.");
+ }
+
+ query::Builder qb(dataMode);
+ query::CoreSegmentSelector& core =
+ id.hasCoreSegmentName()
+ ? qb.coreSegments(withID(id))
+ : qb.coreSegments(all());
+ query::ProviderSegmentSelector& prov =
+ id.hasProviderSegmentName()
+ ? core.providerSegments(withID(id))
+ : core.providerSegments(all());
+ query::EntitySelector& entity =
+ id.hasEntityName()
+ ? prov.entities(withID(id))
+ : prov.entities(all());
+ entity.snapshots(latest());
+
+ return query(qb);
+ }
+
+
+ struct FindLatestSnapshotVisitor : public wm::Visitor
+ {
+ std::optional<wm::EntitySnapshot> latest = std::nullopt;
+
+ bool visitEnter(const wm::EntitySnapshot& snapshot) override;
+ };
+ bool FindLatestSnapshotVisitor::visitEnter(const wm::EntitySnapshot& snapshot)
+ {
+ if (not latest.has_value() or snapshot.time() < latest->time())
+ {
+ latest = snapshot;
+ }
+ return true;
+ }
+
+
+ std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotIn(const MemoryID& id, DataMode dataMode) const
+ {
+ client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
+ if (result.success)
+ {
+ FindLatestSnapshotVisitor visitor;
+ visitor.applyTo(result.memory);
+ return visitor.latest;
}
else
{
- ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
- return {};
+ ARMARX_INFO << "Error querying latest snapshot in " << id;
+ return std::nullopt;
}
}
- void
- Reader::subscribe(const MemoryID& id, callback callback)
+ QueryResult Reader::getAllLatestSnapshots(DataMode dataMode) const
+ {
+ using namespace client::query_fns;
+
+ query::Builder qb(dataMode);
+ qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(latest());
+
+ return this->query(qb);
+ }
+
+
+ QueryResult Reader::getAll(DataMode dataMode) const
{
- callbacks[id].push_back(callback);
+ using namespace client::query_fns;
+
+ query::Builder qb(dataMode);
+ qb.coreSegments(all()).providerSegments(all()).entities(all()).snapshots(all());
+
+ return this->query(qb);
}
- void Reader::subscribe(const MemoryID& subscriptionID, callback_updated_only callback)
+
+ data::StoreResult
+ Reader::readAndStore(const data::StoreInput& input) const
{
- subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ server::StoringMemoryInterfacePrx storingMemoryPrx = server::StoringMemoryInterfacePrx::checkedCast(memoryPrx);
+ if (storingMemoryPrx)
+ {
+ return storingMemoryPrx->store(input);
+ }
+ else
{
- callback(updatedSnapshotIDs);
- });
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ return {};
+ }
}
@@ -144,5 +238,4 @@ namespace armarx::armem::client
{
this->memoryPrx = memory;
}
-
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 5cbeff84a3b6f979cd39d87b8271ff7e81d0c01b..1d69491d2cf33e7bdf460634ef69bc26db4bb52b 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,17 +2,14 @@
// STD/STL
-#include <functional>
-#include <unordered_map>
+#include <optional>
#include <vector>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/StoringMemoryInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h>
+// #include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include "Query.h"
@@ -26,15 +23,6 @@ namespace armarx::armem::client
class Reader
{
- using callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
- using callback_updated_only = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
-
- template <class CalleeT>
- using member_callback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
- template <class CalleeT>
- using member_callback_updated_only = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
-
-
public:
/**
@@ -45,47 +33,83 @@ namespace armarx::armem::client
void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
+ /// Perform a query.
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
QueryResult query(armem::query::data::MemoryQueryPtr query, DataMode dataMode = DataMode::WithData) const;
QueryResult query(const armem::query::data::MemoryQuerySeq& queries, DataMode dataMode = DataMode::WithData) const;
+ QueryResult query(const QueryBuilder& queryBuilder) const;
- QueryResult getAll(DataMode dataMode = DataMode::WithData) const;
- QueryResult getLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
- //data::StoreResult readAndStore(data::StoreInputSeq& input);
- data::StoreResult readAndStore(const data::StoreInput& input) const;
+ /**
+ * @brief Qeury a specific set of memory IDs.
+ *
+ * Each ID can refer to an entity, a snapshot or an instance. When not
+ * referring to an entity instance, the latest snapshot and first
+ * instance will be queried, respectively.
+ *
+ * All memory IDs must refer to the memory this reader is reading from.
+ * If an ID refers to another memory, the query will not find it and it
+ * will not be part of the result.
+ *
+ * @param ids The entity, snapshot or instance IDs.
+ * @param dataMode Whether to include instance data or just meta data.
+ * @return The query result.
+ */
+ QueryResult
+ queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode = DataMode::WithData) const;
+
- void subscribe(const MemoryID& subscriptionID, callback callback);
- void subscribe(const MemoryID& subscriptionID, callback_updated_only callback);
/**
- * Subscribe with a class member function:
- * @code
- * reader.subscribe(entityID, this, &This::myCallback);
- * @endcode
+ * @brief Query the given snapshot and return the latest existing snapshot.
+ * @param snapshotIDs The snapshot (or entity) IDs.
+ * @return The latest snapshot contained in the query result, if any.
*/
- template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback<CalleeT> callback)
- {
- auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(subscriptionID, updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
- }
- template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback_updated_only<CalleeT> callback)
- {
- auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
- }
- /// Function handling updates from the MemoryListener ice topic.
- void updated(const std::vector<MemoryID>& updatedIDs) const;
+ std::optional<wm::EntitySnapshot>
+ getLatestSnapshotOf(const std::vector<MemoryID>& snapshotIDs) const;
+
+
+ /**
+ * @brief Get the latest snapshots under the given memory ID.
+ * @param id A memory, core segment, provider segment or entity ID.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getLatestSnapshotsIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+ /**
+ * @brief Get the latest snapshot under the given memory ID.
+ * @param id A memory, core segment, provider segment or entity ID.
+ * @param dataMode With or without data.
+ * @return The latest contained snapshot, if any.
+ */
+ std::optional<wm::EntitySnapshot>
+ getLatestSnapshotIn(const MemoryID& id, DataMode dataMode = DataMode::WithData) const;
+
+
+ /**
+ * @brief Get all latest snapshots in the memory.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getAllLatestSnapshots(DataMode dataMode = DataMode::WithData) const;
+
+
+ /**
+ * @brief Get the whole memory content.
+ * @param dataMode With or without data.
+ * @return The query result.
+ */
+ QueryResult
+ getAll(DataMode dataMode = DataMode::WithData) const;
+
+
+ //data::StoreResult readAndStore(data::StoreInputSeq& input);
+ data::StoreResult readAndStore(const data::StoreInput& input) const;
inline operator bool() const
@@ -97,10 +121,6 @@ namespace armarx::armem::client
server::ReadingMemoryInterfacePrx memoryPrx;
- private:
-
- std::unordered_map<MemoryID, std::vector<callback>> callbacks;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
index 2e73e852b9ec168e1e4442c00f7668b5cabb9f64..3204c83cc094f3895e4ace10a4b91c9cdfeb1070 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.cpp
@@ -10,16 +10,16 @@
namespace armarx::armem::client::plugins
{
- ReaderComponentPlugin::~ReaderComponentPlugin()
+ ReaderComponentPlugin::ReaderComponentPlugin(ManagedIceObject& parent, std::string pre) :
+ ComponentPlugin(parent, pre)
{
- // pass
+ addPlugin(mnsPlugin, pre);
+ addPluginDependency(mnsPlugin);
}
- void
- ReaderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+
+ ReaderComponentPlugin::~ReaderComponentPlugin()
{
- ClientPlugin::postCreatePropertyDefinitions(properties);
- properties->topic<MemoryListenerInterface>("MemoryUpdates");
}
}
@@ -35,32 +35,22 @@ namespace armarx::armem::client
ReaderComponentPluginUser::~ReaderComponentPluginUser()
{
- // pass
- }
-
-
- void
- ReaderComponentPluginUser::memoryUpdated(
- const std::vector<data::MemoryID>& updatedSnapshotIDsIce, const Ice::Current&)
- {
- std::vector<MemoryID> updatedSnapshotIDs;
- fromIce(updatedSnapshotIDsIce, updatedSnapshotIDs);
- memoryReader.updated(updatedSnapshotIDs);
}
void
- ReaderComponentPluginUser::setReadingMemory(server::ReadingMemoryInterfacePrx memory)
+ ReaderComponentPluginUser::setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory)
{
memoryReader.setReadingMemory(memory);
}
- armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemory(const std::string& memoryName)
+
+ armem::data::WaitForMemoryResult ReaderComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setReadingMemory(result.proxy);
+ setReadingMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 8c7858bfc6e5c14ca5f756512d000928f14b8502..0a2e37ba953e024858090cc64d6510dfebfd8a77 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -11,25 +11,26 @@
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/mns/ClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
namespace armarx::armem::client::plugins
{
- class ReaderComponentPlugin :
- public mns::plugins::ClientPlugin
+ class ReaderComponentPlugin : public ComponentPlugin
{
public:
- using mns::plugins::ClientPlugin::ClientPlugin;
+ ReaderComponentPlugin(ManagedIceObject& parent, std::string pre);
~ReaderComponentPlugin() override;
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
};
@@ -41,8 +42,7 @@ namespace armarx::armem::client
class ReaderComponentPluginUser :
virtual public ManagedIceObject,
- virtual public mns::plugins::ClientPluginUserBase,
- virtual public MemoryListenerInterface
+ virtual public MemoryNameSystemComponentPluginUser
{
public:
@@ -50,18 +50,14 @@ namespace armarx::armem::client
ReaderComponentPluginUser();
~ReaderComponentPluginUser() override;
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ [[deprecated("Use memoryNameSystem member instead of function inherited by ReaderComponentPluginUser.")]]
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
- virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
protected:
- void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
-
-
-
- protected:
+ void setReadingMemoryServer(server::ReadingMemoryInterfacePrx memory);
Reader memoryReader;
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index 93dbb39ae33d6e614fb0f4e95de58e0522a99b20..2021f3a189367dc9134d60ce1dd597cb8513dfaf 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -41,21 +41,14 @@ namespace armarx::armem::client
}
- CommitResult Writer::commit(const Commit& _commit)
+ CommitResult Writer::commit(const Commit& commit)
{
ARMARX_CHECK_NOT_NULL(memory);
- Commit commit = _commit;
-
- Time timeSent = armem::Time::now();
- for (EntityUpdate& update : commit.updates)
- {
- update.timeSent = timeSent;
- }
data::Commit commitIce;
toIce(commitIce, commit);
- data::CommitResult resultIce = this->commit(commitIce);
+ data::CommitResult resultIce = this->_commit(commitIce);
armem::CommitResult result;
fromIce(resultIce, result);
@@ -64,10 +57,60 @@ namespace armarx::armem::client
}
- data::CommitResult Writer::commit(const data::Commit& commit)
+ data::CommitResult Writer::commit(const data::Commit& _commit)
+ {
+ data::Commit commit = _commit;
+ return this->_commit(commit);
+ }
+
+
+ EntityUpdateResult Writer::commit(const EntityUpdate& update)
+ {
+ armem::Commit commit;
+ commit.updates.push_back(update);
+
+ armem::CommitResult result = this->commit(commit);
+ ARMARX_CHECK_EQUAL(result.results.size(), 1);
+ return result.results.at(0);
+ }
+
+ EntityUpdateResult Writer::commit(
+ const MemoryID& entityID,
+ const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
+ Time timeCreated)
+ {
+ EntityUpdate update;
+ update.entityID = entityID;
+ update.instancesData = instancesData;
+ update.timeCreated = timeCreated;
+ return commit(update);
+ }
+
+ void
+ Writer::setWritingMemory(server::WritingMemoryInterfacePrx memory)
+ {
+ this->memory = memory;
+ }
+
+ data::CommitResult Writer::_commit(data::Commit& commit)
{
ARMARX_CHECK_NOT_NULL(memory);
+ /*
+ * This function sets the `timeSent` of each `EntityUpdate` before
+ * sending the data. To allow that, `commit` needs to bo non-const.
+ * Otherwise, the function would need to make a copy of `commit`,
+ * which is not necessary in all cases; e.g. when called by
+ * `commit(Const Commit&)`, which converts the commit to ice types
+ * anyway.
+ */
+
+ const Time timeSent = armem::Time::now();
+ for (data::EntityUpdate& update : commit.updates)
+ {
+ update.timeSentMicroSeconds = timeSent.toMicroSeconds();
+ }
+
data::CommitResult result;
auto handleError = [&commit, &result](const std::string & what)
{
@@ -99,35 +142,6 @@ namespace armarx::armem::client
return result;
}
-
-
- EntityUpdateResult Writer::commit(const EntityUpdate& update)
- {
- armem::Commit commit;
- commit.updates.push_back(update);
-
- armem::CommitResult result = this->commit(commit);
- ARMARX_CHECK_EQUAL(result.results.size(), 1);
- return result.results.at(0);
- }
-
- EntityUpdateResult Writer::commit(
- const MemoryID& entityID,
- const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
- Time timeCreated)
- {
- EntityUpdate update;
- update.entityID = entityID;
- update.instancesData = instancesData;
- update.timeCreated = timeCreated;
- return commit(update);
- }
-
- void
- Writer::setWritingMemory(server::WritingMemoryInterfacePrx memory)
- {
- this->memory = memory;
- }
}
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 9c6e8851e4fe3792ad728e90eaa9319a88568e0c..dd953fa368d9a46363677d3bc39bc71f8da2a9ce 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -50,6 +50,10 @@ namespace armarx::armem::client
const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
Time timeCreated);
+ // with bare-ice types
+ data::CommitResult commit(const data::Commit& commit);
+
+
void setWritingMemory(server::WritingMemoryInterfacePrx memory);
operator bool() const
@@ -59,7 +63,9 @@ namespace armarx::armem::client
private:
- data::CommitResult commit(const data::Commit& commit);
+ /// Sets `timeSent` on all entity updates and performs the commit,
+ data::CommitResult _commit(data::Commit& commit);
+
public:
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
index 04014c77a338c1f6fc836aa8ea1f64f92feb42e9..093f29e6fa2178f19ae13983e21772f17400856d 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.cpp
@@ -10,9 +10,15 @@
namespace armarx::armem::client::plugins
{
+ WriterComponentPlugin::WriterComponentPlugin(ManagedIceObject& parent, std::string pre) :
+ ComponentPlugin(parent, pre)
+ {
+ addPlugin(mnsPlugin, pre);
+ addPluginDependency(mnsPlugin);
+ }
+
WriterComponentPlugin::~WriterComponentPlugin()
{
- // pass
}
}
@@ -32,17 +38,17 @@ namespace armarx::armem::client
void
- WriterComponentPluginUser::setWritingMemory(server::WritingMemoryInterfacePrx memory)
+ WriterComponentPluginUser::setWritingMemoryServer(server::WritingMemoryInterfacePrx memory)
{
memoryWriter.setWritingMemory(memory);
}
- armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemory(const std::string& memoryName)
+ armem::data::WaitForMemoryResult WriterComponentPluginUser::useMemoryServer(const std::string& memoryName)
{
- armem::data::WaitForMemoryResult result = mns::plugins::ClientPluginUserBase::useMemory(memoryName);
+ armem::data::WaitForMemoryResult result = MemoryNameSystemComponentPluginUser::useMemoryServer(memoryName);
if (result.proxy)
{
- setWritingMemory(result.proxy);
+ setWritingMemoryServer(result.proxy);
}
return result;
}
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 0f5f21f808fb646fe8af27192d04c82a5739426e..435881b48b27b9053dd5da5e87b794d3561f3b3c 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -10,20 +10,24 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/mns/ClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
namespace armarx::armem::client::plugins
{
- class WriterComponentPlugin : public mns::plugins::ClientPlugin
+ class WriterComponentPlugin : public ComponentPlugin
{
public:
- using mns::plugins::ClientPlugin::ClientPlugin;
+ WriterComponentPlugin(ManagedIceObject& parent, std::string pre);
~WriterComponentPlugin() override;
+ private:
+
+ plugins::MemoryNameSystemComponentPlugin* mnsPlugin = nullptr;
+
};
}
@@ -32,7 +36,7 @@ namespace armarx::armem::client
{
class WriterComponentPluginUser :
virtual public ManagedIceObject,
- virtual public mns::plugins::ClientPluginUserBase
+ virtual public MemoryNameSystemComponentPluginUser
{
public:
@@ -40,15 +44,17 @@ namespace armarx::armem::client
WriterComponentPluginUser();
~WriterComponentPluginUser() override;
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
+ virtual armem::data::WaitForMemoryResult useMemoryServer(const std::string& memoryName) override;
+ using MemoryNameSystemComponentPluginUser::useMemoryServer;
+
protected:
- void setWritingMemory(server::WritingMemoryInterfacePrx memory);
+ void setWritingMemoryServer(server::WritingMemoryInterfacePrx memory);
Writer memoryWriter;
+
private:
plugins::WriterComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/armem/client/query.h b/source/RobotAPI/libraries/armem/client/query.h
new file mode 100644
index 0000000000000000000000000000000000000000..99606089e06952dd2f5006b628c69f3644acd674
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/query.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#include "Query.h"
+#include "query/Builder.h"
+#include "query/query_fns.h"
+#include "query/selectors.h"
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c491c11f4cc4f223d940a60ef79ab577a428b6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -0,0 +1,87 @@
+#include "MemoryListener.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ MemoryListener::MemoryListener()
+ {
+ }
+
+
+ void
+ MemoryListener::updated(const std::vector<data::MemoryID>& updatedSnapshotIDs) const
+ {
+ std::vector<MemoryID> bos;
+ fromIce(updatedSnapshotIDs, bos);
+ updated(bos);
+ }
+
+
+ void
+ MemoryListener::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
+ {
+ std::stringstream error;
+
+ for (const auto& [subscription, callbacks] : this->callbacks)
+ {
+ std::vector<MemoryID> matchingSnapshotIDs;
+
+ for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+ {
+ try
+ {
+ if (contains(subscription, updatedSnapshotID))
+ {
+ matchingSnapshotIDs.push_back(updatedSnapshotID);
+ }
+ }
+ catch (const armem::error::InvalidMemoryID& e)
+ {
+ // Log to debug, but ignore otherwise
+ error << "Error when comparing subscribed ID " << subscription
+ << " with updated ID " << updatedSnapshotID << ":\n"
+ << e.what() << "\n\n";
+ }
+ }
+
+ if (not matchingSnapshotIDs.empty())
+ {
+ for (auto& callback : callbacks)
+ {
+ callback(subscription, matchingSnapshotIDs);
+ }
+ }
+ }
+ if (error.str().size() > 0)
+ {
+ ARMARX_VERBOSE << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
+ << error.str();
+ }
+ }
+
+
+ void
+ MemoryListener::subscribe(const MemoryID& id, Callback callback)
+ {
+ callbacks[id].push_back(callback);
+ }
+
+
+ void
+ MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ {
+ subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ callback(updatedSnapshotIDs);
+ });
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d530097ce6394ddcf4cc815f8e44f93e057d916
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -0,0 +1,78 @@
+#pragma once
+
+
+// STD/STL
+#include <functional>
+#include <unordered_map>
+#include <vector>
+
+// RobotAPI
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ /**
+ * @brief Handles update signals from the memory system and distributes it
+ * to its subsribers.
+ */
+ class MemoryListener
+ {
+
+ using Callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using CallbackUpdatedOnly = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+
+ template <class CalleeT>
+ using MemberCallback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ template <class CalleeT>
+ using MemberCallbackUpdatedOnly = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+
+
+ public:
+
+ MemoryListener();
+
+
+ void subscribe(const MemoryID& subscriptionID, Callback Callback);
+ void subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
+
+ /**
+ * Subscribe with a class member function:
+ * @code
+ * listener.subscribe(entityID, this, &This::myCallback);
+ * @endcode
+ */
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(subscriptionID, updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
+
+
+ /// Function handling updates from the MemoryListener ice topic.
+ void updated(const std::vector<MemoryID>& updatedIDs) const;
+ void updated(const std::vector<data::MemoryID>& updatedIDs) const;
+
+
+ protected:
+
+ std::unordered_map<MemoryID, std::vector<Callback>> callbacks;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce25109502b1da09cb01399396a5c7bca92ed50f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -0,0 +1,63 @@
+#include "SimpleReaderBase.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::util
+{
+
+ SimpleReaderBase::SimpleReaderBase(MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+
+ void SimpleReaderBase::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+ }
+
+
+ void SimpleReaderBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName
+ << "' ...";
+ try
+ {
+ memoryReaderClient = memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+
+ std::mutex& SimpleReaderBase::memoryReaderMutex()
+ {
+ return memoryMutex;
+ }
+
+
+ const armem::client::Reader& SimpleReaderBase::memoryReader() const
+ {
+ return memoryReaderClient;
+ }
+
+
+ const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+ {
+ return props;
+ }
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a78db401d3f578d7be9f9c86542a37f72326aabc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -0,0 +1,78 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleReaderBase
+ {
+ public:
+
+ SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
+ virtual ~SimpleReaderBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ protected:
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ std::mutex& memoryReaderMutex();
+ const armem::client::Reader& memoryReader() const;
+
+
+ private:
+
+ Properties props;
+
+ armem::client::Reader memoryReaderClient;
+ std::mutex memoryMutex;
+
+ MemoryNameSystem& memoryNameSystem;
+ };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca399bf8f64a2fd347d17c31587d33639cded032
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -0,0 +1,68 @@
+#include "SimpleWriterBase.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem::client::util
+{
+ SimpleWriterBase::SimpleWriterBase(MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+
+ void
+ SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix();
+
+ props = defaultProperties();
+
+ def->optional(props.memoryName, prefix + "Memory");
+ def->optional(props.coreSegmentName, prefix + "CoreSegment");
+
+ def->required(props.providerName,
+ prefix + "Provider",
+ "Name of this provider");
+ }
+
+
+ void SimpleWriterBase::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
+ << props.memoryName << "' ...";
+ try
+ {
+ memoryWriterClient = memoryNameSystem.useWriter(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+
+ std::mutex& SimpleWriterBase::memoryWriterMutex()
+ {
+ return memoryMutex;
+ }
+
+
+ armem::client::Writer& SimpleWriterBase::memoryWriter()
+ {
+ return memoryWriterClient;
+ }
+
+
+ const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+ {
+ return props;
+ }
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..1413299a3081f36dfaa83cce425c50994e7018c2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -0,0 +1,79 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
+
+namespace armarx::armem::client::util
+{
+
+ class SimpleWriterBase
+ {
+ public:
+
+ SimpleWriterBase(MemoryNameSystem& memoryNameSystem);
+ virtual ~SimpleWriterBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+
+ protected:
+
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ std::string providerName = ""; // required property
+ };
+
+ const Properties& properties() const;
+
+ virtual std::string propertyPrefix() const = 0;
+ virtual Properties defaultProperties() const = 0;
+
+ std::mutex& memoryWriterMutex();
+ armem::client::Writer& memoryWriter();
+
+
+ private:
+
+ Properties props;
+
+ armem::client::Writer memoryWriterClient;
+ std::mutex memoryMutex;
+
+ MemoryNameSystem& memoryNameSystem;
+
+ };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index 61dd4836ca9f16ffd6b36342facbd2f610c75ff3..f8d7c832401a4ebf335347c67745c1ce82af4e03 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -80,6 +80,23 @@ namespace armarx::armem
}
+ MemoryID::MemoryID(
+ const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ const std::string& entityName,
+ Time timestamp,
+ int instanceIndex) :
+ memoryName(memoryName),
+ coreSegmentName(coreSegmentName),
+ providerSegmentName(providerSegmentName),
+ entityName(entityName),
+ timestamp(timestamp),
+ instanceIndex(instanceIndex)
+ {
+ }
+
+
std::string MemoryID::str(bool escapeDelimiters) const
{
return str(delimiter, escapeDelimiters);
@@ -418,13 +435,13 @@ namespace armarx::armem
if (!general.isWellDefined())
{
std::stringstream ss;
- ss << "ID `general` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nGeneral ID " << general << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(general, ss.str());
}
if (!specific.isWellDefined())
{
std::stringstream ss;
- ss << "ID `specific` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(specific, ss.str());
}
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 7ddf2130801767eabb60c01caba1d2dc427caba3..f9d3d5b58ca4670a8abb4333e417a9a2109843ae 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -63,6 +63,13 @@ namespace armarx::armem
/// (Re-)Construct a memory ID from a string representation as returned by `str()`.
explicit MemoryID(const std::string& string);
+ MemoryID(const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName = "",
+ const std::string& entityName = "",
+ Time timestamp = Time::microSeconds(-1),
+ int instanceIndex = -1);
+
/**
* @brief Indicate whether this ID is well-defined.
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a1e8aa17bf66356061696eee972e587eef96e61
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
@@ -0,0 +1,26 @@
+#include "MemoryID_operators.h"
+
+#include "MemoryID.h"
+
+
+std::ostream& armarx::armem::operator<<(std::ostream& os, const std::vector<MemoryID>& rhs)
+{
+ os << "std::vector<MemoryID> with " << rhs.size() << " entries:";
+ for (size_t i = 0; i < rhs.size(); ++i)
+ {
+ os << "\n\t[" << i << "] " << rhs[i];
+ }
+ return os;
+}
+
+
+bool armarx::armem::compareTimestamp(const MemoryID& lhs, const MemoryID& rhs)
+{
+ return lhs.timestamp < rhs.timestamp;
+}
+
+
+bool armarx::armem::compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs)
+{
+ return lhs.timestamp > rhs.timestamp;
+}
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.h b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
new file mode 100644
index 0000000000000000000000000000000000000000..7508c81e8ce2245e3b6924f7a706b0eff4390e7e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
@@ -0,0 +1,22 @@
+#pragma once
+
+// #include "MemoryID.h"
+
+#include <vector>
+#include <ostream>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+
+ std::ostream& operator<<(std::ostream& os, const std::vector<MemoryID>& rhs);
+
+ /// lhs.timestamp < rhs.timstamp
+ bool compareTimestamp(const MemoryID& lhs, const MemoryID& rhs);
+ /// lhs.timestamp > rhs.timstamp
+ bool compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs);
+
+}
+
+
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 3c3c0ef2c3ffa6f1020f6c3ff82dfce8e4650f5b..a6e47cafd667d2e0de624acb3fb4c15979727912 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -98,7 +98,7 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry("provider segment", name, getLevelName(), this->getKeyString());
+ throw armem::error::MissingEntry::create<ProviderSegmentT>(name, *this);
}
}
@@ -146,8 +146,10 @@ namespace armarx::armem::base
ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
return provSeg.update(update);
}
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
-
+ else
+ {
+ throw armem::error::MissingEntry::create<ProviderSegmentT>(update.entityID.providerSegmentName, *this);
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index a798cb09eec72b3a368955ebb219fe15f9308cdc..c6fefc5b18133bc7aa36797156687d2a75384f3e 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -164,7 +164,7 @@ namespace armarx::armem::base
}
else
{
- throw error::MissingEntry("entity snapshot", toDateTimeMilliSeconds(time), getLevelName(), this->id().str());
+ throw armem::error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index defdf1c049fa5274b0d112f42d78037730d9eff5..95a5be4f13c9baf4c32ce302158b78acd8cfa8c1 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -131,7 +131,7 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry(EntityInstanceT().getLevelName(), std::to_string(index), getLevelName(), toDateTimeMilliSeconds(time()));
+ throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index), *this);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 713ba580089443b9fc2ca6f982e72970aa28dde7..4a01a472015e86fd824e0867e48ff62bda19fda1 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -92,10 +92,35 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), name, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<CoreSegmentT>(name, *this);
}
}
+
+ bool hasProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ auto it = this->_container.find(providerSegmentID.coreSegmentName);
+ if (it != this->_container.end())
+ {
+ return it->second.hasProviderSegment(providerSegmentID.providerSegmentName);
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID)
+ {
+ return getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ const ProviderSegmentT& getProviderSegment(const MemoryID& providerSegmentID) const
+ {
+ return getCoreSegment(providerSegmentID.coreSegmentName).getProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+
using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
@@ -184,7 +209,7 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry(CoreSegmentT().getLevelName(), update.entityID.coreSegmentName, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<CoreSegmentT>(update.entityID.coreSegmentName, *this);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 503650498d9afb5ddcdbbf22ecb84baaae3201f8..ef8ed9fd94f3ee465dfa2d9e4df511598889b548 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -105,7 +105,7 @@ namespace armarx::armem::base
}
else
{
- throw error::MissingEntry("entity", name, getLevelName(), this->name());
+ throw armem::error::MissingEntry::create<EntityT>(name, *this);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/error.h b/source/RobotAPI/libraries/armem/core/error.h
index 36b7459745b3c0c7887151c9469b3ec54820e297..31bef6c2746eb276f23cb9102c61c295810c39d7 100644
--- a/source/RobotAPI/libraries/armem/core/error.h
+++ b/source/RobotAPI/libraries/armem/core/error.h
@@ -1,6 +1,7 @@
#pragma once
#include "error/ArMemError.h"
+#include "error/mns.h"
namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index c44a40758a73ba64d8827c6f7a751b9df8fc00c3..8936fab6fe953bc3b7fee870a97ffee598234ea7 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -4,6 +4,8 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "../MemoryID.h"
+
namespace armarx::armem::error
{
@@ -63,17 +65,19 @@ namespace armarx::armem::error
MissingEntry::MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName) :
- ArMemError(makeMsg(missingTerm, missingName, ownTerm, ownName))
+ 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& ownTerm, const std::string& ownName)
+ const std::string& containerTerm, const std::string& containerName,
+ size_t size)
{
std::stringstream ss;
ss << "No " << missingTerm << " with name '" << missingName << "' "
- << "in " << ownTerm << " '" << ownName << "'.";
+ << "in " << containerTerm << " '" << containerName << "' (with size " << size << ").";
return ss.str();
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 3456c7a350835e6c67ca1d439d2fc0c44a8be507..e6843d13be5bc07e71d90b00cf6a911ca717e524 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -1,10 +1,14 @@
#pragma once
#include <stdexcept>
+
#include <SimoxUtility/meta/type_name.h>
-#include "../MemoryID.h"
+namespace armarx::armem
+{
+ class MemoryID;
+}
namespace armarx::armem::error
{
@@ -76,11 +80,22 @@ namespace armarx::armem::error
class MissingEntry : public ArMemError
{
public:
+
+ template <class MissingT, class ContainerT>
+ static MissingEntry create(const std::string& missingKey, const ContainerT& container)
+ {
+ return MissingEntry(MissingT().getLevelName(), missingKey,
+ container.getLevelName(), container.getKeyString(), container.size());
+ }
+
+
MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ const std::string& containerTerm, const std::string& containerName,
+ size_t containerSize);
static std::string makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ const std::string& containerTerm, const std::string& containerName,
+ size_t size);
};
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.cpp b/source/RobotAPI/libraries/armem/core/error/mns.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da8b8f4855ac67e575cb1e9a677eea277dc1cc58
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/error/mns.cpp
@@ -0,0 +1,64 @@
+#include "mns.h"
+
+#include <sstream>
+
+#include "../MemoryID.h"
+
+
+namespace armarx::armem::error
+{
+
+ MemoryNameSystemQueryFailed::MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage) :
+ ArMemError(makeMsg(function, errorMessage))
+ {
+ }
+
+ std::string MemoryNameSystemQueryFailed::makeMsg(const std::string& function, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Failed to call '" << function << "' on the memory name system.\n";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+
+
+ CouldNotResolveMemoryServer::CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage) :
+ ArMemError(makeMsg(memoryID, errorMessage))
+ {
+ }
+
+ std::string CouldNotResolveMemoryServer::makeMsg(const MemoryID& memoryID, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Could not resolve the memory name '" << memoryID << "'."
+ << "\nMemory server for '" << memoryID << "' is not registered.";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+
+ ServerRegistrationOrRemovalFailed::ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage) :
+ ArMemError(makeMsg(verb, memoryID, errorMessage))
+ {
+
+ }
+
+ std::string ServerRegistrationOrRemovalFailed::makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage)
+ {
+ std::stringstream ss;
+ ss << "Failed to " << verb << " memory server for '" << memoryID << "' in the Memory Name System (MNS).";
+ if (not errorMessage.empty())
+ {
+ ss << "\n" << errorMessage;
+ }
+ return ss.str();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.h b/source/RobotAPI/libraries/armem/core/error/mns.h
new file mode 100644
index 0000000000000000000000000000000000000000..428df8cae66494f72b1a56267a97b3942f63f7a6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/error/mns.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include "ArMemError.h"
+
+
+namespace armarx::armem::error
+{
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class MemoryNameSystemQueryFailed : public ArMemError
+ {
+ public:
+
+ MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const std::string& function, const std::string& errorMessage = "");
+
+ };
+
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class CouldNotResolveMemoryServer : public ArMemError
+ {
+ public:
+
+ CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ };
+
+
+ /**
+ * @brief Indicates that a query to the Memory Name System failed.
+ */
+ class ServerRegistrationOrRemovalFailed : public ArMemError
+ {
+ public:
+
+ ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ static std::string makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
+
+ };
+
+
+}
+
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..815ee1b34908dc5916e7d909e709bc735409f63c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor.h
@@ -0,0 +1,4 @@
+#pragma once
+
+#include "visitor/FunctionalVisitor.h"
+#include "visitor/Visitor.h"
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ef26181179ae749da153ee3cec87654ae2cb632
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
@@ -0,0 +1,12 @@
+#include "FunctionalVisitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..2cd0e205727bd16a880f95ccbe59855f9225f744
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -0,0 +1,107 @@
+#pragma once
+
+#include <functional>
+
+#include "Visitor.h"
+
+
+namespace armarx::armem::wm
+{
+
+ /**
+ * @brief A `Visitor` which can be parametrized by `std::function`
+ * instead of inheriting and overriding.
+ */
+ class FunctionalVisitor : public Visitor
+ {
+ public:
+
+ FunctionalVisitor();
+ virtual ~FunctionalVisitor() override;
+
+ bool visitEnter(Memory& memory) override
+ {
+ return memoryFn ? memoryFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(Entity& entity) override
+ {
+ return entityFn ? entityFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(EntitySnapshot& snapshot) override
+ {
+ return snapshotFn ? snapshotFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(EntityInstance& instance) override
+ {
+ return instanceFn ? instanceFn(instance) : Visitor::visit(instance);
+ }
+
+
+
+ // Const versions
+
+ bool applyTo(const Memory& memory);
+ bool applyTo(const CoreSegment& coreSegment);
+ bool applyTo(const ProviderSegment& providerSegment);
+ bool applyTo(const Entity& entity);
+ bool applyTo(const EntitySnapshot& snapshot);
+ bool applyTo(const EntityInstance& instance);
+
+
+ bool visitEnter(const Memory& memory) override
+ {
+ return memoryFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
+ }
+ bool visitEnter(const CoreSegment& coreSegment) override
+ {
+ return coreSegmentFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ }
+ bool visitEnter(const ProviderSegment& providerSegment) override
+ {
+ return providerSegmentFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ }
+ bool visitEnter(const Entity& entity) override
+ {
+ return entityFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
+ }
+ bool visitEnter(const EntitySnapshot& snapshot) override
+ {
+ return memoryFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
+ }
+
+ bool visit(const EntityInstance& instance) override
+ {
+ return instanceFn ? instanceConstFn(instance) : Visitor::visit(instance);
+ }
+
+
+ std::function<bool(Memory& memory)> memoryFn;
+ std::function<bool(const Memory& memory)> memoryConstFn;
+
+ std::function<bool(CoreSegment& coreSegment)> coreSegmentFn;
+ std::function<bool(const CoreSegment& coreSegment)> coreSegmentConstFn;
+
+ std::function<bool(ProviderSegment& providerSegment)> providerSegmentFn;
+ std::function<bool(const ProviderSegment& providerSegment)> providerSegmentConstFn;
+
+ std::function<bool(Entity& entity)> entityFn;
+ std::function<bool(const Entity& entity)> entityConstFn;
+
+ std::function<bool(EntitySnapshot& snapshot)> snapshotFn;
+ std::function<bool(const EntitySnapshot& snapshot)> snapshotConstFn;
+
+ std::function<bool(EntityInstance& instance)> instanceFn;
+ std::function<bool(const EntityInstance& instance)> instanceConstFn;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
similarity index 100%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.cpp
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
similarity index 98%
rename from source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
rename to source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
index c0a3854a242c030113eeb46409c172563660294a..fa789fcf0b6876ec5d9fa0ff1a3944b4e3bf3fd3 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/Visitor.h
@@ -12,7 +12,7 @@ namespace armarx::armem::wm
/**
- * @brief A visitor for the hierarchical Memory data structure.
+ * @brief A visitor for the hierarchical memory data structure.
*/
class Visitor
{
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp b/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
deleted file mode 100644
index f7bba9445c73a7c13a3374969d631530145d9c14..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-#include "ClientPlugin.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include "../error.h"
-
-
-
-namespace armarx::armem::mns::plugins
-{
-
- ClientPlugin::~ClientPlugin()
- {}
-
-
- void ClientPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
- {
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
- {
- properties->defineOptionalProperty<std::string>(
- makePropertyName(PROPERTY_MNS_NAME_NAME),
- parent<ClientPluginUserBase>().memoryNameSystemName,
- "Name of the Memory Name System (MNS) component.");
- }
- if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
- {
- properties->defineOptionalProperty<bool>(
- makePropertyName(PROPERTY_MNS_ENABLED_NAME),
- parent<ClientPluginUserBase>().memoryNameSystemEnabled,
- "Whether to use (and depend on) the Memory Name System (MNS)."
- "\nSet to false to use this memory as a stand-alone.");
- }
- }
-
- void ClientPlugin::preOnInitComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- parent().usingProxy(getMemoryNameSystemName());
- }
- }
-
- void ClientPlugin::preOnConnectComponent()
- {
- if (isMemoryNameSystemEnabled())
- {
- parent<ClientPluginUserBase>().memoryNameSystem = getMemoryNameSystem();
- }
- }
-
- bool ClientPlugin::isMemoryNameSystemEnabled()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
- parent<ClientPluginUserBase>().memoryNameSystemEnabled;
- }
-
- std::string ClientPlugin::getMemoryNameSystemName()
- {
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
- std::string{parent<ClientPluginUserBase>().memoryNameSystemName};
- }
-
- mns::MemoryNameSystemInterfacePrx ClientPlugin::getMemoryNameSystem()
- {
- return isMemoryNameSystemEnabled() && parentDerives<Component>()
- ? parent<Component>().getProxy<MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
- : nullptr;
- }
-
- ClientPluginUserBase::~ClientPluginUserBase()
- {
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const MemoryID& id)
- {
- return useMemory(id.memoryName);
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const std::string& memoryName)
- {
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
-
- armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
- if (result.success)
- {
- if (Component* comp = dynamic_cast<Component*>(this))
- {
- // Add dependency.
- comp->usingProxy(result.proxy->ice_getIdentity().name);
- }
- }
- return result;
- }
-
- armem::data::WaitForMemoryResult ClientPluginUserBase::waitForMemory(const std::string& memoryName)
- {
- if (!memoryNameSystem)
- {
- return {};
- }
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
- return memoryNameSystem->waitForMemory(input);
- }
-
- armem::data::ResolveMemoryNameResult ClientPluginUserBase::resolveMemoryName(const std::string& memoryName)
- {
- if (!memoryNameSystem)
- {
- return {};
- }
- armem::data::ResolveMemoryNameInput input;
- input.name = memoryName;
- return memoryNameSystem->resolveMemoryName(input);
- }
-
- bool ClientPluginUserBase::isMemoryAvailable(const std::string& memoryName)
- {
- armem::data::ResolveMemoryNameResult result = resolveMemoryName(memoryName);
- return result.success && result.proxy;
- }
-
-}
-
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
deleted file mode 100644
index 4ebc9aad33392ffdf48c019882b14217638174cf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
+++ /dev/null
@@ -1,82 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/Component.h>
-
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
-
-namespace armarx::armem::mns
-{
- class ClientPluginUserBase;
-}
-
-
-namespace armarx::armem::mns::plugins
-{
- /**
- * @brief A base plugin offering optional access and dependency
- * to the Memory Name System (MNS).
- */
- class ClientPlugin : public ComponentPlugin
- {
- public:
-
- using ComponentPlugin::ComponentPlugin;
- virtual ~ClientPlugin() override;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- /**
- * @brief Indicate whether the Memory Name System (MNS) is enabled.
- */
- bool isMemoryNameSystemEnabled();
- /**
- * @brief Get the name of the MNS component.
- */
- std::string getMemoryNameSystemName();
-
- /**
- * @brief Get the MNS proxy.
- * @return The MNS proxy when MNS is enabled, nullptr when MNS is disabled.
- */
- mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
-
- public:
- static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
- static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
- };
-
-
- /**
- * @brief Base class for users of the `ClientPlugin`.
- * This is itself not a usable plugin user (hence still in the plugins namespace).
- */
- class ClientPluginUserBase
- {
- protected:
-
- virtual ~ClientPluginUserBase();
-
-
- armem::data::WaitForMemoryResult waitForMemory(const std::string& memoryName);
- armem::data::ResolveMemoryNameResult resolveMemoryName(const std::string& memoryName);
- bool isMemoryAvailable(const std::string& memoryName);
-
- armem::data::WaitForMemoryResult useMemory(const MemoryID& id);
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
-
-
- public:
-
- /// Only set when enabled.
- mns::MemoryNameSystemInterfacePrx memoryNameSystem = nullptr;
-
- bool memoryNameSystemEnabled = true;
- std::string memoryNameSystemName = "MemoryNameSystem";
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index f7046b6b6a5efd30debcc2e31e1125a3e1dc831c..41ef71d9d14729e5f272348ef6e3d633da196a33 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -2,12 +2,13 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "../error.h"
+#include <RobotAPI/libraries/armem/core/error.h>
#include "MemoryToIceAdapter.h"
//#include <RobotAPI/libraries/armem/core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h>
+
namespace armarx::armem::server::plugins
{
ComponentPlugin::~ComponentPlugin()
@@ -16,7 +17,7 @@ namespace armarx::armem::server::plugins
void ComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- ClientPlugin::postCreatePropertyDefinitions(properties);
+ MemoryNameSystemComponentPlugin::postCreatePropertyDefinitions(properties);
properties->topic(memoryListener, this->parent<ComponentPluginUser>().memoryListenerDefaultName);
properties->optional(this->parent<ComponentPluginUser>().longtermMemory.dbsettings.host, "Longtermmemoryhost");
@@ -54,19 +55,22 @@ namespace armarx::armem::server::plugins
data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
{
- data::RegisterMemoryInput input;
- input.name = parent.workingMemory.name();
- input.proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
- ARMARX_CHECK_NOT_NULL(input.proxy);
- data::RegisterMemoryResult result = parent.memoryNameSystem->registerMemory(input);
- if (result.success)
+ MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
+ MemoryInterfacePrx proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
+ ARMARX_CHECK_NOT_NULL(proxy);
+
+ data::RegisterMemoryResult result;
+ try
{
- ARMARX_DEBUG << "Registered memory '" << input.name << "' in the Memory Name System (MNS).";
+ parent.memoryNameSystem.registerServer(id, proxy);
+ result.success = true;
+ ARMARX_DEBUG << "Registered memory server for " << id << " in the Memory Name System (MNS).";
}
- else
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
{
- ARMARX_WARNING << "Failed to register this memory in the Memory Name System (MNS):"
- << "\n" << result.errorMessage;
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
}
return result;
}
@@ -74,21 +78,20 @@ namespace armarx::armem::server::plugins
data::RemoveMemoryResult ComponentPlugin::removeMemory(ComponentPluginUser& parent)
{
+ MemoryID id = MemoryID().withMemoryName(parent.workingMemory.name());
+
data::RemoveMemoryResult result;
try
{
- data::RemoveMemoryInput input;
- input.name = parent.workingMemory.name();
- result = parent.memoryNameSystem->removeMemory(input);
- if (result.success)
- {
- ARMARX_DEBUG << "Removed memory '" << input.name << "' from the Memory Name System (MNS).";
- }
- else
- {
- ARMARX_WARNING << "Failed to remove this memory in the Memory Name System (MNS):"
- << "\n" << result.errorMessage;
- }
+ parent.memoryNameSystem.removeServer(id);
+ result.success = true;
+ ARMARX_DEBUG << "Removed memory server for " << id << " from the Memory Name System (MNS).";
+ }
+ catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ ARMARX_WARNING << e.what();
}
catch (const Ice::NotRegisteredException&)
{
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index 4262ee9eba64b29519612cd57505dd9fbe2cc9d1..4ed6c107e6c1f29144ed2ed8ede7e01a3b4ed710 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -8,10 +8,10 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/longtermmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
-#include "../mns/ClientPlugin.h"
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
#include "MemoryToIceAdapter.h"
@@ -24,11 +24,12 @@ namespace armarx::armem::server
namespace armarx::armem::server::plugins
{
- class ComponentPlugin : public mns::plugins::ClientPlugin
+ class ComponentPlugin : public client::plugins::MemoryNameSystemComponentPlugin
{
+ using Base = client::plugins::MemoryNameSystemComponentPlugin;
public:
- using ClientPlugin::ClientPlugin;
+ using Base::MemoryNameSystemComponentPlugin;
virtual ~ComponentPlugin() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
@@ -67,7 +68,7 @@ namespace armarx::armem::server
class ComponentPluginUser :
virtual public ManagedIceObject
, virtual public MemoryInterface
- , virtual public mns::plugins::ClientPluginUserBase
+ , virtual public client::MemoryNameSystemComponentPluginUser
{
public:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 2a4308ebdc837255a9f6a816dc62880dc09e8c21..b96b22e8921c8a9a51190e7ec8828a9416526c86 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -96,12 +96,16 @@ namespace armarx::armem::base::query_proc
}
catch (const armem::error::EntityHistoryEmpty&)
{
- // Leave empty.
+ if (false)
+ {
+ ARMARX_IMPORTANT << "Failed to retrieve latest snapshot from entity " << entity.id() << ". "
+ << "Entity has not timestamps.";
+ }
}
}
else
{
- Time time = fromIce<Time>(query.timestamp);
+ const Time time = fromIce<Time>(query.timestamp);
try
{
addResultSnapshot(result, entity.getSnapshot(time));
@@ -109,6 +113,19 @@ namespace armarx::armem::base::query_proc
catch (const armem::error::MissingEntry&)
{
// Leave empty.
+ if (false)
+ {
+ std::stringstream ss;
+ ss << "Failed to retrieve snapshot with timestamp "
+ << armem::toDateTimeMilliSeconds(time)
+ << " from entity " << entity.id() << ".\n"
+ << "Entity has timestamps: ";
+ for (const Time& t : entity.getTimestamps())
+ {
+ ss << "\n- " << armem::toDateTimeMilliSeconds(t);
+ }
+ ARMARX_IMPORTANT << ss.str();
+ }
}
}
}
@@ -316,11 +333,11 @@ namespace armarx::armem::base::query_proc
ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
return;
- }
+ }
// -> now one or both are valid ...
// is 'before' a perfect match?
- if(isPerfectMatch)
+ if (isPerfectMatch)
{
addResultSnapshot(result, beforeOrAt);
return;
@@ -357,7 +374,7 @@ namespace armarx::armem::base::query_proc
{
addResultSnapshot(result, after);
}
-
+
}
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index d381a529fb0bc17c6fc74447a46694b89512d1ef..696c6db777d8512d49168682c8a368243f2b930d 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -14,16 +14,19 @@ set(LIBRARIES
)
set(SOURCES
- MemoryViewer.cpp
- PeriodicUpdateWidget.cpp
- MemoryControlWidget.cpp
gui_utils.cpp
lifecycle.cpp
+ MemoryControlWidget.cpp
+ MemoryViewer.cpp
+ PeriodicUpdateWidget.cpp
+
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
+ instance/MemoryIDTreeWidgetItem.cpp
+ instance/WidgetsWithToolbar.cpp
instance/sanitize_typename.cpp
instance/serialize_path.cpp
@@ -46,17 +49,23 @@ set(SOURCES
query_widgets/SnapshotSelectorWidget.cpp
)
set(HEADERS
+
+ gui_utils.h
+ lifecycle.h
+
+ MemoryControlWidget.h
MemoryViewer.h
+ PeriodicUpdateWidget.h
+ TreeWidgetBuilder.h
PeriodicUpdateWidget.h
- MemoryControlWidget.h
TreeWidgetBuilder.h
- gui_utils.h
- lifecycle.h
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
+ instance/MemoryIDTreeWidgetItem.h
+ instance/WidgetsWithToolbar.h
instance/sanitize_typename.h
instance/serialize_path.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index e66191fc6896eec1c7b23a10290285ea4b01c943..6d711ac1dfee696b069a0408a001f5d98a08f2cd 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -14,9 +14,10 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <QBoxLayout>
-#include <QDialog>
#include <QCheckBox>
+#include <QDialog>
#include <QGroupBox>
+#include <QMenu>
#include <QLabel>
#include <QLayout>
#include <QSettings>
@@ -39,6 +40,18 @@ namespace armarx::armem::gui
this->statusLabel = statusLabel;
this->statusLabel->clear();
+ statusLabel->setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(statusLabel, &QLabel::customContextMenuRequested, [statusLabel](const QPoint & pos)
+ {
+ QMenu menu(statusLabel);
+ menu.addAction("Clear status", [statusLabel]()
+ {
+ statusLabel->clear();
+ });
+ menu.exec(statusLabel->mapToGlobal(pos));
+ });
+
+
// Update timer
this->updateWidgetLayout = updateWidgetLayout;
updateWidget = new armem::gui::PeriodicUpdateWidget(2.0, 60);
@@ -77,6 +90,7 @@ namespace armarx::armem::gui
connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::updated, this, &This::memoryTreeUpdated);
connect(instanceGroup, &armem::gui::InstanceGroupBox::viewUpdated, this, &This::instanceTreeUpdated);
+ connect(instanceGroup->view, &armem::gui::InstanceView::memoryIdResolutionRequested, this, &This::resolveMemoryID);
}
void MemoryViewer::setLogTag(const std::string& tag)
@@ -102,13 +116,12 @@ namespace armarx::armem::gui
{
if (!mnsName.empty())
{
- component.getProxy(mns, mnsName);
- auto res = mns->getAllRegisteredMemories();
- for (auto& [name, proxy] : res.proxies)
- {
- armem::client::Reader memoryReader{proxy};
- memoryReaders[name] = memoryReader;
- }
+ armem::mns::MemoryNameSystemInterfacePrx mnsProxy;
+ component.getProxy(mnsProxy, mnsName);
+ mns = client::MemoryNameSystem(mnsProxy);
+
+ const bool update = true;
+ memoryReaders = mns.getAllReaders(update);
}
// DebugObserver is optional (check for null on every call)
if (!debugObserverName.empty())
@@ -127,6 +140,27 @@ namespace armarx::armem::gui
emit disconnected();
}
+ const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
+ {
+ auto it = memoryData.find(memoryName);
+ if (it == memoryData.end())
+ {
+ std::stringstream ss;
+ ss << "Memory name '" << memoryName << "' is unknown. Known are: "
+ << simox::alg::get_keys(memoryData);
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ return nullptr;
+ }
+ else if (not it->second.has_value())
+ {
+ return nullptr;
+ }
+ else
+ {
+ return &it->second.value();
+ }
+ }
+
void MemoryViewer::store()
{
TIMING_START(MemoryStore);
@@ -177,11 +211,7 @@ namespace armarx::armem::gui
memoryReaders.clear();
memoryData.clear();
- for (auto& [name, proxy] : mns->getAllRegisteredMemories().proxies)
- {
- armem::client::Reader memoryReader{proxy};
- memoryReaders[name] = memoryReader;
- }
+ memoryReaders = mns.getAllReaders(true);
bool dataChanged = false;
@@ -232,17 +262,7 @@ namespace armarx::armem::gui
void MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
- if (memoryData.find(selectedID.memoryName) == memoryData.end())
- {
- std::stringstream ss;
- ss << "Memory name '" << selectedID.memoryName << "' is unknown. Known are: "
- << simox::alg::get_keys(memoryData);
- statusLabel->setText(QString::fromStdString(ss.str()));
- return;
- }
-
- const std::optional<armem::wm::Memory>& data = memoryData.at(selectedID.memoryName);
-
+ const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
{
if (!selectedID.hasEntityName())
@@ -289,6 +309,74 @@ namespace armarx::armem::gui
}
}
+ void MemoryViewer::resolveMemoryID(const MemoryID& id)
+ {
+ // ARMARX_IMPORTANT << "Resolving memory ID: " << id;
+
+ auto handleError = [this](const std::string & msg)
+ {
+ statusLabel->setText(QString::fromStdString(msg));
+ ARMARX_WARNING << msg;
+ };
+
+ if (id.memoryName.empty())
+ {
+ handleError("Memory name is empty.");
+ }
+
+ aron::typenavigator::ObjectNavigatorPtr segmentType;
+ std::optional<wm::EntityInstance> instance;
+ try
+ {
+ const wm::Memory* data = getSingleMemoryData(id.memoryName);
+ if (data)
+ {
+ segmentType = data->getProviderSegment(id).aronType();
+
+ if (id.hasInstanceIndex())
+ {
+ instance = data->getEntityInstance(id);
+ }
+ else if (id.hasTimestamp())
+ {
+ instance = data->getEntitySnapshot(id).getInstance(0);
+ }
+ else
+ {
+ instance = data->getEntity(id).getLatestSnapshot().getInstance(0);
+ }
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ // May be handled by remote lookup
+ (void) e;
+ }
+
+ if (!instance)
+ {
+ try
+ {
+ // Resolve remotely (may still fail, returns an optional).
+ instance = mns.resolveEntityInstance(id);
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING << e.what();
+ statusLabel->setText(e.what());
+ }
+ }
+
+ if (instance)
+ {
+ instanceGroup->view->addInstanceView(*instance, segmentType);
+ }
+ else
+ {
+ // ToDo: Propagate error back to highlight selected entry in red
+ }
+ }
+
void MemoryViewer::updateMemoryTree()
{
std::map<std::string, const armem::wm::Memory*> convMap;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 9931f6e3523ec8e020a5183da8fb15319d25ae8b..22455503637f55114516bf6e93c6772cd18c179e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -8,6 +8,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
@@ -15,6 +16,7 @@
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
#include <RobotAPI/libraries/armem_gui/MemoryControlWidget.h>
+
class QBoxLayout;
class QDialog;
class QGroupBox;
@@ -63,14 +65,15 @@ namespace armarx::armem::gui
public slots:
+ void updateMemories();
+ void updateInstanceTree(const armem::MemoryID& selectedID);
+
+ void resolveMemoryID(const MemoryID& id);
+
// LTMControlWidget
void store();
void exportHere();
- // Other
- void updateMemories();
- void updateInstanceTree(const armem::MemoryID& selectedID);
-
signals:
@@ -97,11 +100,13 @@ namespace armarx::armem::gui
void onConnect(ManagedIceObject& component);
void onDisconnect(ManagedIceObject& component);
+ const armem::wm::Memory* getSingleMemoryData(const std::string& memoryName);
+
public:
std::string mnsName;
- armem::mns::MemoryNameSystemInterfacePrx mns;
+ armem::client::MemoryNameSystem mns;
std::map<std::string, armem::client::Reader> memoryReaders;
std::map<std::string, std::optional<armem::wm::Memory>> memoryData;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
index b33c381970dfe3f3eb0ce0b0b7be9e9599f75f7f..ff828d4437f042c7857332ec3957f9b5f55c60a5 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
@@ -19,10 +19,11 @@ namespace armarx::armem::gui::instance
useTypeInfoCheckBox->setChecked(true);
QHBoxLayout* checkBoxLayout = new QHBoxLayout();
+ checkBoxLayout->setDirection(QBoxLayout::RightToLeft);
checkBoxLayout->addWidget(useTypeInfoCheckBox);
- layout->addLayout(checkBoxLayout);
layout->addWidget(view);
+ layout->addLayout(checkBoxLayout);
this->setTitle("Instance View (select an entity instance on the left)");
const int margin = 3;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 429d2f3603967f64728e59113841b85c862b8d46..cc65c5a304154d6aa4c7b4ac86a29ca1634be0da 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -1,6 +1,8 @@
#include "InstanceView.h"
#include <QAction>
+#include <QApplication>
+#include <QClipboard>
#include <QGroupBox>
#include <QHBoxLayout>
#include <QHeaderView>
@@ -19,18 +21,27 @@
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
#include <RobotAPI/libraries/armem_gui/instance/ImageView.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
+#include "MemoryIDTreeWidgetItem.h"
+#include "WidgetsWithToolbar.h"
+
namespace armarx::armem::gui::instance
{
InstanceView::InstanceView()
{
+ Logging::setTag("InstanceView");
+
QLayout* layout = new QVBoxLayout();
this->setLayout(layout);
int margin = 3;
@@ -50,15 +61,10 @@ namespace armarx::armem::gui::instance
tree->setHeaderLabels(columns);
tree->header()->resizeSection(int(Columns::KEY), 250);
- tree->header()->resizeSection(int(Columns::VALUE), 150);
+ tree->header()->resizeSection(int(Columns::VALUE), 200);
- treeItemInstanceID = new QTreeWidgetItem({"Instance ID"});
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Memory Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Core Segment Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Provider Segment Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Entity Name"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Timestamp"}));
- treeItemInstanceID->addChild(new QTreeWidgetItem({"Instance Index"}));
+ treeItemInstanceID = new MemoryIDTreeWidgetItem({"Instance ID"});
+ treeItemInstanceID->addKeyChildren();
treeItemMetadata = new QTreeWidgetItem({"Metadata"});
treeItemMetadata->addChild(new QTreeWidgetItem({"Confidence"}));
@@ -74,6 +80,7 @@ namespace armarx::armem::gui::instance
{
item->setExpanded(true);
}
+ treeItemMetadata->setExpanded(false);
tree->setContextMenuPolicy(Qt::CustomContextMenu);
connect(tree, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
@@ -100,7 +107,7 @@ namespace armarx::armem::gui::instance
instance = &memory.getEntityInstance(id);
if (useTypeInfo)
{
- aronType = memory.getCoreSegment(id.coreSegmentName).getProviderSegment(id.providerSegmentName).aronType();
+ aronType = memory.getProviderSegment(id).aronType();
}
}
catch (const armem::error::ArMemError& e)
@@ -122,6 +129,7 @@ namespace armarx::armem::gui::instance
update();
}
+
void InstanceView::update()
{
if (currentInstance)
@@ -136,18 +144,34 @@ namespace armarx::armem::gui::instance
}
+ void InstanceView::addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
+ {
+ ARMARX_IMPORTANT << "Adding instance view with toolbar for instance: " << instance.id();
+ InstanceView* view = new InstanceView;
+ view->setStatusLabel(statusLabel);
+ view->setUseTypeInfo(useTypeInfo);
+
+ WidgetsWithToolbar* child = new WidgetsWithToolbar();
+ child->addWidget(view);
+
+
+ splitter->addWidget(child);
+
+ // Propagate this signal upwards.
+ connect(view, &InstanceView::memoryIdResolutionRequested, this, &This::memoryIdResolutionRequested);
+
+ view->update(instance, aronType);
+ }
+
+
void InstanceView::updateInstanceID(const MemoryID& id)
{
- const std::vector<std::string> items = id.getAllItems();
- ARMARX_CHECK_EQUAL(treeItemInstanceID->childCount(), static_cast<int>(items.size()));
- int i = 0;
- for (const std::string& item : items)
- {
- treeItemInstanceID->child(i++)->setText(int(Columns::VALUE), QString::fromStdString(item));
- }
+ treeItemInstanceID->setInstanceID(id, int(Columns::VALUE));
}
- void InstanceView::updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
+
+ void InstanceView::updateData(
+ const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
{
if (!data)
{
@@ -157,6 +181,8 @@ namespace armarx::armem::gui::instance
}
else if (useTypeInfo && aronType)
{
+ treeItemData->setText(int(Columns::TYPE), QString::fromStdString(sanitizeTypeName(aronType->getName())));
+
TypedDataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
builder.updateTree(treeItemData, *aronType, *data);
@@ -196,24 +222,43 @@ namespace armarx::armem::gui::instance
}
+ aron::Path InstanceView::getElementPath(const QTreeWidgetItem* item) const
+ {
+ QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
+ aron::Path path = deserializePath(qpath);
+ return path;
+ }
+
+
void InstanceView::prepareTreeContextMenu(const QPoint& pos)
{
- QTreeWidgetItem* item = tree->itemAt(pos);
- if (item == nullptr || item->parent() == nullptr)
+ const QTreeWidgetItem* item = tree->itemAt(pos);
+ if (item == nullptr)
{
- // No item or top level item => no context menu.
- return;
+ return; // No item => no context menu.
}
QMenu menu(this);
+ if (item == this->treeItemInstanceID && currentInstance.has_value())
+ {
+ if (QAction* action = makeActionCopyMemoryID(currentInstance->id()))
+ {
+ menu.addAction(action);
+ }
+ }
+ else if (item->parent() == nullptr)
+ {
+ return; // Other top level item => no context menu.
+ }
+
+ // Type descriptor based actions
aron::type::Descriptor type = static_cast<aron::type::Descriptor>(item->data(int(Columns::TYPE), Qt::UserRole).toInt());
switch (type)
{
case aron::type::Descriptor::eIVTCByteImage:
{
- QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = deserializePath(qpath);
+ const aron::Path path = getElementPath(item);
QAction* viewAction = new QAction("Show image");
menu.addAction(viewAction);
@@ -224,12 +269,118 @@ namespace armarx::armem::gui::instance
}
break;
default:
- return;
+ break;
+ }
+
+ // Type name based actions
+ const std::string typeName = item->text(int(Columns::TYPE)).toStdString();
+ if (typeName == instance::sanitizedMemoryIDTypeName)
+ {
+ const aron::Path path = getElementPath(item);
+
+ if (std::optional<MemoryID> id = getElementMemoryID(path))
+ {
+ if (QAction* action = makeActionCopyMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ if (QAction* action = makeActionResolveMemoryID(id.value()))
+ {
+ menu.addAction(action);
+ }
+ }
+ }
+
+
+ if (menu.actions().size() > 0)
+ {
+ menu.exec(tree->mapToGlobal(pos));
+ }
+ }
+
+
+ std::optional<MemoryID> InstanceView::getElementMemoryID(const aron::Path& elementPath)
+ {
+ aron::datanavigator::NavigatorPtr element;
+ try
+ {
+ element = currentInstance->data()->navigateAbsolute(elementPath);
+ }
+ // This can happen when the underlying entity structure changes (a new entity has been selected).
+ catch (const aron::error::AronException&)
+ {
+ // showErrorMessage(e.what());
+ return std::nullopt;
+ }
+ catch (const armarx::LocalException& e)
+ {
+ showErrorMessage(e.what());
+ return std::nullopt;
+ }
+
+ std::stringstream couldNotParseMsg;
+ couldNotParseMsg << "Element " << elementPath.toString() << " could not be parsed as MemoryID.";
+
+ auto dictElement = std::dynamic_pointer_cast<aron::datanavigator::DictNavigator>(element);
+ if (!dictElement)
+ {
+ showErrorMessage(couldNotParseMsg.str() + " (Failed to cast to DictNavigator.)");
+ return std::nullopt;
}
- menu.exec(tree->mapToGlobal(pos));
+ try
+ {
+ arondto::MemoryID dto;
+ dto.fromAron(dictElement);
+
+ MemoryID id;
+ armem::fromAron(dto, id);
+ return id;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ showErrorMessage(couldNotParseMsg.str());
+ return std::nullopt;
+ }
}
+
+ QAction* InstanceView::makeActionResolveMemoryID(const MemoryID& id)
+ {
+ QAction* action = new QAction("Resolve memory ID");
+
+ if (not(id.hasEntityName() and id.isWellDefined()))
+ {
+ action->setDisabled(true);
+ action->setText(action->text() + " (incomplete Memory ID)");
+ }
+ connect(action, &QAction::triggered, [this, id]()
+ {
+ // ARMARX_IMPORTANT << "emit memoryIdResolutionRequested(id = " << id << ")";
+ emit memoryIdResolutionRequested(id);
+ });
+
+ return action;
+ }
+
+ QAction* InstanceView::makeActionCopyMemoryID(const MemoryID& id)
+ {
+ QAction* action = new QAction("Copy memory ID to clipboard");
+
+ connect(action, &QAction::triggered, [/*this,*/ id]() // `this` for ARMARX_IMPORTANT
+ {
+ const QString idStr = QString::fromStdString(id.str());
+
+ // ARMARX_IMPORTANT << "Copy '" << idStr.toStdString() << "' to clipboard.";
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(idStr);
+ QApplication::processEvents();
+ });
+
+ return action;
+ }
+
+
void InstanceView::showImageView(const aron::Path& elementPath)
{
if (!currentInstance)
@@ -238,7 +389,18 @@ namespace armarx::armem::gui::instance
}
if (!imageView)
{
- imageView = ImageView(splitter);
+ WidgetsWithToolbar* toolbar = new WidgetsWithToolbar();
+
+ imageView = new ImageView();
+ imageView->toolbar = toolbar;
+ toolbar->addWidget(imageView);
+
+ splitter->addWidget(toolbar);
+
+ connect(toolbar, &WidgetsWithToolbar::closing, [this]()
+ {
+ imageView = nullptr;
+ });
}
imageView->elementPath = elementPath;
updateImageView(currentInstance->data());
@@ -246,11 +408,11 @@ namespace armarx::armem::gui::instance
void InstanceView::removeImageView()
{
- imageView->group->hide();
- imageView->group->deleteLater();
- imageView = std::nullopt;
+ imageView->toolbar->close();
+ imageView = nullptr;
}
+
void InstanceView::updateImageView(const aron::datanavigator::DictNavigatorPtr& data)
{
using aron::datanavigator::NDArrayNavigator;
@@ -317,28 +479,25 @@ namespace armarx::armem::gui::instance
std::stringstream title;
title << "Image element '" << imageView->elementPath.toString() << "'"; // of entity instance " << currentInstance->id();
- imageView->group->setTitle(QString::fromStdString(title.str()));
+ imageView->setTitle(QString::fromStdString(title.str()));
imageView->view->setImage(QImage(imageData->getData(), shape.at(0), shape.at(1), format));
}
- InstanceView::ImageView::ImageView(QSplitter* parent)
+ InstanceView::ImageView::ImageView()
{
- group = new QGroupBox();
- parent->addWidget(group);
-
- group->setLayout(new QHBoxLayout());
+ setLayout(new QHBoxLayout());
int margin = 2;
- group->layout()->setContentsMargins(margin, margin, margin, margin);
+ layout()->setContentsMargins(margin, margin, margin, margin);
if (false)
{
- QFont font = group->font();
+ QFont font = this->font();
font.setPointSizeF(font.pointSize() * 0.75);
- group->setFont(font);
+ setFont(font);
}
view = new instance::ImageView();
- group->layout()->addWidget(view);
+ layout()->addWidget(view);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 0b0a0ee2655f506fa09f15b99dd0edc149e88470..26f9d3be5fccfdce4fdb17a9ddbe430c2f334433 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -3,6 +3,7 @@
#include <optional>
#include <QWidget>
+#include <QGroupBox>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
@@ -19,9 +20,11 @@ class QTreeWidgetItem;
namespace armarx::armem::gui::instance
{
class ImageView;
+ class MemoryIDTreeWidgetItem;
+ class WidgetsWithToolbar;
- class InstanceView : public QWidget
+ class InstanceView : public QWidget, public Logging
{
Q_OBJECT
using This = InstanceView;
@@ -38,16 +41,20 @@ namespace armarx::armem::gui::instance
void update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
void update();
+ void addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+
signals:
void updated();
void instanceSelected(const MemoryID& id);
+ void memoryIdResolutionRequested(const MemoryID& id);
private slots:
void prepareTreeContextMenu(const QPoint& pos);
+
void showImageView(const aron::Path& elementPath);
void removeImageView();
@@ -61,6 +68,12 @@ namespace armarx::armem::gui::instance
void showErrorMessage(const std::string& message);
+ aron::Path getElementPath(const QTreeWidgetItem* item) const;
+ std::optional<MemoryID> getElementMemoryID(const aron::Path& elementPath);
+
+ QAction* makeActionResolveMemoryID(const MemoryID& id);
+ QAction* makeActionCopyMemoryID(const MemoryID& id);
+
private:
@@ -78,21 +91,22 @@ namespace armarx::armem::gui::instance
QSplitter* splitter;
QTreeWidget* tree;
- QTreeWidgetItem* treeItemInstanceID;
+ MemoryIDTreeWidgetItem* treeItemInstanceID;
QTreeWidgetItem* treeItemMetadata;
QTreeWidgetItem* treeItemData;
- class ImageView
+ class ImageView : public QGroupBox
{
public:
- ImageView(QSplitter* parent);
+ ImageView();
- QGroupBox* group;
instance::ImageView* view;
aron::Path elementPath;
+
+ WidgetsWithToolbar* toolbar;
};
- std::optional<ImageView> imageView;
+ ImageView* imageView = nullptr;
QLabel* statusLabel = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8f568c79cdd1d84fa803df9ffaee737e24c9ea2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
@@ -0,0 +1,41 @@
+#include "MemoryIDTreeWidgetItem.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ void MemoryIDTreeWidgetItem::addKeyChildren()
+ {
+ addChild(new QTreeWidgetItem({"Memory Name"}));
+ addChild(new QTreeWidgetItem({"Core Segment Name"}));
+ addChild(new QTreeWidgetItem({"Provider Segment Name"}));
+ addChild(new QTreeWidgetItem({"Entity Name"}));
+ addChild(new QTreeWidgetItem({"Timestamp"}));
+ addChild(new QTreeWidgetItem({"Instance Index"}));
+ }
+
+
+ void MemoryIDTreeWidgetItem::setInstanceID(const MemoryID& id, int valueColumn)
+ {
+ setText(valueColumn, QString::fromStdString(id.str()));
+
+ const std::vector<std::string> items = id.getAllItems();
+ ARMARX_CHECK_EQUAL(childCount(), static_cast<int>(items.size()));
+ int i = 0;
+ for (const std::string& item : items)
+ {
+ child(i++)->setText(valueColumn, QString::fromStdString(item));
+ }
+ // Timestamp in human-readable format
+ if (id.hasTimestamp())
+ {
+ child(4)->setText(valueColumn, QString::fromStdString(toDateTimeMilliSeconds(id.timestamp)));
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
new file mode 100644
index 0000000000000000000000000000000000000000..a011a59aff48b8725e42aac1c7eab71ac5ca8676
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <QTreeWidgetItem>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+}
+
+namespace armarx::armem::gui::instance
+{
+
+ class MemoryIDTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+
+ using QTreeWidgetItem::QTreeWidgetItem;
+
+ void addKeyChildren();
+ void setInstanceID(const MemoryID& id, int valueColumn = 1);
+
+
+ };
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b41ce7649aa21303658ed3a400f691e5efca1d0e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
@@ -0,0 +1,80 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
+* @date 2021
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "WidgetsWithToolbar.h"
+
+#include <QAction>
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <QToolBar>
+#include <QVBoxLayout>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "InstanceView.h"
+
+
+namespace armarx::armem::gui::instance
+{
+
+ WidgetsWithToolbar::WidgetsWithToolbar(QWidget* parent) : QWidget(parent)
+ {
+ const int margin = 0;
+ this->setContentsMargins(margin, margin, margin, margin);
+
+ _layout = new QHBoxLayout();
+ this->setLayout(_layout);
+ _layout->setContentsMargins(margin, margin, margin, margin);
+ _layout->setSpacing(0);
+
+
+ toolbar = new QToolBar();
+ toolbar->setOrientation(Qt::Orientation::Vertical);
+ toolbar->setContentsMargins(margin, margin, margin, margin);
+
+ QAction* action = toolbar->addAction(QIcon(":/icons/dialog-close.ico"), "Close", [this]()
+ {
+ this->close();
+ });
+ action->setToolTip("Remove this instance view");
+
+ _layout->addWidget(toolbar);
+ }
+
+
+ void WidgetsWithToolbar::addWidget(QWidget* widget)
+ {
+ ARMARX_CHECK_GREATER_EQUAL(_layout->count(), 1);
+ _layout->insertWidget(_layout->count() - 1, widget);
+ }
+
+ void WidgetsWithToolbar::close()
+ {
+ // ARMARX_IMPORTANT << "Closing instance view ...";
+ emit closing();
+
+ this->hide();
+ this->deleteLater();
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d2f2a00aa0d2ee74d47f95ce88e49b85b3392e4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
@@ -0,0 +1,76 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::
+* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu)
+* @date 2021
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <QWidget>
+
+class QHBoxLayout;
+class QVBoxLayout;
+class QToolBar;
+
+
+namespace armarx::armem::gui::instance
+{
+ class WidgetsWithToolbar : public QWidget
+ {
+ Q_OBJECT
+ using This = WidgetsWithToolbar;
+
+ public:
+
+ WidgetsWithToolbar(QWidget* parent = nullptr);
+
+ void addWidget(QWidget* widget);
+
+
+ public slots:
+
+ void close();
+
+
+ signals:
+
+ void closing();
+
+
+ protected slots:
+
+ signals:
+
+
+ protected:
+
+
+ public:
+
+ QToolBar* toolbar;
+
+
+ private:
+
+ QHBoxLayout* _layout;
+
+ };
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
index afe3fdc8fd7e9223d804506264bb3df49595a271..0180b51961c795ca4a45fd748078cb5cda1ca663 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -7,13 +7,14 @@
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-static const std::string MemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getName();
+const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getName();
+const std::string armarx::armem::gui::instance::sanitizedMemoryIDTypeName = "MemoryID";
std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
{
- if (typeName == MemoryIDTypeName)
+ if (typeName == rawMemoryIDTypeName)
{
- return "MemoryID";
+ return sanitizedMemoryIDTypeName;
}
namespace s = simox::alg;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
index 80c77acf229ecb6d70b55b6ac1c5b951a0cb89bd..80bc8fe789e5d2f6a442632f3c88b93020ddbb02 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -6,6 +6,9 @@
namespace armarx::armem::gui::instance
{
+ extern const std::string rawMemoryIDTypeName;
+ extern const std::string sanitizedMemoryIDTypeName;
+
std::string sanitizeTypeName(const std::string& typeName);
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index 5be64a3f5657893e2c6f36e4695eff4a08c9dc9e..56d204abf42573540c363c0973f4bf10db357317 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -2,10 +2,15 @@
#include <QTreeWidgetItem>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h>
namespace armarx::armem::gui::instance
@@ -39,12 +44,26 @@ namespace armarx::armem::gui::instance
}
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::ObjectNavigator& type,
aron::datanavigator::DictNavigator& data)
{
DictBuilder builder = getDictBuilder();
+ builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
+ {
+ if (type.getMemberType(key)->getName() == instance::rawMemoryIDTypeName)
+ {
+ MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
+ item->addKeyChildren();
+ return item;
+ }
+ else
+ {
+ return this->makeItem(key);
+ }
+ });
builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
{
auto childType = type.getMemberType(key);
@@ -85,6 +104,7 @@ namespace armarx::armem::gui::instance
}
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::PairNavigator& type,
@@ -109,6 +129,7 @@ namespace armarx::armem::gui::instance
builder.updateTree(parent, getIndex(data.childrenSize()));
}
+
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
aron::typenavigator::TupleNavigator& type,
@@ -139,37 +160,54 @@ namespace armarx::armem::gui::instance
aron::typenavigator::Navigator& type,
aron::datanavigator::Navigator& data)
{
- using namespace aron::typenavigator;
+ using namespace aron;
- const std::string value = armarx::aron::TypedDataDisplayVisitor::getValue(type, data);
- const std::string typeName = sanitizeTypeName(type.getName());
+ const std::string value = aron::TypedDataDisplayVisitor::getValue(type, data);
+ const std::string typeName = instance::sanitizeTypeName(type.getName());
setRowTexts(item, key, value, typeName);
- item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
+ item->setData(columnKey, Qt::UserRole, instance::serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
- if (auto t = dynamic_cast<ObjectNavigator*>(&type))
+ if (typeName == sanitizedMemoryIDTypeName)
+ {
+ MemoryIDTreeWidgetItem* memoryIDItem = dynamic_cast<MemoryIDTreeWidgetItem*>(item);
+ datanavigator::DictNavigator* dictData = dynamic_cast<datanavigator::DictNavigator*>(&data);
+ if (memoryIDItem && dictData)
+ {
+ arondto::MemoryID dto;
+ // Because fromAron does not take refs -.-
+ dto.fromAron(std::make_shared<datanavigator::DictNavigator>(*dictData));
+
+ MemoryID id = aron::fromAron<MemoryID>(dto);
+ memoryIDItem->setInstanceID(id);
+ return; // Done, no recursion.
+ }
+ }
+
+ if (auto t = dynamic_cast<typenavigator::ObjectNavigator*>(&type))
{
- _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ _updateTree<datanavigator::DictNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<DictNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::DictNavigator*>(&type))
{
- _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ _updateTree<datanavigator::DictNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<ListNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::ListNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<PairNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::PairNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
- else if (auto t = dynamic_cast<TupleNavigator*>(&type))
+ else if (auto t = dynamic_cast<typenavigator::TupleNavigator*>(&type))
{
- _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ _updateTree<datanavigator::ListNavigator>(item, *t, data);
}
}
+
template <class DataT, class TypeT>
void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
{
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
index 508ec50924baacdfe90df941a8f55ac730620837..ce054ff30d516dd5bb64c120b197fd2f50887ae1 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
@@ -58,16 +58,31 @@ namespace armarx::gui
void LifecycleClient::onInit(ManagedIceObject& component)
{
(void) component;
+ onInit();
}
void LifecycleClient::onConnect(ManagedIceObject& component)
{
(void) component;
+ onConnect();
}
void LifecycleClient::onDisconnect(ManagedIceObject& component)
{
(void) component;
+ onDisconnect();
+ }
+
+ void LifecycleClient::onInit()
+ {
+ }
+
+ void LifecycleClient::onConnect()
+ {
+ }
+
+ void LifecycleClient::onDisconnect()
+ {
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.h b/source/RobotAPI/libraries/armem_gui/lifecycle.h
index 0bc404387b3165841526c12a09a9fa6bbad0fdd9..cb5cb3ddfc4d2f2fc3f1b96709db0a3bc49c245e 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.h
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.h
@@ -70,6 +70,11 @@ namespace armarx::gui
virtual void onConnect(ManagedIceObject& component);
virtual void onDisconnect(ManagedIceObject& component);
+ // Alternative override
+ virtual void onInit();
+ virtual void onConnect();
+ virtual void onDisconnect();
+
};
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 4dc890e600b5419db408d3110455a19fd96f2e68..1d4a2b908eba38fbb4be2d028030598c220aae44 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -34,7 +34,7 @@ namespace armarx::armem::gui::memory
header()->setMinimumSectionSize(25);
header()->resizeSection(int(Columns::KEY), 250);
- header()->resizeSection(int(Columns::SIZE), 30);
+ header()->resizeSection(int(Columns::SIZE), 40);
header()->setTextElideMode(Qt::TextElideMode::ElideRight);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
index 8e00d3811f24d12146df6aa0c782af2437a71711..f0ee979a814fff0257a4f87d1003b34ab0b3aa58 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
@@ -42,6 +42,22 @@ namespace armarx::armem::gui
}
+ // Source: https://stackoverflow.com/a/26538572
+ class LeadingZeroSpinBox : public QSpinBox
+ {
+ using QSpinBox::QSpinBox;
+
+ int numDigits = 6;
+ int base = 10;
+
+ virtual QString textFromValue(int value) const;
+ };
+ QString LeadingZeroSpinBox::textFromValue(int value) const
+ {
+ return QString("%1").arg(value, numDigits, base, QChar('0'));
+ }
+
+
SnapshotFormSingle::SnapshotFormSingle()
{
const QDateTime now = QDateTime::currentDateTime();
@@ -52,25 +68,41 @@ namespace armarx::armem::gui
dateTime->setDisabled(true);
setDateTimeDisplayFormat(dateTime);
+ microseconds = new LeadingZeroSpinBox();
+ microseconds->setDisabled(true);
+ microseconds->setMinimum(0);
+ microseconds->setMaximum(1000 * 1000 - 1);
+ microseconds->setSingleStep(1);
+ microseconds->setValue(static_cast<int>(now.toMSecsSinceEpoch() % 1000) * 1000);
+
+ QHBoxLayout* timestampLayout = new QHBoxLayout();
+ timestampLayout->addWidget(dateTime);
+ timestampLayout->addWidget(microseconds);
+
latest = new QCheckBox("Latest");
latest->setChecked(true);
QGridLayout* layout = new QGridLayout(this);
layout->addWidget(label, 0, 0);
- layout->addWidget(dateTime, 0, 1);
+ layout->addLayout(timestampLayout, 0, 1);
layout->addWidget(latest, 0, 2);
connect(latest, &QCheckBox::toggled, dateTime, &QDateTimeEdit::setDisabled);
+ connect(latest, &QCheckBox::toggled, microseconds, &QSpinBox::setDisabled);
connect(dateTime, &QDateTimeEdit::dateTimeChanged, this, &SnapshotForm::queryChanged);
+ connect(microseconds, QOverload<int>::of(&QSpinBox::valueChanged), this, &SnapshotForm::queryChanged);
connect(latest, &QCheckBox::toggled, this, &SnapshotForm::queryChanged);
}
+
void SnapshotFormSingle::fillEntitySelector(client::query::SnapshotSelector& selector)
{
- selector.atTime(latest->isChecked()
- ? Time::microSeconds(-1)
- : Time::milliSeconds(dateTime->dateTime().toMSecsSinceEpoch()));
+ const Time time = latest->isChecked()
+ ? Time::microSeconds(-1)
+ : (Time::seconds(dateTime->dateTime().toSecsSinceEpoch()))
+ + Time::microSeconds(microseconds->value());
+ selector.atTime(time);
}
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
index 4fa9b3a8d9148048747174c8c72ed155953dc8b3..0ca9950b33a0585c97a8157bf17ad3c272fc8abc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
@@ -59,6 +59,7 @@ namespace armarx::armem::gui
private:
QLabel* label;
QDateTimeEdit* dateTime;
+ QSpinBox* microseconds;
QCheckBox* latest;
};
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 d11b7421313e4586e4347b31c9f8c79c09f169f5..8824b1e7f8b172b0c837a3441b7db97480a7e3e1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -3,14 +3,15 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
namespace fs = ::std::filesystem;
@@ -18,7 +19,9 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -41,23 +44,23 @@ namespace armarx::armem::articulated_object
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
-
armem::MemoryID id = armem::MemoryID();
id.memoryName = properties.memoryName;
id.coreSegmentName = properties.coreClassSegmentName;
// listen to all provider segments!
- memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+ memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
}
void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
@@ -159,6 +162,16 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
+ std::string Reader::getProviderName() const
+ {
+ return properties.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)
{
@@ -321,4 +334,4 @@ namespace armarx::armem::articulated_object
return descriptions;
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
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 21a4591cd2f1f5c9a125177e50285b3802f561b9..5bc5efc3e3fe2a66716467444bd4ee54c86ac92c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -24,18 +24,21 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
#include "interfaces.h"
+
namespace armarx::armem::articulated_object
{
class Reader:
virtual public ReaderInterface
{
public:
- Reader(armem::ClientReaderComponentPluginUser& component);
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
@@ -51,6 +54,9 @@ namespace armarx::armem::articulated_object
std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::string getProviderName() const;
+ void setProviderName(const std::string& providerName);
+
// TODO(fabian.reister): register property defs
protected:
@@ -72,11 +78,11 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
-
- armem::ClientReaderComponentPluginUser& component;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
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 9062747de124ac3db5ba37e3f14f2b8cdfc7b692..f351382f36d94372154116327dc3e65e38eb0dfc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -5,20 +5,24 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
namespace armarx::armem::articulated_object
{
- Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -34,6 +38,8 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
+
+ ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
}
@@ -41,18 +47,18 @@ namespace armarx::armem::articulated_object
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
- ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- memoryReader.setReadingMemory(result.proxy);
-
const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
const auto resultCoreInstanceSegmentName =
@@ -64,7 +70,7 @@ namespace armarx::armem::articulated_object
id.setCoreSegmentID(refId); // listen to all provider segments!
updateKnownObjects();
- memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
+ memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
}
void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
@@ -155,31 +161,50 @@ namespace armarx::armem::articulated_object
return updateResult.snapshotID;
}
+ std::string Writer::getProviderName() const
+ {
+ return properties.providerName;
+ }
+
+ void Writer::setProviderName(const std::string& providerName)
+ {
+ this->properties.providerName = providerName;
+ }
+
bool Writer::storeInstance(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
ARMARX_DEBUG << "Trying to create core segment + provider segment";
- const auto result =
- memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
-
- if (not result.success)
+ // Provider segments are now added when necessary.
+ // Adding them explicitly is only needed to set a deriving provider segment type.
+ if (false)
{
- ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
- return false;
+ const auto result =
+ memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+ return false;
+ }
+ else
+ {
+ ARMARX_IMPORTANT << VAROUT(result.segmentID);
+ }
}
const auto& timestamp = obj.timestamp;
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId
- .withEntityName(obj.description.name)
- .withTimestamp(timestamp);
+ const auto providerId = armem::MemoryID()
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreInstanceSegmentName)
+ .withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
- update.entityID = entityID;
+ update.entityID = providerId.withEntityName(obj.description.name);
+ // .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
arondto::Robot aronArticulatedObject;
robot::toAron(aronArticulatedObject, obj);
@@ -323,4 +348,4 @@ namespace armarx::armem::articulated_object
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index f2031dd402e546a21a80a17c28f8b2e4107f9059..c5fa97dd6e5d37402b1a2a7d0626131348f1e25c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -23,9 +23,11 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem/client/Writer.h"
-#include "RobotAPI/libraries/armem/client.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include "interfaces.h"
@@ -37,10 +39,9 @@ namespace armarx::armem::articulated_object
virtual public WriterInterface
{
public:
- Writer(armem::ClientComponentPluginUser& component);
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Writer() = default;
-
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
@@ -52,7 +53,12 @@ namespace armarx::armem::articulated_object
// const std::string& getPropertyPrefix() const override;
+ std::string getProviderName() const;
+ void setProviderName(const std::string& providerName);
+
+
private:
+
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
@@ -70,13 +76,15 @@ namespace armarx::armem::articulated_object
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string providerName = "";
bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
@@ -85,9 +93,7 @@ namespace armarx::armem::articulated_object
// key: name of object: RobotDescription::name
std::unordered_map<std::string, MemoryID> knownObjects;
-
- armem::ClientComponentPluginUser& component;
};
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index e3ce169b9ef41292b7ad565f94b74faf141ee7ad..f723f30b2eb2d704dcfae73f47e9351cf659b7ad 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -3,19 +3,21 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem/util/util.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
namespace armarx::armem::attachment
{
@@ -72,7 +74,9 @@ namespace armarx::armem::attachment
}
} // namespace
- Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -92,16 +96,16 @@ namespace armarx::armem::attachment
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
@@ -207,4 +211,4 @@ namespace armarx::armem::attachment
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
index bcfb3d1a135a2446f255a0038a2aaad85c2a3412..1080156c8bbbdb95afc62c0467d53dbe700c49f9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -24,16 +24,19 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
namespace armarx::armem::attachment
{
class Reader
{
public:
- Reader(armem::ClientReaderComponentPluginUser& component);
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
@@ -56,11 +59,10 @@ namespace armarx::armem::attachment
const std::string propertyPrefix = "mem.obj.attachment.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
-
- armem::ClientReaderComponentPluginUser& component;
};
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
index f4d16070b84e4d29aca1742ce0356fb0c69ea5b0..69f5806c2bb6dc4cbfae1f196d4f967c04ab9a12 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -5,20 +5,23 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
namespace armarx::armem::attachment
{
- Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -38,17 +41,17 @@ namespace armarx::armem::attachment
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- memoryReader.setReadingMemory(result.proxy);
}
@@ -141,4 +144,4 @@ namespace armarx::armem::attachment
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
index f20421d4b27d9e2fc10e106e238a9684fca9d2cf..754f4f28041429cd1bb93c9bd3b962335c984530 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -23,13 +23,14 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/Reader.h"
-#include "RobotAPI/libraries/armem/client/Writer.h"
-#include "RobotAPI/libraries/armem/client.h"
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
-#include "RobotAPI/libraries/armem_objects/types.h"
namespace armarx::armem::attachment
{
@@ -37,7 +38,7 @@ namespace armarx::armem::attachment
class Writer
{
public:
- Writer(armem::ClientComponentPluginUser& component);
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Writer() = default;
@@ -60,14 +61,14 @@ namespace armarx::armem::attachment
const std::string propertyPrefix = "mem.obj.articulated.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
+
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
armem::client::Reader memoryReader;
std::mutex memoryReaderMutex;
-
- armem::ClientComponentPluginUser& component;
};
-} // namespace armarx::armem::attachment
\ No newline at end of file
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index b2e6be2f6a9c0d398ef37844e686bab8547d7653..36ff86743abbbae569d43b44aff1e92a25896b19 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 0e2d95dda06126717c383c9b71f7b1088bacd71a..f2490c841d0042a133fb1c8cf58740eb2af614cd 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -9,7 +9,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 669f076124b6897325a7fbcf8540466a157cb92c..7b7dc7a428af8896fd9084f887e0c883c9dd4034 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -3,7 +3,7 @@
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
diff --git a/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3624c35b18ff85c1b71bdf58ad5a37c245f7959e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_gui
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem_objects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <iostream>
+
+
+
+BOOST_AUTO_TEST_CASE(test_armem_objects)
+{
+ BOOST_CHECK(true);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bb67820c6e69342d583d11dc158448d7bc532ea6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore "${LIB_NAME}")
+
+armarx_add_test(ArMemObjectsTest ArMemObjectsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
deleted file mode 100644
index 145ced819b68db659a92e232747c645c24037ce5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME armem_robot_mapping)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- # This package
- RobotAPICore
- RobotAPI::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./aron_conversions.h
- ./MappingDataWriter.h
- ./MappingDataReader.h
- SOURCES
- ./aron_conversions.cpp
- ./MappingDataWriter.cpp
- ./MappingDataReader.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/LaserScan.xml
-)
-
-
-add_library(RobotAPI::armem_robot_mapping ALIAS armem_robot_mapping)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
deleted file mode 100644
index 351d71398a6959f346aadf45c6c604edcd865a53..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "MappingDataWriter.h"
-
-#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-
-
-namespace armarx::armem
-{
-
- MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
- : component(component) {}
-
- MappingDataWriter::~MappingDataWriter() = default;
-
- void MappingDataWriter::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void MappingDataWriter::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
- << "' ...";
- auto result = component.useMemory(properties.memoryName);
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
- return;
- }
-
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
- }
-
-
- bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
- const std::string& frame,
- const std::string& agentName,
- const std::int64_t& timestamp)
- {
- std::lock_guard g{memoryWriterMutex};
-
- const auto result =
- memoryWriter.addSegment(properties.mappingMemoryName, agentName);
-
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
- return false;
- }
-
- const auto iceTimestamp = Time::microSeconds(timestamp);
-
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(frame).withTimestamp(iceTimestamp);
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- arondto::LaserScanStamped aronSensorData;
- // currently only sets the header
- toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
-
- auto dict = aronSensorData.toAron();
- dict->addElement("scan", toAron(laserScan));
-
- update.instancesData = {dict};
- 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;
- }
-
-} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
deleted file mode 100644
index 7ecffc237d480fd33c30226ef7f1cf02c09308de..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-#pragma once
-
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-namespace armarx
-{
-
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace aron
-
- // struct LaserScan;
- struct LaserScanStamped;
-
- void fromAron(
- const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName);
-
-
- template<typename T>
- auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
- {
- return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
- }
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
- void toAron(
- const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScan);
-
- inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
- {
- return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
- }
-
-
-} // namespace armarx
\ No newline at end of file
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 46d4b1014dba8dfe1ed59a8343395b9063e4050d..7a500d1391c0f0942795b49af65740f350eecbed 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -3,25 +3,27 @@
#include <mutex>
#include <optional>
-#include "ArmarXCore/core/exceptions/LocalException.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
-#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
-#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
-#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) :
- memoryClient(component), transformReader(component)
+ RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
{
}
@@ -42,16 +44,16 @@ namespace armarx::armem::robot_state
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
{
- ARMARX_ERROR << result.errorMessage;
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
std::optional<robot::Robot> RobotReader::get(const std::string& name,
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 b5bd01e5d182a88cc6d36e8c8112e1683c017d16..13b5572d382059bcff17ce2bb5bfb04cc819b04b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,13 +24,14 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client.h"
-#include "RobotAPI/libraries/armem/client/Reader.h"
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
-#include "RobotAPI/libraries/armem_robot/client/interfaces.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
namespace armarx::armem::robot_state
{
@@ -43,7 +44,7 @@ namespace armarx::armem::robot_state
class RobotReader : virtual public robot::ReaderInterface
{
public:
- RobotReader(armem::ClientReaderComponentPluginUser& component);
+ RobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~RobotReader() = default;
void connect();
@@ -89,12 +90,11 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
- armem::ClientReaderComponentPluginUser& memoryClient;
-
client::robot_state::localization::TransformReader transformReader;
};
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
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 a3655fee1ae2597b768424f0b89fd581a1bb0680..10e13611f3fd8608a5d0b5027fd92b960c3a7754 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -14,8 +14,8 @@
namespace armarx::armem::robot_state
{
- VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
- RobotReader(component)
+ VirtualRobotReader::VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ RobotReader(memoryNameSystem)
{
}
@@ -83,4 +83,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index a280e71b6132f065aec28a008ebb5ab803845dad..e5945103cf4956fae90644b630cc61132624227b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,6 +27,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+
namespace armarx::armem::robot_state
{
/**
@@ -40,7 +41,7 @@ namespace armarx::armem::robot_state
class VirtualRobotReader : virtual public RobotReader
{
public:
- VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
+ VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~VirtualRobotReader() = default;
void connect();
@@ -61,4 +62,4 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure);
};
-} // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 8a37b2dec14cfc6721b7458fce984dac59bdfb21..6aa026431e7d9efd0128a9007d23918b291b54bc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -47,6 +47,7 @@
// this package
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
@@ -61,7 +62,8 @@
namespace armarx::armem::client::robot_state::localization
{
- TransformReader::TransformReader(armem::ClientReaderComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+ TransformReader::TransformReader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem) {}
TransformReader::~TransformReader() = default;
@@ -83,16 +85,16 @@ namespace armarx::armem::client::robot_state::localization
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
TransformResult TransformReader::getGlobalPose(const std::string& agentName,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 2d1d98ddf82c5c137b934b3be91079bb87298503..433694ec819859bdcda209dd57aad21d22512116 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -22,10 +22,9 @@
#pragma once
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client.h>
-
#include "interfaces.h"
namespace armarx::armem::client::robot_state::localization
@@ -45,7 +44,7 @@ namespace armarx::armem::client::robot_state::localization
virtual public TransformReaderInterface
{
public:
- TransformReader(armem::ClientReaderComponentPluginUser& memoryClient);
+ TransformReader(armem::client::MemoryNameSystem& memoryNameSystem);
~TransformReader() override;
@@ -61,19 +60,17 @@ namespace armarx::armem::client::robot_state::localization
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
const std::string propertyPrefix = "mem.robot_state.";
-
- armem::ClientReaderComponentPluginUser& memoryClient;
-
};
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 2327c141840bc74f8b43ab14df03b03f2b98ccdf..47660fc439e597f4c8f59bbcff666223c5964e18 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -46,14 +46,16 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::robot_state::localization
{
- TransformWriter::TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+ TransformWriter::TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem) {}
TransformWriter::~TransformWriter() = default;
@@ -73,16 +75,16 @@ namespace armarx::armem::client::robot_state::localization
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
-
- memoryWriter.setWritingMemory(result.proxy);
}
bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 0402d8188e52b9a967805a7ddb9ca5df9c14320c..299c79b69222accb8290a7064229a9dfe59e8572 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -24,13 +24,12 @@
#include <mutex>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-// #include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-
-#include <RobotAPI/libraries/armem/client.h>
#include "interfaces.h"
+
namespace armarx::armem::client::robot_state::localization
{
@@ -49,8 +48,8 @@ namespace armarx::armem::client::robot_state::localization
virtual public TransformWriterInterface
{
public:
- TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient);
+ TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem);
~TransformWriter() override;
// TransformWriterInterface
@@ -63,19 +62,19 @@ namespace armarx::armem::client::robot_state::localization
bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
private:
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- std::string memoryName = "RobotStateMemory";
+ std::string memoryName = "RobotState";
std::string localizationSegment = "Localization";
} properties;
const std::string propertyPrefix = "mem.robot_state.";
-
- armem::ClientWriterComponentPluginUser& memoryClient;
};
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 07583dac6d71197a8bad02f71bf4aaa29414a239..a59aa5a4c6cdd3ebe552522c0ffd532e873da82a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -353,11 +353,11 @@ namespace armarx::armem::common::robot_state::localization
{
ARMARX_DEBUG << "empty transform";
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2", 0);
}
ARMARX_DEBUG << "single transform";
return transforms.front().transform;
}
-} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index cc0bb1f071d71d228a0893e9dce85afdc402b78c..d50d965608f7cf4ad8541467d75442303a293211 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -18,7 +18,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index e84dbe98f558d98c746f9a323d9f89ad521aa988..e0dcf6c2bcd9d86012d98cf5aded6fee585ed866 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -25,7 +25,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 5ec63c12d9c3f6485839ff138b1ebe409dd1adc8..7f95e43878aee62619f8c4997db25460e11ed8cd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -10,7 +10,7 @@
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1767c9cebd878b1d15cad6f8043a2bd7cf932814
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_vision)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ aroncommon
+ # System / External
+ Eigen3::Eigen
+ HEADERS
+ ./aron_conversions.h
+ ./client/laser_scans/Reader.h
+ ./client/laser_scans/Writer.h
+ ./client/occupancy_grid/Reader.h
+ ./client/occupancy_grid/Writer.h
+ SOURCES
+ ./aron_conversions.cpp
+ ./client/laser_scans/Reader.cpp
+ ./client/laser_scans/Writer.cpp
+ ./client/occupancy_grid/Reader.cpp
+ ./client/occupancy_grid/Writer.cpp
+ ./OccupancyGridHelper.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/LaserScan.xml
+ aron/OccupancyGrid.xml
+)
+
+add_library(
+ RobotAPI::armem_vision
+ ALIAS
+ armem_vision
+)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f88cc75454fd6f83545c3207c1e58642bedcc225
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
@@ -0,0 +1,39 @@
+#include "OccupancyGridHelper.h"
+
+#include "types.h"
+
+namespace armarx
+{
+ OccupancyGridHelper::OccupancyGridHelper(const OccupancyGrid& occupancyGrid,
+ const Params& params) :
+ occupancyGrid(occupancyGrid), params(params)
+ {
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::knownCells() const
+ {
+ return (occupancyGrid.grid > 0.F).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::freespace() const
+ {
+ // matrix1 = matrix1 .unaryExpr(std::ptr_fun(ReplaceNanWithValue<1>));
+ // return (occupancyGrid.grid ).cast<bool>();
+
+ const auto isFree = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p < params.freespaceThreshold and p > 0.F); };
+
+ // TODO(fabian.reister): which one to choose?
+ // return occupancyGrid.grid.unaryExpr(isFree).cast<bool>();
+ return occupancyGrid.grid.unaryViewExpr(isFree).cast<bool>();
+ }
+
+ OccupancyGridHelper::BinaryArray OccupancyGridHelper::obstacles() const
+ {
+ const auto isOccupied = [&](OccupancyGrid::CellType p) -> float
+ { return static_cast<float>(p > params.occupiedThreshold); };
+
+ return occupancyGrid.grid.unaryViewExpr(isOccupied).cast<bool>();
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::armem
+{
+ struct OccupancyGrid;
+}
+
+namespace armarx
+{
+ using armarx::armem::OccupancyGrid;
+
+ namespace detail
+ {
+ struct OccupancyGridHelperParams
+ {
+ float freespaceThreshold = 0.45F;
+ float occupiedThreshold = 0.55F;
+ };
+ }
+
+ class OccupancyGridHelper
+ {
+ public:
+ using Params = detail::OccupancyGridHelperParams;
+
+ OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+
+ using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
+
+ BinaryArray knownCells() const;
+ BinaryArray freespace() const;
+ BinaryArray obstacles() const;
+
+ private:
+ const OccupancyGrid& occupancyGrid;
+ const Params params;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
similarity index 83%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index 659df4a1536b0f81b81ba64f77e4049ba42fdd5c..f2d11c2e4111dfd0c24896ba807aef6435b7014b 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::arondto::SensorHeader">
+ <Object name="armarx::armem::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::arondto::SensorHeader />
+ <armarx::armem::arondto::SensorHeader />
</ObjectChild>
<!--
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0c508a4e2138b4b04c126287bf46d8826fb3da6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -0,0 +1,30 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::arondto::OccupancyGrid'>
+ <ObjectChild key='resolution'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+
+ <!--
+ <ObjectChild key='grid'>
+ <NdArray />
+ </ObjectChild> -->
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
similarity index 66%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 9b45357198639190d12b9a896705885f8046ea85..47ada08bb62a3ae78fe9520255be23b1f92e7612 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,22 +5,21 @@
#include <iterator>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
#include "types.h"
-
-namespace armarx
+namespace armarx::armem
{
/************ fromAron ************/
SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
{
-
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
.timestamp = aronSensorHeader.timestamp};
}
@@ -31,8 +30,10 @@ namespace armarx
// laserScan.data = fromAron(aronLaserScan.data);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
- std::int64_t& timestamp, std::string& frame,
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
std::string& agentName)
{
const auto header = fromAron(aronLaserScan.header);
@@ -40,14 +41,12 @@ namespace armarx
// laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp.toMicroSeconds();
- frame = header.frame;
+ frame = header.frame;
agentName = header.agent;
}
-
/************ toAron ************/
-
// auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
// {
// aronLaserScan.scan = toAron(laserScan);
@@ -62,8 +61,8 @@ namespace armarx
{
arondto::SensorHeader aronSensorHeader;
- aronSensorHeader.agent = sensorHeader.agent;
- aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
aronSensorHeader.timestamp = sensorHeader.timestamp;
return aronSensorHeader;
@@ -86,9 +85,26 @@ namespace armarx
{
.agent = agentName, .frame = frame, .timestamp = timestamp};
- const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+ const LaserScanStamped laserScanStamped{.header = header,
+ .data = laserScan};
toAron(laserScanStamped, aronLaserScanStamped);
}
-} // namespace armarx
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.pose, bo.pose);
+ aron::toAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.pose, bo.pose);
+ aron::fromAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..db97ad5076457805d953100201821f912a046291
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -0,0 +1,82 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+namespace armarx::armem
+{
+
+ namespace arondto
+ {
+ struct LaserScanStamped;
+ } // namespace arondto
+
+ // struct LaserScan;
+ struct LaserScanStamped;
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName);
+
+ template <typename T>
+ auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(
+ navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScanStamped& laserScan);
+
+ void toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScan);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr
+ toAron(const LaserScan& laserScan)
+ {
+ using aron::converter::AronVectorConverter;
+ return AronVectorConverter::ConvertFromVector(laserScan);
+ }
+
+ // OccupancyGrid
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr
+ toAron(const OccupancyGrid::Grid& grid)
+ {
+ return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+ }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index c3301dc95ccd2e63191821dd7e9fecf29dce5d33..9c46b0c4729dfe6ca038c1f31ef81255f56d4968 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -1,31 +1,32 @@
-#include "MappingDataReader.h"
+#include "Reader.h"
// STD / STL
-#include <cstring>
-#include <vector>
#include <algorithm>
+#include <cstring>
#include <map>
#include <optional>
#include <ostream>
-#include <type_traits>
#include <utility>
+#include <vector>
+
+#include <type_traits>
// ICE
-#include <IceUtil/Time.h>
#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
// ArmarXCore
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
// RobotAPI Aron
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -37,72 +38,72 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-
#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
- MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
- : memoryClient(memoryClient) {}
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Reader::~Reader() = default;
- MappingDataReader::~MappingDataReader() = default;
- void MappingDataReader::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
- def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
"Name of the mapping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- void MappingDataReader::connect()
+ void Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
<< properties.memoryName << "' ...";
- auto result = memoryClient.useMemory(properties.memoryName);
- if (not result.success)
+ try
{
- ARMARX_ERROR << result.errorMessage;
+ memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
return;
}
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '"
- << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
}
- armem::client::query::Builder
- MappingDataReader::buildQuery(const Query& query) const
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
{
- armem::client::query::Builder qb;
+ armarx::armem::client::query::Builder qb;
ARMARX_INFO << "Query for agent: " << query.agent
- << " memory name: " << properties.mappingMemoryName;
+ << " memory name: " << properties.memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().all()
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -112,7 +113,7 @@ namespace armarx::armem
{
// clang-format off
qb
- .coreSegments().withName(properties.mappingMemoryName)
+ .coreSegments().withName(properties.memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
.snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -120,10 +121,10 @@ namespace armarx::armem
}
return qb;
-
}
- std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
+ std::vector<LaserScanStamped>
+ asLaserScans(const std::map<std::string, wm::Entity>& entities)
{
std::vector<LaserScanStamped> outV;
@@ -132,25 +133,27 @@ namespace armarx::armem
ARMARX_WARNING << "No entities!";
}
- const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert =
+ [](const arondto::LaserScanStamped & aronLaserScanStamped,
+ const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
- const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+ const auto ndArrayNavigator =
+ aron::datanavigator::NDArrayNavigator::DynamicCast(
+ ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
ARMARX_IMPORTANT << "4";
-
return laserScanStamped;
-
};
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
if (entity.empty())
{
@@ -159,11 +162,12 @@ namespace armarx::armem
ARMARX_DEBUG << "History size: " << entity.size();
- for (const auto &[ss, entitySnapshot] : entity)
+ for (const auto& [ss, entitySnapshot] : entity)
{
for (const auto& entityInstance : entitySnapshot.instances())
{
- const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+ const auto o =
+ tryCast<arondto::LaserScanStamped>(entityInstance);
if (o)
{
@@ -176,8 +180,7 @@ namespace armarx::armem
return outV;
}
- MappingDataReader::Result
- MappingDataReader::queryData(const Query& query) const
+ Reader::Result Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
@@ -192,25 +195,25 @@ namespace armarx::armem
{
ARMARX_WARNING << "Failed to query data from memory: "
<< qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
// now create result from memory
const auto& entities =
- qResult.memory.getCoreSegment(properties.mappingMemoryName)
+ qResult.memory.getCoreSegment(properties.memoryName)
.getProviderSegment(query.agent)
.entities();
const auto laserScans = asLaserScans(entities);
- const auto sensors = simox::alg::get_keys(entities);
+ const auto sensors = simox::alg::get_keys(entities);
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
similarity index 78%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index aee783764f4b5720e4ae0d8d69fbf6faed297e98..138717c42257e02275a15785940783330126ee4d 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -21,7 +21,7 @@
#pragma once
-#include <stdint.h>
+#include <cstdint>
#include <string>
#include <vector>
@@ -30,8 +30,8 @@
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx
@@ -39,7 +39,7 @@ namespace armarx
class ManagedIceObject;
}
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
struct TimeRange
@@ -59,16 +59,15 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader
+ class Reader
{
public:
- MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
- virtual ~MappingDataReader();
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
void connect();
-
struct Query
{
std::string agent;
@@ -77,7 +76,6 @@ namespace armarx::armem
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
-
};
struct Result
@@ -94,32 +92,29 @@ namespace armarx::armem
} status;
std::string errorMessage;
-
};
Result queryData(const Query& query) const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
- client::query::Builder buildQuery(const Query& query) const ;
-
+ armarx::armem::client::query::Builder
+ buildQuery(const Query& query) const;
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
-
- armem::ClientReaderComponentPluginUser& memoryClient;
};
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7358e504ffecf3a2a710bd77b1631437e3818a42
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -0,0 +1,100 @@
+#include "Writer.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+
+
+namespace armarx::armem::vision::laser_scans::client
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem)
+ : memoryNameSystem(memoryNameSystem) {}
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
+ << properties.memoryName << "' ...";
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
+ << properties.memoryName;
+ }
+
+ bool Writer::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ const auto result =
+ memoryWriter.addSegment(properties.memoryName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::LaserScanStamped aronSensorData;
+ // currently only sets the header
+ toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
+
+ auto dict = aronSensorData.toAron();
+ dict->addElement("scan", toAron(laserScan));
+
+ update.instancesData = {dict};
+ 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;
+ }
+
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
similarity index 65%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index e5de996746b7d54b46faa891cf2cd26007fbaf02..afc383634b87a31fc84a841a5658f83ec845ee27 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -24,13 +24,14 @@
#include <mutex>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
{
/**
@@ -44,11 +45,13 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter
+ class Writer
{
public:
- MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
- virtual ~MappingDataWriter();
+
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
void connect();
@@ -57,27 +60,29 @@ namespace armarx::armem
// void connect() override;
/// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+ void registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def) /*override*/;
- bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+ bool storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string mappingMemoryName = "Mapping";
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScans";
} properties;
std::mutex memoryWriterMutex;
+ const std::string propertyPrefix = "mem.vision.";
- const std::string propertyPrefix = "mem.mapping.";
-
- armem::ClientWriterComponentPluginUser& component;
};
-
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba7edf547d1d45873609d723b92934b18d7f5c88
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <utility>
+#include <vector>
+
+#include <type_traits>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().memoryName)
+ .providerSegments().withName(query.providerName)
+ .entities().all()
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+ OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+ {
+ ARMARX_CHECK(not entities.empty()) << "No entities";
+ ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+
+ const wm::Entity& entity = entities.begin()->second;
+ ARMARX_CHECK(not entity.empty()) << "No snapshots";
+
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ const auto& entityInstance = entitySnapshot.instances().front();
+
+ const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+ ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+
+ OccupancyGrid occupancyGrid;
+ fromAron(*aronDto, occupancyGrid);
+
+ // direct access to grid data
+ const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+ entityInstance.data()->getElement("grid"));
+ ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
+
+ return occupancyGrid;
+ }
+
+ Reader::Result Reader::query(const Query& query) const
+ {
+ const auto qb = buildQuery(query);
+
+ ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult =
+ memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: "
+ << qResult.errorMessage;
+ return {.occupancyGrid = std::nullopt,
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
+ .getProviderSegment(query.providerName)
+ .entities();
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities.";
+ return {.occupancyGrid = std::nullopt,
+ .status = Result::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ const auto occupancyGrid = asOccupancyGrid(entities);
+ return Result{.occupancyGrid = occupancyGrid,
+ .status = Result::Status::Success};
+ }
+ catch (...)
+ {
+ return Result{.status = Result::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..a43c2c7c1151223358d5b1ab608f824fd3cdf66f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -0,0 +1,74 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ };
+
+ struct Result
+ {
+ std::optional<OccupancyGrid> occupancyGrid = std::nullopt;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ Result query(const Query& query) const;
+
+ ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2930f14f479d9ae68ee222f2643edf327392a43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -0,0 +1,70 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+ Writer::~Writer() = default;
+
+ bool Writer::store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ arondto::OccupancyGrid aronGrid;
+ // currently only sets the header
+ toAron(aronGrid, grid);
+
+ auto dict = aronGrid.toAron();
+ dict->addElement("grid", toAron(grid.grid));
+
+ update.instancesData = {dict};
+ 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;
+ }
+
+ std::string Writer::propertyPrefix() const
+ {
+ return "mem.vision.occupancy_grid.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+
+ return SimpleWriterBase::Properties{.memoryName = "Vision",
+ .coreSegmentName = "OccupancyGrid"};
+ }
+} // namespace armarx::armem::vision::occupancy_grid::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf1268444c333d10bb3c2f9efb68da11fe697cc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -0,0 +1,62 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const OccupancyGrid& grid,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_vision/types.h
similarity index 73%
rename from source/RobotAPI/libraries/armem_robot_mapping/types.h
rename to source/RobotAPI/libraries/armem_vision/types.h
index d822597e103177b0833014d25f0530e25f2b2075..dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -21,10 +21,12 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <vector>
+
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx
+namespace armarx::armem
{
struct SensorHeader
@@ -40,4 +42,20 @@ namespace armarx
LaserScan data;
};
-} // namespace armarx
+ // template<typename _ValueT = float>
+ struct OccupancyGrid
+ {
+ float resolution;
+
+ std::string frame;
+ Eigen::Affine3f pose;
+
+ // using ValueType = _ValueT;
+ using CellType = float;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+
+ Grid grid;
+ };
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 787ae30a23f097821989fcd9658a3fa31d6b4761..4a5a02e56edc8fae399b2a216b4ee77a6f4536d8 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -28,6 +28,7 @@
// Eigen
#include <Eigen/Geometry>
#include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -103,9 +104,26 @@ namespace armarx::aron::converter
return ConvertToMatrix<T, Rows, Cols>(*nav);
}
- template<typename T, int Rows, int Cols>
+ template<typename T>
+ static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+ MatrixT ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
{
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicMatrix<T>(nav);
+ }
+
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
auto dims = nav.getDimensions();
@@ -114,6 +132,61 @@ namespace armarx::aron::converter
return ret;
}
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+ // Eigen::Array
+
+ template<typename T>
+ static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ const auto dims = nav.getDimensions();
+
+ 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<>()));
+ return ret;
+ }
+
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+ static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+ {
+ if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+ {
+ return ConvertToDynamicArray<T>(nav);
+ }
+
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ auto dims = nav.getDimensions();
+
+ Eigen::Array<T, Rows, Cols> ret;
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ return ret;
+ }
+
+ template<typename T>
+ static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ {
+ datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
+
+
private:
/**
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 0b2dd67089394ee5c219ec66d919eb4d07a4f927..d3b4eb51940bfdf8dbefb2daf7809c1e9d27c7a4 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
#include <memory>
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 4e17ef0d016a6c12400b4192e993f4e9b10993ae..9e183e77050ab09abf3a5e1b1daa309b087eccf5 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -27,7 +27,6 @@
#include "MultiDimPIDController.h"
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
#include <Eigen/Core>
@@ -36,6 +35,11 @@
namespace armarx
{
+ namespace rtfilters
+ {
+ class RTFilterBase;
+ }
+
class PIDController :
public Logging
{
@@ -77,8 +81,8 @@ namespace armarx
bool firstRun;
bool limitless;
bool threadSafe = true;
- rtfilters::RTFilterBasePtr differentialFilter;
- rtfilters::RTFilterBasePtr pdOutputFilter;
+ std::shared_ptr<rtfilters::RTFilterBase> differentialFilter;
+ std::shared_ptr<rtfilters::RTFilterBase> pdOutputFilter;
private:
using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index 7a7fb5c8dd21363e246ad3a45bfbcdb304640f8b..4944430b100596e3336d6feeb2b45870b5258d8c 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -22,9 +22,9 @@
#pragma once
-#include <math.h>
-#include <Eigen/Eigen>
+#include <Eigen/Core>
#include <vector>
+#include <math.h>
namespace armarx::math
{
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 91f15876447e836e6a7e5f883747ae613765a0e5..65db64e872112548b72048542cfdecb7835cd81f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -90,7 +90,7 @@ namespace armarx
return _root;
}
- bool RemoteRobot::hasRobotNode(const std::string& robotNodeName)
+ bool RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
{
if (_cachedNodes.find(name) == _cachedNodes.end())
{
@@ -103,7 +103,7 @@ namespace armarx
}
- bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode)
+ bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
{
return this->hasRobotNode(robotNode->getName());
@@ -159,7 +159,7 @@ namespace armarx
}
}
- bool RemoteRobot::hasRobotNodeSet(const std::string& name)
+ bool RemoteRobot::hasRobotNodeSet(const std::string& name) const
{
return _robot->hasRobotNodeSet(name);
}
@@ -539,17 +539,17 @@ namespace armarx
// Private (unused methods)
- bool RemoteRobot::hasEndEffector(const std::string& endEffectorName)
+ bool RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
{
return false;
}
- EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName)
+ EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName) const
{
return EndEffectorPtr();
}
- void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) {}
+ void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const {}
void RemoteRobot::setRootNode(RobotNodePtr node) {}
void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index b687f0e0fc8cc1eac7b967b10408ec8b63440f56..734db7407354682df6fa45c866acdb0e9c8c0a6f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -145,13 +145,13 @@ namespace armarx
VirtualRobot::RobotNodePtr getRootNode() const override;
- bool hasRobotNode(const std::string& robotNodeName) override;
- bool hasRobotNode(VirtualRobot::RobotNodePtr) override;
+ bool hasRobotNode(const std::string& robotNodeName) const override;
+ bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
- bool hasRobotNodeSet(const std::string& name) override;
+ bool hasRobotNodeSet(const std::string& name) const override;
VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
@@ -223,11 +223,11 @@ namespace armarx
protected:
/// Not implemented yet
- bool hasEndEffector(const std::string& endEffectorName) override;
+ bool hasEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) override;
+ VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) override;
+ void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
/// Not implemented yet
void setRootNode(VirtualRobot::RobotNodePtr node) override;