diff --git a/scenarios/AzureKinectCameraOnTripod/AzureKinectCameraOnTripod.scx b/scenarios/AzureKinectCameraOnTripod/AzureKinectCameraOnTripod.scx
new file mode 100644
index 0000000000000000000000000000000000000000..499fbd36f8822e358aa60285dda1202b69e4777b
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/AzureKinectCameraOnTripod.scx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="AzureKinectCameraOnTripod" creation="2022-09-27.18:29:10" globalConfigName="./config/global.cfg" package="VisionX" deploymentType="local" nodeName="NodeMain">
+ <application name="AzureKinectPointCloudProviderApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="SimpleVirtualRobot" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="PointCloudToArVizApp" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/ArVizStorage.cfg b/scenarios/AzureKinectCameraOnTripod/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArVizStorage.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history is serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/AzureKinectPointCloudProviderApp.cfg b/scenarios/AzureKinectCameraOnTripod/config/AzureKinectPointCloudProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..3e52361297e493db5d1d03430cfec43bee229ef9
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/AzureKinectPointCloudProviderApp.cfg
@@ -0,0 +1,332 @@
+# ==================================================================
+# AzureKinectPointCloudProviderApp 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.AzureKinectPointCloudProvider.CaptureTimeOffset: In Milliseconds. Time offset between capturing the image on the hardware and receiving the image in this process.
+# Attributes:
+# - Default: 16
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.CaptureTimeOffset = 16
+
+
+# ArmarX.AzureKinectPointCloudProvider.ColorResolution: Resolution of the RGB camera image.
+# Attributes:
+# - Default: 720
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1080, 1440, 1536, 2160, 3072, 720}
+# ArmarX.AzureKinectPointCloudProvider.ColorResolution = 720
+
+
+# ArmarX.AzureKinectPointCloudProvider.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.AzureKinectPointCloudProvider.DepthMode: Resolution/mode of the depth camera image.
+# Attributes:
+# - Default: NFOV_UNBINNED
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {NFOV_2X2BINNED, NFOV_UNBINNED, OFF, PASSIVE_IR, WFOV_2X2BINNED, WFOV_UNBINNED}
+# ArmarX.AzureKinectPointCloudProvider.DepthMode = NFOV_UNBINNED
+
+
+# ArmarX.AzureKinectPointCloudProvider.EnableColorUndistortion: Undistort the color images using the full 8 radial and tangential distortion parameters provided by the Azure Kinect.
+# This can help for processing tasks which cannot handle radial parameters k3-k6.
+# Note that this drastically reduces the FPS (to something like 3).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.AzureKinectPointCloudProvider.EnableColorUndistortion = 1
+
+
+# ArmarX.AzureKinectPointCloudProvider.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.AzureKinectPointCloudProvider.EnableProfiling = false
+
+
+# ArmarX.AzureKinectPointCloudProvider.ExternalCalibrationFilePath: Path to an optional external calibration file, which has a camera matrix and distortion parameters.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.ExternalCalibrationFilePath = ""
+
+
+# ArmarX.AzureKinectPointCloudProvider.MaxDepth: Max. allowed depth value in mm. Depth values above this threshold will be set to nan.
+# Attributes:
+# - Default: 6000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.MaxDepth = 6000
+
+
+# ArmarX.AzureKinectPointCloudProvider.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.AzureKinectPointCloudProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.AzureKinectPointCloudProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.ObjectName = ""
+
+
+# ArmarX.AzureKinectPointCloudProvider.bodyCameraFrameName:
+# Attributes:
+# - Default: AzureKinectDepthCamera
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.bodyCameraFrameName = AzureKinectDepthCamera
+
+
+# ArmarX.AzureKinectPointCloudProvider.bodyTrackingModelPath: Path where the .onnx DNN files can be found
+# Attributes:
+# - Default: ${K4A_BODY_TRACKING_DNN_MODEL_FILEPATH}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.bodyTrackingModelPath = ${K4A_BODY_TRACKING_DNN_MODEL_FILEPATH}
+
+
+# ArmarX.AzureKinectPointCloudProvider.cmp.HumanMemoryServer: Ice object name of the `HumanMemoryServer` component.
+# Attributes:
+# - Default: HumanMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.cmp.HumanMemoryServer = HumanMemory
+
+
+# ArmarX.AzureKinectPointCloudProvider.device_id: ID of the device.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.device_id = 0
+
+
+# ArmarX.AzureKinectPointCloudProvider.frameName: name of the source
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.frameName = ""
+
+
+# ArmarX.AzureKinectPointCloudProvider.framerate: framerate for the point clouds
+# Attributes:
+# - Default: 30
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.framerate = 30
+
+
+# ArmarX.AzureKinectPointCloudProvider.isEnabled: enable the capturing process immediately
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AzureKinectPointCloudProvider.isEnabled = true
+
+
+# ArmarX.AzureKinectPointCloudProvider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AzureKinectPointCloudProvider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.AzureKinectPointCloudProvider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AzureKinectPointCloudProvider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.AzureKinectPointCloudProvider.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.AzureKinectPointCloudProvider.robotName = AzureKinectCamera
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/DebugObserver.cfg b/scenarios/AzureKinectCameraOnTripod/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/HumanMemoryApp.cfg b/scenarios/AzureKinectCameraOnTripod/config/HumanMemoryApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..28099ebc83514ac57b5af9e5acba37e72a6f3846
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/HumanMemoryApp.cfg
@@ -0,0 +1,368 @@
+# ==================================================================
+# HumanMemoryApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.HumanMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.HumanMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.HumanMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.HumanMemory.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.HumanMemory.EnableProfiling = false
+
+
+# ArmarX.HumanMemory.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.HumanMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HumanMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ObjectName = ""
+
+
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize: Maximal size of the FaceRecognition entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.face.seg.CoreSegmentName: Name of the FaceRecognition core segment.
+# Attributes:
+# - Default: FaceRecognition
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreSegmentName = FaceRecognition
+
+
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize: Maximal size of the Identification entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName: Name of the Identification core segment.
+# Attributes:
+# - Default: Identification
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName = Identification
+
+
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize: Maximal size of the PersonInstance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 32
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize = 32
+
+
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName: Name of the PersonInstance core segment.
+# Attributes:
+# - Default: PersonInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName = PersonInstance
+
+
+# ArmarX.HumanMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.MemoryName = Human
+
+
+# ArmarX.HumanMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.HumanMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.mem.ltm.enabled = false
+
+
+# ArmarX.HumanMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.HumanMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.HumanMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize: Maximal size of the Pose entity histories (-1 for infinite).
+# Attributes:
+# - Default: 256
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize = 256
+
+
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName: Name of the Pose core segment.
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName = Pose
+
+
+# ArmarX.HumanMemory.profile.pk.load: Load profiles from prior knowledge on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.profile.pk.load = true
+
+
+# ArmarX.HumanMemory.profile.pk.packageName: ArmarX package to load human profiles from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.pk.packageName = PriorKnowledgeData
+
+
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize: Maximal size of the Profile entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName: Name of the Profile core segment.
+# Attributes:
+# - Default: Profile
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName = Profile
+
+
+# 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/AzureKinectCameraOnTripod/config/MemoryNameSystem.cfg b/scenarios/AzureKinectCameraOnTripod/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/ObjectMemory.cfg b/scenarios/AzureKinectCameraOnTripod/config/ObjectMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cbbb41a8c878ca0031de20838704a5a39ffc90ca
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/ObjectMemory.cfg
@@ -0,0 +1,717 @@
+# ==================================================================
+# ObjectMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObjectMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectMemory.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.EnableProfiling = false
+
+
+# ArmarX.ObjectMemory.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
+
+
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
+# Attributes:
+# - Default: KinematicUnitObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+
+
+# ArmarX.ObjectMemory.mem..marker.Name: Marker Memory Name
+# Attributes:
+# - Default: Marker
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.Name = Marker
+
+
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize: Maximum marker memory history size
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.MemoryName = Object
+
+
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Attachments
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName = Attachments
+
+
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Building/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
+
+
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize: Maximal size of the Class entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName: Name of the Class core segment.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName = Class
+
+
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName: Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
+# Attributes:
+# - Default: Neck_2_Pitch
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
+
+
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# Attributes:
+# - Default: 0.0500000007
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+
+
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
+# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory: Directory in Package/data/Package/ containing the scene snapshots.
+# Attributes:
+# - Default: scenes
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/scenes/'.
+# You can also specify a ; separated list of scenes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName: Name of the Instance core segment.
+# Attributes:
+# - Default: Instance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName = Instance
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position: Enable showing pose gaussians (orientation part).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced: Displace center orientation (co)variance circle arrows along their rotation axis.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale: Scaling of pose gaussians (orientation part).
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha: Alpha of linear prediction ghosts.
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow: Show arrows from current object poses to the linearly predicted ones.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame: Show frames at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost: Show ghosts at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow: The time window (in seconds) into the past to perform the regression on.
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow = 2
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ObjectMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/PointCloudToArVizApp.cfg b/scenarios/AzureKinectCameraOnTripod/config/PointCloudToArVizApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..ddf4e7b5713ab1d938180c78653d6694d555be7d
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/PointCloudToArVizApp.cfg
@@ -0,0 +1,322 @@
+# ==================================================================
+# PointCloudToArVizApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.PointCloudToArViz.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.PointCloudToArViz.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.PointCloudToArViz.AutomaticTypeConversion: Automatically convert different point cloud types.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.PointCloudToArViz.AutomaticTypeConversion = false
+
+
+# ArmarX.PointCloudToArViz.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.PointCloudToArViz.EnableProfiling = false
+
+
+# ArmarX.PointCloudToArViz.FrameRate: Number of frames per second. If zero then this property is ignored.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.FrameRate = 0
+
+
+# ArmarX.PointCloudToArViz.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.PointCloudToArViz.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.PointCloudToArViz.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.ObjectName = ""
+
+
+# ArmarX.PointCloudToArViz.ProviderName: Name(s) of the point cloud provider(s).
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.PointCloudToArViz.ProviderName = AzureKinectPointCloudProvider
+
+
+# ArmarX.PointCloudToArViz.ProviderWaitMs: Time to wait for each point cloud provider [ms].
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.ProviderWaitMs = 100
+
+
+# ArmarX.PointCloudToArViz.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.PointCloudToArViz.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.PointCloudToArViz.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.PointCloudToArViz.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.PointCloudToArViz.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.PointCloudToArViz.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.PointCloudToArViz.pointCloudNodeName: If the point cloud header does not provide a frame id, this info is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.PointCloudToArViz.pointCloudNodeName = AzureKinectCamera
+
+
+# ArmarX.PointCloudToArViz.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.PointCloudToArViz.robotName = AzureKinectCamera
+
+
+# ArmarX.PointCloudToArViz.viz.checkFinite: Enable to check points for being finite.
+# Enable this option if the drawn point cloud causes the screen to become purely white.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.PointCloudToArViz.viz.checkFinite = false
+
+
+# ArmarX.PointCloudToArViz.viz.labeled: If true and point cloud is labeled, draw colors according to labels.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.PointCloudToArViz.viz.labeled = false
+
+
+# ArmarX.PointCloudToArViz.viz.pointSizeInPixels: The point size in pixels.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.PointCloudToArViz.viz.pointSizeInPixels = 1
+
+
+# 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/AzureKinectCameraOnTripod/config/RemoteGuiProviderApp.cfg b/scenarios/AzureKinectCameraOnTripod/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteGuiProvider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/RobotStateMemory.cfg b/scenarios/AzureKinectCameraOnTripod/config/RobotStateMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6628b865775889f4fb61656c27a3bb98d59b3d2f
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/RobotStateMemory.cfg
@@ -0,0 +1,370 @@
+# ==================================================================
+# RobotStateMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.RobotStateMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.RobotStateMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotStateMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.RobotStateMemory.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.EnableProfiling = false
+
+
+# ArmarX.RobotStateMemory.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RobotStateMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ObjectName = ""
+
+
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix: Prefix of all sensor values.
+# Attributes:
+# - Default: sens.*
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix = sens.*
+
+
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency: The frequency to store values in Hz. All other values get discarded. Minimum is 1, max is 100.000000.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency = 50
+
+
+# ArmarX.RobotStateMemory.RobotUnitName: Name of the RobotUnit
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnitName = ""
+
+
+# ArmarX.RobotStateMemory.WaitForRobotUnit: Add the robot unit as dependency to the component. This memory requires a running RobotUnit, therefore we should add it as explicit dependency.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.WaitForRobotUnit = false
+
+
+# ArmarX.RobotStateMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.MemoryName = RobotState
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize: Maximal size of the Description entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName: Name of the Description core segment.
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName = Description
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize: Maximal size of the Localization entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName: Name of the Localization core segment.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName = Localization
+
+
+# ArmarX.RobotStateMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.RobotStateMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.ltm.enabled = false
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize: Maximal size of the Proprioception entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName: Name of the Proprioception core segment.
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName = Proprioception
+
+
+# ArmarX.RobotStateMemory.mem.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.enabled = true
+
+
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled: Enable or disable visualization of frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled = false
+
+
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz = 25
+
+
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStateMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.prediction.TimeWindow = 2
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/SimpleVirtualRobot.cfg b/scenarios/AzureKinectCameraOnTripod/config/SimpleVirtualRobot.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cf71edd78959335b6a7476a97fbf15746920ef57
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/SimpleVirtualRobot.cfg
@@ -0,0 +1,255 @@
+# ==================================================================
+# SimpleVirtualRobot properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.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.SimpleVirtualRobot.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.SimpleVirtualRobot.EnableProfiling = false
+
+
+# ArmarX.SimpleVirtualRobot.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.SimpleVirtualRobot.MinimumLoggingLevel = Verbose
+
+
+# ArmarX.SimpleVirtualRobot.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.ObjectName = ""
+
+
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.SimpleVirtualRobot.p.oneShot: If true, commit once after connecting, then stop.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.SimpleVirtualRobot.p.oneShot = false
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues: Specify a certain joint configuration.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.jointValues = {"mount_joint_height": 0,"mount_joint_pitch": 1.9}
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.name: Optional override for the robot name. If not set, the default name from the robot model is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.name = AzureKinectCamera
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.package: Package of the Simox robot XML.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.package = PriorKnowledgeData
+
+
+# ArmarX.SimpleVirtualRobot.p.robot.path: Local path of the Simox robot XML.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.SimpleVirtualRobot.p.robot.path = robots/AzureKinectOnTripod/AzureKinectOnTripod.xml
+
+
+# ArmarX.SimpleVirtualRobot.p.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.p.updateFrequency = 10
+
+
+# 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 = Verbose
+
+
diff --git a/scenarios/AzureKinectCameraOnTripod/config/global.cfg b/scenarios/AzureKinectCameraOnTripod/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..2d65808797a77b0acab22d27eefde41e6e1487d4
--- /dev/null
+++ b/scenarios/AzureKinectCameraOnTripod/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario AzureKinectCameraOnTripod
+# ==================================================================
+
diff --git a/source/VisionX/components/armem/server/HumanMemory/CMakeLists.txt b/source/VisionX/components/armem/server/HumanMemory/CMakeLists.txt
index d39e993f49c9f9b611910c5fbbb39f5b1c953b61..db88010d0677b4960a07665a5345f4efefb221b4 100644
--- a/source/VisionX/components/armem/server/HumanMemory/CMakeLists.txt
+++ b/source/VisionX/components/armem/server/HumanMemory/CMakeLists.txt
@@ -1,16 +1,31 @@
armarx_component_set_name("HumanMemory")
+
+armarx_add_component_interface_lib(
+ SLICE_FILES
+ ComponentInterface.ice
+ ICE_LIBS
+ ArmarXCore
+ RobotAPI
+ VisionX
+ armem_human_server
+ armem_human_serverInterfaces
+)
+
set(COMPONENT_LIBS
# ArmarXCore
- ArmarXCore ArmarXCoreInterfaces
+ ArmarXCore ArmarXCoreInterfaces ArmarXCoreComponentPlugins
# ArmarXGui
ArmarXGuiComponentPlugins
# RobotAPI
- RobotAPICore RobotAPIInterfaces
+ RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins
armem_server
# VisionX
VisionXCore VisionXInterfaces
armem_human_server
+ HumanMemoryInterfaces
+ armem_robot_state
+ armem_robot
)
set(SOURCES
@@ -23,7 +38,4 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
#generate the application
-armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server APPLICATION_APP_SUFFIX)
-
-
-
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server::human_memory APPLICATION_APP_SUFFIX)
diff --git a/source/VisionX/components/armem/server/HumanMemory/ComponentInterface.ice b/source/VisionX/components/armem/server/HumanMemory/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..38d02b307d7ded2827830e3baa93a2eef3dbb076
--- /dev/null
+++ b/source/VisionX/components/armem/server/HumanMemory/ComponentInterface.ice
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+#include <VisionX/libraries/armem_human/server/HumanMemoryServerInterface.ice>
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+ module human_memory
+ {
+
+ interface ComponentInterface extends
+ ::armarx::armem::server::MemoryInterface,
+ ::armarx::armem::human::HumanMemoryServerInterface
+ {
+
+ }
+
+ }
+ }
+ }
+}
diff --git a/source/VisionX/components/armem/server/HumanMemory/HumanMemory.cpp b/source/VisionX/components/armem/server/HumanMemory/HumanMemory.cpp
index 65e61171c670884add0fa41cd3558d55714a55a5..cfd0f2ebe1d7408c0c1e6ab19e14670bd22097e2 100644
--- a/source/VisionX/components/armem/server/HumanMemory/HumanMemory.cpp
+++ b/source/VisionX/components/armem/server/HumanMemory/HumanMemory.cpp
@@ -22,15 +22,31 @@
#include "HumanMemory.h"
+#include <qnamespace.h>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/interface/armem/commit.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include "VisionX/libraries/armem_human/server/PoseSegment.h"
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
-namespace armarx::armem::server
+
+namespace armarx::armem::server::human_memory
{
- armarx::PropertyDefinitionsPtr HumanMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ HumanMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
setMemoryName("Human");
@@ -49,41 +65,219 @@ namespace armarx::armem::server
personInstanceSegment(iceAdapter()),
poseSegment(iceAdapter()),
faceRecognitionSegment(iceAdapter()),
- identificationSegment(iceAdapter())
+ identificationSegment(iceAdapter()),
+ visu(poseSegment)
{
+ addPlugin(virtualRobotReaderPlugin);
}
- std::string HumanMemory::getDefaultName() const
+ std::string
+ HumanMemory::getDefaultName() const
{
return "HumanMemory";
}
- void HumanMemory::onInitComponent()
+ void
+ HumanMemory::onInitComponent()
{
profileSegment.init();
personInstanceSegment.init();
poseSegment.init();
faceRecognitionSegment.init();
identificationSegment.init();
+ visu.init();
workingMemory().addCoreSegment("RecognizedAction");
}
- void HumanMemory::onConnectComponent()
+ void
+ HumanMemory::onConnectComponent()
+ {
+ visu.connect(arviz, getDebugObserver());
+ }
+
+
+ void
+ HumanMemory::onDisconnectComponent()
{
}
- void HumanMemory::onDisconnectComponent()
+ void
+ HumanMemory::onExitComponent()
{
}
+ const VirtualRobot::RobotPtr&
+ HumanMemory::getSynchronizedRobot(const std::string& robotName,
+ const armarx::DateTime& timestamp)
+ {
+ // seach and retrieve robot
+ auto& robot = [&]() -> VirtualRobot::RobotPtr&
+ {
+ // retrieve robot from memory if it is not in cache
+ if (robots.count(robotName) == 0)
+ {
+ auto newRobot = virtualRobotReaderPlugin->get().getRobot(robotName);
+ ARMARX_CHECK_NOT_NULL(newRobot)
+ << "Failed to obtain robot with name `" << robotName << "`.";
+
+ robots[robotName] = newRobot;
+ }
- void HumanMemory::onExitComponent()
+ return robots.at(robotName);
+ }();
+
+ // synchronize robot
+ virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp);
+
+ return robot;
+ }
+
+ void
+ HumanMemory::commitHumanPosesInCameraFrame(
+ const ::armarx::aron::data::dto::AronDictSeq& dictSeq,
+ const ::std::string& providerName,
+ const ::armarx::core::time::dto::DateTime& iceTimestamp,
+ const ::Ice::Current& /*c*/)
{
+ if (dictSeq.empty())
+ {
+ return;
+ }
+
+ // convert from ice
+ armarx::DateTime timestamp;
+ fromIce(iceTimestamp, timestamp);
+
+ // convert to our aron type
+ const auto toHumanPose = [](const ::armarx::aron::data::dto::DictPtr& dict)
+ { return armarx::human::arondto::HumanPose::FromAron(dict); };
+
+ std::vector<armarx::human::arondto::HumanPose> humanPoses =
+ simox::alg::apply(dictSeq, toHumanPose);
+
+ // synchronize robot
+ const std::string robotName =
+ humanPoses.front().keypoints.begin()->second.positionCamera.agent;
+
+ const auto robot = getSynchronizedRobot(robotName, timestamp);
+ ARMARX_CHECK_NOT_NULL(robot) << "Failed to obtain robot with name `" << robotName << "`.";
+
+ // fill global pose info
+ {
+ for (armarx::human::arondto::HumanPose& humanPose : humanPoses)
+ {
+ for (auto& [_, keyPoint] : humanPose.keypoints)
+ {
+ const auto& cameraFrame = keyPoint.positionCamera.frame;
+ const auto& agent = keyPoint.positionCamera.agent;
+
+ const FramedPosition keypoint(
+ keyPoint.positionCamera.toEigen(), cameraFrame, agent);
+
+ // global pose
+ {
+ keyPoint.positionGlobal = armarx::FramedPosition(
+ keypoint.toGlobalEigen(robot), armarx::GlobalFrame, agent);
+ }
+
+ // pose in root frame
+ {
+ keyPoint.positionRobot = armarx::FramedPosition(
+ keypoint.toRootEigen(robot), robot->getRootNode()->getName(), agent);
+ }
+
+ // Orientation information available.
+ if (keyPoint.orientationCamera.has_value())
+ {
+ ARMARX_CHECK_EQUAL(keyPoint.orientationCamera->frame,
+ keyPoint.positionCamera.frame);
+
+ const FramedPose keypointPose(keyPoint.orientationCamera->toEigen(),
+ keyPoint.positionCamera.toEigen(),
+ cameraFrame,
+ agent);
+
+ // global pose
+ {
+ const Eigen::Isometry3f global_T_human(
+ keypointPose.toGlobalEigen(robot));
+
+ keyPoint.orientationGlobal =
+ armarx::FramedOrientation(Eigen::Matrix3f{global_T_human.linear()},
+ armarx::GlobalFrame,
+ agent);
+ }
+
+ // pose in root frame
+ {
+ const Eigen::Isometry3f robot_root_T_human(
+ keypointPose.toRootEigen(robot));
+
+ keyPoint.orientationRobot = armarx::FramedOrientation(
+ Eigen::Matrix3f{robot_root_T_human.linear()},
+ robot->getRootNode()->getName(),
+ agent);
+ }
+ }
+ }
+ }
+ }
+
+ // commit human poses
+ commitHumanPoses(humanPoses, providerName, timestamp);
}
-}
+ void
+ HumanMemory::commitHumanPoses(
+ const std::vector<::armarx::human::arondto::HumanPose>& humanPoses,
+ const std::string& providerName,
+ const armarx::DateTime& timestamp)
+ {
+ armem::Commit commit;
+
+
+ // update.instancesData.reserve(humanPoses.size());
+ size_t humanPoseCounter = 0;
+ for (const auto& humanPose : humanPoses)
+ {
+ std::string entityName = "human_";
+ if (humanPose.humanTrackingId.has_value())
+ {
+ entityName += humanPose.humanTrackingId.value();
+ }
+ else
+ {
+ // aritrary ID to avoid the misinterpretation that the observations belonged to the
+ // same tracked person
+ entityName +=
+ timestamp.toDateTimeString() + "_pose" + std::to_string(humanPoseCounter);
+ ++humanPoseCounter;
+ }
+
+ const armarx::armem::MemoryID entityID(workingMemory().name(),
+ human::PoseSegment::CORE_SEGMENT_NAME,
+ providerName,
+ entityName);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ // update.confidence = humanPose.
+ update.timeSent = timestamp; // TODO this is not fully correct
+ update.timeCreated = timestamp;
+
+ update.instancesData.push_back(humanPose.toAron());
+ commit.add(update);
+ }
+
+
+ armem::data::Commit dtoCommit;
+ toIce(dtoCommit, commit);
+
+ HumanMemory::commit(dtoCommit);
+ }
+} // namespace armarx::armem::server::human_memory
diff --git a/source/VisionX/components/armem/server/HumanMemory/HumanMemory.h b/source/VisionX/components/armem/server/HumanMemory/HumanMemory.h
index ac6835813275d086d128b5174a94ca33bc368c7c..8a1b610cbf9c2ab305839dd60b7a2e1f72a09682 100644
--- a/source/VisionX/components/armem/server/HumanMemory/HumanMemory.h
+++ b/source/VisionX/components/armem/server/HumanMemory/HumanMemory.h
@@ -22,30 +22,41 @@
#pragma once
+#include <mutex>
+
+#include "ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-#include <VisionX/libraries/armem_human/server/PoseSegment.h>
+
+#include "VisionX/libraries/armem_human/server/Visualization.h"
+#include <VisionX/components/armem/server/HumanMemory/ComponentInterface.h>
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
+#include <VisionX/libraries/armem_human/server/HumanMemoryServerInterface.h>
#include <VisionX/libraries/armem_human/server/IdentificationSegment.h>
#include <VisionX/libraries/armem_human/server/PersonInstanceSegment.h>
-#include <VisionX/libraries/armem_human/server/profile/Segment.h>
+#include <VisionX/libraries/armem_human/server/PoseSegment.h>
#include <VisionX/libraries/armem_human/server/face_recognition/Segment.h>
+#include <VisionX/libraries/armem_human/server/profile/Segment.h>
-
-namespace armarx::armem::server
+namespace armarx::armem::server::human_memory
{
class HumanMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser,
+ virtual public ComponentInterface,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
{
public:
-
HumanMemory();
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -53,14 +64,33 @@ namespace armarx::armem::server
void onDisconnectComponent() override;
void onExitComponent() override;
+ const VirtualRobot::RobotPtr& getSynchronizedRobot(const std::string& robotName,
+ const armarx::DateTime& timestamp);
- private:
+ void
+ commitHumanPosesInCameraFrame(const ::armarx::aron::data::dto::AronDictSeq& dictSeq,
+ const ::std::string& providerName,
+ const ::armarx::core::time::dto::DateTime& timestamp,
+ const ::Ice::Current& /*c*/ = ::Ice::emptyCurrent) override;
+ void commitHumanPoses(const std::vector<::armarx::human::arondto::HumanPose>& humanPoses,
+ const std::string& providerName,
+ const armarx::DateTime& timestamp);
+
+ private:
human::ProfileSegment profileSegment;
human::PersonInstanceSegment personInstanceSegment;
human::PoseSegment poseSegment;
human::FaceRecognitionSegment faceRecognitionSegment;
human::IdentificationSegment identificationSegment;
+ human::Visu visu;
+
+ client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
+
+ std::mutex robotsMtx;
+ std::map<std::string, VirtualRobot::RobotPtr> robots;
};
-}
+
+} // namespace armarx::armem::server::human_memory
diff --git a/source/VisionX/components/image_processor/OpenPoseEstimation/Memory/RGBDPoseEstimationWithMemoryWriter/RGBDPoseEstimationWithMemoryWriter.cpp b/source/VisionX/components/image_processor/OpenPoseEstimation/Memory/RGBDPoseEstimationWithMemoryWriter/RGBDPoseEstimationWithMemoryWriter.cpp
index 885b951da7421c032e075b6c746094d4575614e0..539ffcdebf8d01e5e992788b60b0db0455238048 100644
--- a/source/VisionX/components/image_processor/OpenPoseEstimation/Memory/RGBDPoseEstimationWithMemoryWriter/RGBDPoseEstimationWithMemoryWriter.cpp
+++ b/source/VisionX/components/image_processor/OpenPoseEstimation/Memory/RGBDPoseEstimationWithMemoryWriter/RGBDPoseEstimationWithMemoryWriter.cpp
@@ -25,8 +25,9 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
-#include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/core/time/Clock.h>
using namespace visionx;
@@ -84,21 +85,19 @@ namespace armarx
.withTimestamp(ts);
update.timeCreated = ts;
- for (auto& [humanId, humanPose] : openposeResult3D)
+ for (auto& [_, humanPose] : openposeResult3D)
{
- (void) humanId;
auto pose = humanPose.keypointMap;
- armarx::human::arondto::Body25Pose3D dto;
+ armarx::human::arondto::HumanPose dto;
+ dto.poseModelId = op_settings.model_pose;
+ // OpenPose does not allow for tracking.
+ dto.humanTrackingId = std::nullopt;
for (auto& [name, keypoint] : pose)
{
- armarx::human::arondto::Keypoint3D kp;
+ armarx::human::arondto::PoseKeypoint kp;
kp.confidence = keypoint.confidence;
- kp.dominantColor.r = keypoint.dominantColor.r;
- kp.dominantColor.g = keypoint.dominantColor.g;
- kp.dominantColor.b = keypoint.dominantColor.b;
- kp.label = keypoint.label;
armarx::arondto::FramedPosition posGlobal;
posGlobal.header.agent = localRobot->getName();
@@ -112,19 +111,7 @@ namespace armarx
armarx::FramedPosition posRobot(pos.toRootFrame(localRobot)->toEigen(), localRobot->getRootNode()->getName(), localRobot->getName());
kp.positionRobot = posRobot;
- armarx::human::arondto::Keypoint2D kp2;
- kp2.confidence = openposeResult[humanId].keypointMap[name].confidence;
- kp2.dominantColor.r = openposeResult[humanId].keypointMap[name].dominantColor.r;
- kp2.dominantColor.g = openposeResult[humanId].keypointMap[name].dominantColor.g;
- kp2.dominantColor.b = openposeResult[humanId].keypointMap[name].dominantColor.b;
- kp2.x = openposeResult[humanId].keypointMap[name].x;
- kp2.y = openposeResult[humanId].keypointMap[name].y;
- kp2.label = openposeResult[humanId].keypointMap[name].label;
-
-
- dto.keypoints3D[kp.label] = kp;
- dto.keypoints2D[kp.label] = kp2;
-
+ dto.keypoints[keypoint.label] = kp;
}
update.instancesData.push_back(dto.toAron());
diff --git a/source/VisionX/components/image_processor/OpenPoseEstimation/_LegacyOpenPoseEstimation/OpenPoseEstimation.h b/source/VisionX/components/image_processor/OpenPoseEstimation/_LegacyOpenPoseEstimation/OpenPoseEstimation.h
index d029fbad309f58f0b2eb79a9fdf1c2ad4494f33f..90d07ec631d08ff437059cfed2f808c0d0c2c29e 100644
--- a/source/VisionX/components/image_processor/OpenPoseEstimation/_LegacyOpenPoseEstimation/OpenPoseEstimation.h
+++ b/source/VisionX/components/image_processor/OpenPoseEstimation/_LegacyOpenPoseEstimation/OpenPoseEstimation.h
@@ -33,6 +33,7 @@
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
+#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
#include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
@@ -103,7 +104,7 @@ namespace armarx
"If you want to change the initial calib->get scale, you actually want to multiply the "
"`net_resolution` by your desired initial scale.");
defineOptionalProperty<int>("OP_scale_number", 1, "Number of scales to average.");
- defineOptionalProperty<std::string>("OP_model_pose", "BODY_25", "Model to be used. E.g. `BODY_25` (25 keypoints, best model), `COCO` (18 keypoints), `MPI` (15 keypoints, ~10% faster), "
+ defineOptionalProperty<std::string>("OP_model_pose", human::pose::model::openpose_body_25::ModelId, "Model to be used. E.g. `BODY_25` (25 keypoints, best model), `COCO` (18 keypoints), `MPI` (15 keypoints, ~10% faster), "
"MPI_4_layers` (15 keypoints, even faster but less accurate).");
defineOptionalProperty<std::string>("OP_model_folder", "models/", "Folder path (absolute or relative) where the models (pose, face, ...) are located.");
defineOptionalProperty<int>("OP_num_gpu_start", 0, "GPU device start number.");
diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp
index 100b92281d2ecf94cb99fa929de991a0ae163a5a..d76207941a05d52c5d8d3528784200eb0edce5bf 100644
--- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp
+++ b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp
@@ -23,11 +23,13 @@
#include "AzureKinectPointCloudProvider.h"
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include "ArmarXCore/core/logging/Logging.h"
// STD/STL
#include <cstdio>
+#include <filesystem>
#include <functional>
// IVT
@@ -35,22 +37,34 @@
#include <Calibration/Calibration.h>
// ArmarX
+#include "ArmarXCore/core/time/Metronome.h"
+#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "RobotAPI/libraries/core/FramedPose.h"
// VisionX
+#include "VisionX/libraries/armem_human/client/HumanPoseWriter.h"
+#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.h"
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/libraries/imrec/helper.h>
#include <opencv2/core/hal/interface.h>
+#include <mutex>
#include <utility>
// OpenCV
#include <opencv2/imgproc/imgproc.hpp>
+#include <IceUtil/Time.h>
+
+#ifdef INCLUDE_BODY_TRACKING
+#include <VisionX/libraries/armem_human/types.h>
+#endif
+
namespace
{
@@ -90,23 +104,52 @@ namespace
}
}
- std::function<void(armarx::Duration)>
- createSwCallback(
- armarx::ManagedIceObject* obj,
- const std::string& description)
+
+
+#ifdef INCLUDE_BODY_TRACKING
+ void
+ printBodyInformation(k4abt_body_t body)
{
- return [ = ](armarx::Duration duration)
+ ARMARX_VERBOSE << "Body ID: " << body.id;
+ for (int i = 0; i < static_cast<int>(K4ABT_JOINT_COUNT); i++)
{
- std::string name = "duration " + description + " in [ms]";
- obj->setMetaInfo(name, new armarx::Variant{duration.toMilliSecondsDouble()});
- };
+ const k4a_float3_t position = body.skeleton.joints[i].position;
+ const k4a_quaternion_t orientation = body.skeleton.joints[i].orientation;
+ const k4abt_joint_confidence_level_t confidence_level =
+ body.skeleton.joints[i].confidence_level;
+
+ ARMARX_VERBOSE
+ << "Joint" << i << ": "
+ << "Position[mm] (" << position.v[0] << "," << position.v[1] << "," << position.v[2] << "); "
+ << "Orientation (" << orientation.v[0] << "," << orientation.v[1] << "," << orientation.v[2] << "," << orientation.v[3] << "); "
+ << "Confidence Level " << confidence_level;
+ }
}
-
+#endif
}
namespace visionx
{
+ std::function<void(armarx::Duration)>
+ AzureKinectPointCloudProvider::createSwCallback(
+ const std::string& description)
+ {
+ return [ & ](armarx::Duration duration)
+ {
+ std::string name = "duration " + description + " in [ms]";
+ {
+ std::lock_guard g{metaInfoMtx};
+ setMetaInfo(name, new armarx::Variant{duration.toMilliSecondsDouble()});
+ }
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield(name, duration.toMilliSecondsDouble());
+ }
+ };
+ }
+
AzureKinectPointCloudProviderPropertyDefinitions::AzureKinectPointCloudProviderPropertyDefinitions(
std::string prefix) :
visionx::CapturingPointCloudProviderPropertyDefinitions(std::move(prefix))
@@ -156,6 +199,15 @@ namespace visionx
" parameters.");
defs->optional(mDeviceId, "device_id", "ID of the device.");
+ defs->optional(robotName, "robotName");
+ defs->optional(bodyCameraFrameName, "bodyCameraFrameName");
+
+#ifdef INCLUDE_BODY_TRACKING
+ defs->optional(bodyTrackingModelFilename, "bodyTrackingModelPath", "Path where the .onnx DNN files can be found");
+
+ defs->component(humanMemoryServer, "HumanMemory");
+#endif
+
return defs;
}
@@ -220,7 +272,6 @@ namespace visionx
color_dim.first * 3 * static_cast<int32_t>(sizeof(int16_t)));
pointcloud = std::make_unique<pcl::PointCloud<CloudPointType>>();
-
}
void
@@ -228,12 +279,22 @@ namespace visionx
{
ARMARX_DEBUG << "Connecting " << getName();
+ setDebugObserverBatchModeEnabled(true);
+
pointcloudTask = armarx::RunningTask<AzureKinectPointCloudProvider>::pointer_type(
new armarx::RunningTask<AzureKinectPointCloudProvider>(
this,
&AzureKinectPointCloudProvider::runPointcloudPublishing));
pointcloudTask->start();
+#ifdef INCLUDE_BODY_TRACKING
+ humanPoseWriter.emplace(memoryNameSystem(),humanMemoryServer);
+
+ bodyTrackingPublishTask = new armarx::RunningTask<AzureKinectPointCloudProvider>(this, &AzureKinectPointCloudProvider::runPublishBodyTrackingResults);
+ bodyTrackingPublishTask->start();
+
+#endif
+
ARMARX_VERBOSE << "Connected " << getName();
}
@@ -254,6 +315,11 @@ namespace visionx
ARMARX_DEBUG << "Pointcloud processing thread stopped";
}
+#ifdef INCLUDE_BODY_TRACKING
+ bodyTrackingPublishTask->stop();
+#endif
+
+
ARMARX_DEBUG << "Disconnected " << getName();
}
@@ -327,6 +393,25 @@ namespace visionx
transformation = k4a::transformation(k4aCalibration);
+#ifdef INCLUDE_BODY_TRACKING
+ {
+ k4abt_tracker_configuration_t bodyTrackingConfig = K4ABT_TRACKER_CONFIG_DEFAULT;
+
+ // eventually, resolve environment variable
+ armarx::ArmarXDataPath::ReplaceEnvVars(bodyTrackingModelFilename);
+
+ const bool found = armarx::ArmarXDataPath::getAbsolutePath(bodyTrackingModelFilename, bodyTrackingModelFilename);
+ ARMARX_CHECK(found) << "Body tracking DNN model could not be found/resolved at `" << bodyTrackingModelFilename << "`.";
+
+ ARMARX_INFO << "Using body tracking DNN model from directory `" << bodyTrackingModelFilename << "`.";
+
+ ARMARX_CHECK(std::filesystem::exists(bodyTrackingModelFilename)) << "The path `" << bodyTrackingModelFilename << "` does not exist!";
+ bodyTrackingConfig.model_path = bodyTrackingModelFilename.c_str();
+
+ bodyTracker = k4abt::tracker::create(k4aCalibration, bodyTrackingConfig);
+ }
+#endif
+
ARMARX_TRACE;
device.set_color_control(K4A_COLOR_CONTROL_BRIGHTNESS, K4A_COLOR_CONTROL_MODE_MANUAL, 128);
@@ -384,7 +469,7 @@ namespace visionx
using armarx::core::time::ScopedStopWatch;
using armarx::core::time::StopWatch;
- ScopedStopWatch sw_total{::createSwCallback(this, "doCapture")};
+ ScopedStopWatch sw_total{createSwCallback("doCapture")};
k4a::capture capture;
const std::chrono::milliseconds TIMEOUT{1000};
@@ -409,9 +494,33 @@ namespace visionx
if (status)
{
- ::createSwCallback(this, "waiting for get_capture")(sw_get_capture.stop());
+ createSwCallback("waiting for get_capture")(sw_get_capture.stop());
+
+ {
+ std::lock_guard g{metaInfoMtx};
+ setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
+ }
- setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
+ // see ROS: This will give INCORRECT timestamps until the first image.
+ const bool mustInitializeTimestampOffset = [&](){
+ std::lock_guard g{deviceToRealtimeOffsetMtx};
+ return device_to_realtime_offset_.count() == 0;
+ }();
+ if (mustInitializeTimestampOffset)
+ {
+ initializeTimestampOffset(capture.get_depth_image().get_device_timestamp());
+ }
+
+ // next, we update the timestamp offset continuously
+ updateTimestampOffset(capture.get_ir_image().get_device_timestamp(), capture.get_ir_image().get_system_timestamp());
+
+#ifdef INCLUDE_BODY_TRACKING
+ if (not bodyTracker.enqueue_capture(capture))
+ {
+ ARMARX_WARNING << "Add capture to tracker process queue timeout";
+ }
+
+#endif
const k4a::image DEPTH_IMAGE = capture.get_depth_image();
const k4a::image COLOR_IMAGE = capture.get_color_image();
@@ -427,8 +536,16 @@ namespace visionx
imagesTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
- setMetaInfo("image age in [ms]",
- new armarx::Variant{(real_time - imagesTime).toMilliSecondsDouble()});
+ {
+ std::lock_guard g{metaInfoMtx};
+ setMetaInfo("image age in [ms]",
+ new armarx::Variant{(real_time - imagesTime).toMilliSecondsDouble()});
+ }
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("image age in [ms]", (real_time - imagesTime).toMilliSecondsDouble());
+ }
// This function assumes that the image is made of depth pixels (i.e. uint16_t's),
// which is only true for IR/depth images.
@@ -459,14 +576,14 @@ namespace visionx
else
{
// Convert the k4a image to an IVT image.
- ScopedStopWatch sw{::createSwCallback(this, "convert k4a image to IVT")};
+ ScopedStopWatch sw{createSwCallback("convert k4a image to IVT")};
::k4aToIvtImage(COLOR_IMAGE, *resultColorImage);
}
// Provide data for pointcloud processing thread and signal to start processing.
{
- ScopedStopWatch sw{::createSwCallback(this, "transform depth image to camera")};
+ ScopedStopWatch sw{createSwCallback("transform depth image to camera")};
// Acquire lock and write data needed by pointcloud thread (i.e.,
// alignedDepthImageScaled and depthImageReady).
{
@@ -484,7 +601,7 @@ namespace visionx
// Prepare result depth image.
{
- ScopedStopWatch sw{::createSwCallback(this, "prepare result depth image")};
+ ScopedStopWatch sw{createSwCallback("prepare result depth image")};
const int DW = alignedDepthImage.get_width_pixels();
const int DH = alignedDepthImage.get_height_pixels();
@@ -513,11 +630,12 @@ namespace visionx
index_2 += 1;
}
}
+
}
// Broadcast RGB-D image.
{
- ScopedStopWatch sw{::createSwCallback(this, "broadcast RGB-D image")};
+ ScopedStopWatch sw{createSwCallback("broadcast RGB-D image")};
CByteImage* images[2] = {resultColorImage.get(), resultDepthImage.get()};
provideImages(images, imagesTime);
}
@@ -531,17 +649,165 @@ namespace visionx
ARMARX_DEBUG << "Capturing thread received signal.";
}
+ {
+ std::lock_guard g{debugObserverMtx};
+ sendDebugObserverBatch();
+ }
+
return true;
}
else
{
ARMARX_INFO << deactivateSpam(30) << "Did not get frame until timeout of " << TIMEOUT;
+ {
+ std::lock_guard g{debugObserverMtx};
+ sendDebugObserverBatch();
+ }
+
return false;
}
}
+#ifdef INCLUDE_BODY_TRACKING
+ void AzureKinectPointCloudProvider::runPublishBodyTrackingResults()
+ {
+ armarx::Metronome metronome(armarx::Frequency::Hertz(100));
+ while(true)
+ {
+
+ // body frames might just not be available.
+ const k4abt::frame body_frame = [&]() -> k4abt::frame{
+ try {
+ auto result = bodyTracker.pop_result();
+ return result;
+ } catch (...) {
+ ARMARX_VERBOSE << deactivateSpam(1) << "Exception in body tracking publishing: " << armarx::GetHandledExceptionString();
+ return {};
+ }
+ }();
+
+ if (body_frame != nullptr)
+ {
+ armarx::core::time::ScopedStopWatch sw{createSwCallback("publish body tracking result")};
+ // see https://github.com/microsoft/Azure_Kinect_ROS_Driver/blob/melodic/src/k4a_ros_device.cpp
+ const armarx::DateTime timestamp = timestampToArmarX(body_frame.get_device_timestamp());
+
+ {
+ auto real_time = IceUtil::Time::now();
+ auto monotonic_time = IceUtil::Time::now(IceUtil::Time::Monotonic);
+ auto clock_diff = real_time - monotonic_time;
+
+ auto image_monotonic_time = IceUtil::Time::microSeconds(
+ std::chrono::duration_cast<std::chrono::microseconds>(
+ body_frame.get_system_timestamp()).count());
+ // long offset = long(getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f);
+
+ // IceUtil::Time imageTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
+ IceUtil::Time imageTime = image_monotonic_time + clock_diff;
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("ros_vs_ice_timestamp [µs]", imageTime.toMicroSeconds());
+ }
+ }
+
+ {
+ const armarx::Clock realtimeClock = armarx::Clock(armarx::ClockType::Realtime);
+ const armarx::Clock monotonicClock =
+ armarx::Clock(armarx::ClockType::Monotonic);
+
+ auto real_time = realtimeClock.now();
+ auto monotonic_time = monotonicClock.now();
+ auto clock_diff = real_time - monotonic_time;
+
+ auto image_monotonic_time = armarx::Duration::MicroSeconds(
+ std::chrono::duration_cast<std::chrono::microseconds>(
+ body_frame.get_system_timestamp()).count());
+ // long offset = long(getProperty<float>("CaptureTimeOffset").getValue() * 1000.0f);
+
+ // IceUtil::Time imageTime = image_monotonic_time + clock_diff - IceUtil::Time::microSeconds(offset);
+ auto imageTime = image_monotonic_time + clock_diff;
+
+ armarx::DateTime imageTimestamp = armarx::DateTime(imageTime);
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("ros_vs_armarx_timestamp [µs]", imageTime.toMicroSeconds());
+ }
+
+ }
+
+
+ std::uint32_t num_bodies = body_frame.get_num_bodies();
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("n_bodies_detected", num_bodies);
+ }
+
+ std::vector<armarx::armem::human::HumanPose> humanPoses;
+ humanPoses.reserve(num_bodies);
+
+ for (std::uint32_t i = 0; i < num_bodies; i++)
+ {
+ k4abt_body_t body = body_frame.get_body(i);
+ printBodyInformation(body);
+
+ armarx::armem::human::HumanPose humanPose;
+ humanPose.humanTrackingId = std::to_string(body.id);
+ humanPose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
+
+ for (int i = 0; i < static_cast<int>(K4ABT_JOINT_COUNT); i++)
+ {
+ const auto joint = static_cast<armarx::human::pose::model::k4a_bt_body_32::Joints>(i);
+ const auto name = armarx::human::pose::model::k4a_bt_body_32::JointNames.to_name(joint);
+
+ k4a_float3_t position = body.skeleton.joints[i].position;
+ k4a_quaternion_t orientation = body.skeleton.joints[i].orientation;
+ k4abt_joint_confidence_level_t confidence_level =
+ body.skeleton.joints[i].confidence_level;
+
+ humanPose.keypoints[name] = armarx::armem::human::PoseKeypoint{
+ .label = name,
+ .confidence = static_cast<float>(static_cast<int>(confidence_level)),
+ .positionCamera = armarx::FramedPosition(Eigen::Vector3f{position.v[0], position.v[1], position.v[2]}, bodyCameraFrameName, robotName),
+ .orientationCamera = armarx::FramedOrientation(Eigen::Quaternionf(orientation.wxyz.w, orientation.wxyz.x, orientation.wxyz.y, orientation.wxyz.z).toRotationMatrix(), bodyCameraFrameName, robotName)
+ };
+ }
+
+ humanPoses.push_back(humanPose);
+ }
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("bodyTrackingCaptureUntilPublish [ms]", (armarx::Clock::Now() - timestamp).toMilliSeconds());
+ }
+
+ humanPoseWriter->commitHumanPosesInCameraFrame(humanPoses, getName(), timestamp);
+
+
+ // k4a::image body_index_map = body_frame.get_body_index_map();
+ // if (body_index_map != nullptr)
+ // {
+ // //print_body_index_map_middle_line(body_index_map);
+ // }
+ // else
+ // {
+ // ARMARX_WARNING << "Error: Failed to generate bodyindex map!";
+ // }
+ }
+ else
+ {
+ // It should never hit timeout when K4A_WAIT_INFINITE is set.
+ ARMARX_VERBOSE << "Error! Pop body frame result time out!";
+ }
+
+ metronome.waitForNextTick();
+ }
+ }
+#endif
+
void
AzureKinectPointCloudProvider::runPointcloudPublishing()
{
@@ -779,4 +1045,88 @@ namespace visionx
return {rgb, depth};
}
+ // the code below is taken from https://github.com/microsoft/Azure_Kinect_ROS_Driver
+ // -> MIT license
+
+ void AzureKinectPointCloudProvider::updateTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us,
+ const std::chrono::nanoseconds& k4a_system_timestamp_ns)
+ {
+ // System timestamp is on monotonic system clock.
+ // Device time is on AKDK hardware clock.
+ // We want to continuously estimate diff between realtime and AKDK hardware clock as low-pass offset.
+ // This consists of two parts: device to monotonic, and monotonic to realtime.
+
+ // First figure out realtime to monotonic offset. This will change to keep updating it.
+ std::chrono::nanoseconds realtime_clock = std::chrono::system_clock::now().time_since_epoch();
+ std::chrono::nanoseconds monotonic_clock = std::chrono::steady_clock::now().time_since_epoch();
+
+ std::chrono::nanoseconds monotonic_to_realtime = realtime_clock - monotonic_clock;
+
+ // Next figure out the other part (combined).
+ std::chrono::nanoseconds device_to_realtime =
+ k4a_system_timestamp_ns - k4a_device_timestamp_us + monotonic_to_realtime;
+
+ {
+ std::lock_guard g{deviceToRealtimeOffsetMtx};
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("device_to_realtime_offset [ms]", device_to_realtime_offset_.count() / 1'000'000.f); // [ns] -> [ms]
+ setDebugObserverDatafield("clock_error", std::abs<float>((device_to_realtime_offset_ - device_to_realtime).count()) / 1'000.f); // [ns] -> [µs]
+ }
+
+ const std::int64_t timeOffsetThreshold = 1e7; // 10 ms
+
+ // If we're over a certain time off, just snap into place.
+ if (device_to_realtime_offset_.count() == 0 ||
+ std::abs((device_to_realtime_offset_ - device_to_realtime).count()) >timeOffsetThreshold )
+ {
+ ARMARX_WARNING << "Initializing or re-initializing the device to realtime offset: " << device_to_realtime.count()
+ << " ns";
+ device_to_realtime_offset_ = device_to_realtime;
+ }
+ else
+ {
+ // Low-pass filter!
+ constexpr double alpha = 0.10;
+
+ const std::chrono::nanoseconds timeCorrection(static_cast<int64_t>(
+ std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count())));
+ device_to_realtime_offset_ = device_to_realtime_offset_ + timeCorrection;
+
+ {
+ std::lock_guard g{debugObserverMtx};
+ setDebugObserverDatafield("timeCorrection [µs]", timeCorrection.count() / 1'000); // [ns] -> [µs]
+ }
+ }
+ }
+
+ }
+
+ armarx::DateTime AzureKinectPointCloudProvider::timestampToArmarX(const std::chrono::microseconds& k4a_timestamp_us)
+ {
+ std::lock_guard g{deviceToRealtimeOffsetMtx};
+
+ // must be initialized beforehand
+ ARMARX_CHECK(device_to_realtime_offset_.count() != 0);
+
+ std::chrono::nanoseconds timestamp_in_realtime = k4a_timestamp_us + device_to_realtime_offset_;
+
+ return armarx::Duration::MicroSeconds(timestamp_in_realtime.count() / 1'000); // [ns] -> [µs]
+ }
+
+
+ void AzureKinectPointCloudProvider::initializeTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us)
+ {
+ std::lock_guard g{deviceToRealtimeOffsetMtx};
+
+ // We have no better guess than "now".
+ std::chrono::nanoseconds realtime_clock = std::chrono::system_clock::now().time_since_epoch();
+
+ device_to_realtime_offset_ = realtime_clock - k4a_device_timestamp_us;
+
+ ARMARX_WARNING << "Initializing the device to realtime offset based on wall clock: "
+ << device_to_realtime_offset_.count() << " ns";
+ }
+
}
diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h
index 5dee3865f1659714262536563a46c5c6f7deb8b6..8d94ecb63d3a45ceab443304c5275e96e5a020a0 100644
--- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h
+++ b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h
@@ -36,10 +36,12 @@
#include <opencv2/opencv.hpp>
// ArmarXCore
+#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// RobotAPI
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
@@ -49,11 +51,18 @@
#include <VisionX/components/pointcloud_provider/ImageToPointCloud/DepthImageUtils.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
#include <VisionX/core/CapturingImageProvider.h>
+#include <VisionX/libraries/armem_human/server/HumanMemoryServerInterface.h>
// K4A
#include <k4a/k4a.h>
#include <k4a/k4a.hpp>
+#ifdef INCLUDE_BODY_TRACKING
+#include <k4abt.hpp>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include "VisionX/libraries/armem_human/client/HumanPoseWriter.h"
+#endif
+
namespace visionx
{
@@ -79,7 +88,11 @@ namespace visionx
class AzureKinectPointCloudProvider :
virtual public visionx::RGBDPointCloudProviderInterface,
virtual public visionx::CapturingPointCloudProvider,
- virtual public visionx::ImageProvider
+ virtual public visionx::ImageProvider,
+ virtual public armarx::DebugObserverComponentPluginUser
+#ifdef INCLUDE_BODY_TRACKING
+ ,virtual public armarx::armem::ClientPluginUser
+#endif
{
public:
using CloudPointType = pcl::PointXYZRGBA;
@@ -213,6 +226,16 @@ namespace visionx
return s.str();
}
+ std::function<void(armarx::Duration)>
+ createSwCallback(const std::string& description);
+
+ void updateTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us,
+ const std::chrono::nanoseconds& k4a_system_timestamp_ns);
+
+ void initializeTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us);
+
+
+ armarx::DateTime timestampToArmarX(const std::chrono::microseconds& k4a_timestamp_us);
private:
// Time the last image was recorded.
@@ -258,6 +281,19 @@ namespace visionx
k4a::transformation transformation;
k4a::image alignedDepthImage, xyzImage;
+
+#ifdef INCLUDE_BODY_TRACKING
+ k4abt::tracker bodyTracker;
+ armarx::armem::human::HumanMemoryServerInterfacePrx humanMemoryServer;
+
+ std::optional<armarx::armem::human::client::Writer> humanPoseWriter;
+
+ armarx::RunningTask<AzureKinectPointCloudProvider>::pointer_type bodyTrackingPublishTask;
+
+ void runPublishBodyTrackingResults();
+
+#endif
+
int mDeviceId = K4A_DEVICE_DEFAULT;
struct diagnostics
@@ -267,5 +303,15 @@ namespace visionx
diagnostics mDiagnostics;
+ std::string bodyTrackingModelFilename = "${K4A_BODY_TRACKING_DNN_MODEL_FILEPATH}";
+
+ std::string bodyCameraFrameName = "AzureKinectDepthCamera";
+ std::string robotName = "Armar6";
+
+ std::mutex deviceToRealtimeOffsetMtx;
+ std::chrono::nanoseconds device_to_realtime_offset_{0};
+
+ std::mutex debugObserverMtx;
+ std::mutex metaInfoMtx;
};
}
diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt
index a30e15c87ddc7915c05e3c5789f1e961dcd539d2..d67795f295762a5a0f384e852c3c15d7f4ed6b24 100644
--- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt
+++ b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt
@@ -1,19 +1,29 @@
armarx_component_set_name("AzureKinectPointCloudProvider")
find_package(k4a 1.2 QUIET)
+find_package(k4abt 1.1 QUIET)
message(STATUS "k4a found: ${k4a_FOUND} k4a version: ${k4a_VERSION}")
armarx_build_if(k4a_FOUND "Azure Kinect SDK not available")
-set(COMPONENT_LIBS ArmarXCore ArmarXCoreObservers RobotAPIInterfaces VisionXInterfaces VisionXTools VisionXCore
- VisionXPointCloud ImageToPointCloud
+set(COMPONENT_LIBS ArmarXCore ArmarXCoreObservers RobotAPIInterfaces VisionXInterfaces VisionXTools VisionXCore ArmarXCoreComponentPlugins
+ VisionXPointCloud ImageToPointCloud human armem_human
k4a::k4a
)
set(SOURCES AzureKinectPointCloudProvider.cpp)
set(HEADERS AzureKinectPointCloudProvider.h)
armarx_add_component("${SOURCES}" "${HEADERS}")
-if(k4a_FOUND)
- target_include_directories(AzureKinectPointCloudProvider PUBLIC ${k4a_INCLUDE_DIR})
+
+option(ARMARX_ENABLE_AZURE_KINECT_BODY_TRACKING "" OFF)
+
+if(ARMARX_ENABLE_AZURE_KINECT_BODY_TRACKING)
+ message(STATUS "Azure Kinect Body Tracking enabled by user")
+
+ if(k4abt_FOUND)
+ target_link_libraries(AzureKinectPointCloudProvider PUBLIC k4abt::k4abt)
+ target_compile_definitions(AzureKinectPointCloudProvider PUBLIC INCLUDE_BODY_TRACKING)
+ endif()
+
endif()
# add unit tests
diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/test/AzureKinectPointCloudProviderTest.cpp b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/test/AzureKinectPointCloudProviderTest.cpp
index e269afc96d3145635b0196aaf0cdb080b9e55953..b29a5466622ba2f93bcadc5e636e3e77f7d23b62 100644
--- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/test/AzureKinectPointCloudProviderTest.cpp
+++ b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/test/AzureKinectPointCloudProviderTest.cpp
@@ -35,7 +35,7 @@
BOOST_AUTO_TEST_CASE(testExample)
{
- visionx::AzureKinectPointCloudProvider instance;
+ //visionx::AzureKinectPointCloudProvider instance;
BOOST_CHECK_EQUAL(true, true);
}
diff --git a/source/VisionX/libraries/ArViz/CMakeLists.txt b/source/VisionX/libraries/ArViz/CMakeLists.txt
index 072592b8e7769b26a95a3989e862e25e1f62b276..2696b255a3ab21eb1e75e28ec2f10f3f07de20f5 100644
--- a/source/VisionX/libraries/ArViz/CMakeLists.txt
+++ b/source/VisionX/libraries/ArViz/CMakeLists.txt
@@ -1,16 +1,21 @@
armarx_set_target("VisionXArViz")
set(SOURCES
+ HumanPose.cpp
HumanPoseBody25.cpp
+ HumanPoseK4ABTBody32.cpp
)
set(HEADERS
+ HumanPose.h
HumanPoseBody25.h
+ HumanPoseK4ABTBody32.h
)
set(COMPONENT_LIBS
ArViz
VisionXCore
+ human
)
armarx_add_library(VisionXArViz "${SOURCES}" "${HEADERS}" "${COMPONENT_LIBS}")
diff --git a/source/VisionX/libraries/ArViz/HumanPose.cpp b/source/VisionX/libraries/ArViz/HumanPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69bf081f1638effbb4e3c3d09cc2343fda75a38d
--- /dev/null
+++ b/source/VisionX/libraries/ArViz/HumanPose.cpp
@@ -0,0 +1,73 @@
+#include "HumanPose.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <VisionX/libraries/ArViz/HumanPoseK4ABTBody32.h>
+#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
+#include <VisionX/libraries/human/pose/model/k4a_bt_body_32.h>
+#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
+
+namespace armarx
+{
+
+ void
+ viz::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix)
+ {
+ namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
+ namespace openpose_body_25 = human::pose::model::openpose_body_25;
+
+ if (pose.poseModelId == openpose_body_25::ModelId)
+ {
+ body_25::addPoseToLayer(pose, layer, prefix);
+ }
+ else if (pose.poseModelId == k4a_bt_body_32::ModelId)
+ {
+ k4abt_body_32::addPoseToLayer(pose, layer, prefix);
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown pose model ID '" << pose.poseModelId << "'. No generic "
+ << "visualization is available at this point.";
+
+ generic::addPoseToLayer(pose, layer, prefix);
+ }
+ }
+
+ void
+ viz::addPosesToLayer(const std::map<std::string, armarx::armem::human::HumanPose>& poses,
+ viz::Layer& layer,
+ const std::string& prefix)
+ {
+ for (const auto& [name, pose] : poses)
+ {
+ addPoseToLayer(pose, layer, prefix + name);
+ }
+ }
+
+
+ void
+ viz::generic::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix)
+ {
+ static const int jointThickness = 30;
+ armarx::viz::Color color =
+ armarx::viz::Color::fromRGBA(1, 1, 1, 1); // TODO: Random, seed with person ID.
+
+ // Cannot visualize segments without pose model information.
+
+ // Keypoints.
+ for (auto const& [name, kp] : pose.keypoints)
+ {
+ armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
+ .position(kp.positionGlobal->toEigen())
+ .radius(jointThickness)
+ .color(color);
+ layer.add(sphere);
+ }
+ }
+
+
+} // namespace armarx
diff --git a/source/VisionX/libraries/ArViz/HumanPose.h b/source/VisionX/libraries/ArViz/HumanPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..294d5fc10053bb29e60b7f70eefa1d3fa44c54de
--- /dev/null
+++ b/source/VisionX/libraries/ArViz/HumanPose.h
@@ -0,0 +1,30 @@
+#pragma once
+
+
+#include <map>
+#include <string>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+
+namespace armarx::viz
+{
+
+ void addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix);
+
+ void addPosesToLayer(const std::map<std::string, armarx::armem::human::HumanPose>& poses,
+ viz::Layer& layer,
+ const std::string& prefix);
+
+ namespace generic
+ {
+ void addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix);
+ }
+
+} // namespace armarx::viz
diff --git a/source/VisionX/libraries/ArViz/HumanPoseBody25.cpp b/source/VisionX/libraries/ArViz/HumanPoseBody25.cpp
index bc53d16c5156d4c9ea2806a5f7ac1aef1c898502..924a105c0b13e4877e8832fdee1b1ebfaacef086 100644
--- a/source/VisionX/libraries/ArViz/HumanPoseBody25.cpp
+++ b/source/VisionX/libraries/ArViz/HumanPoseBody25.cpp
@@ -1,142 +1,77 @@
-#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
-
+#include "HumanPoseBody25.h"
#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
+#include <ArmarXCore/core/logging/Logging.h>
-const std::map<unsigned int, std::string>
-armarx::viz::HumanPoseBody25::parts =
-{
- {0, "Nose"},
- {1, "Neck"},
- {2, "RShoulder"},
- {3, "RElbow"},
- {4, "RWrist"},
- {5, "LShoulder"},
- {6, "LElbow"},
- {7, "LWrist"},
- {8, "MidHip"},
- {9, "RHip"},
- {10, "RKnee"},
- {11, "RAnkle"},
- {12, "LHip"},
- {13, "LKnee"},
- {14, "LAnkle"},
- {15, "REye"},
- {16, "LEye"},
- {17, "REar"},
- {18, "LEar"},
- {19, "LBigToe"},
- {20, "LSmallToe"},
- {21, "LHeel"},
- {22, "RBigToe"},
- {23, "RSmallToe"},
- {24, "RHeel"},
- {25, "Background"}
-};
-
-
-const std::vector<std::tuple<float, float, float>>
- armarx::viz::HumanPoseBody25::colors =
-{
- {255.f, 0.f, 85.f},
- {255.f, 0.f, 0.f},
- {255.f, 85.f, 0.f},
- {255.f, 170.f, 0.f},
- {255.f, 255.f, 0.f},
- {170.f, 255.f, 0.f},
- {85.f, 255.f, 0.f},
- {0.f, 255.f, 0.f},
- {255.f, 0.f, 0.f},
- {0.f, 255.f, 85.f},
- {0.f, 255.f, 170.f},
- {0.f, 255.f, 255.f},
- {0.f, 170.f, 255.f},
- {0.f, 85.f, 255.f},
- {0.f, 0.f, 255.f},
- {255.f, 0.f, 170.f},
- {170.f, 0.f, 255.f},
- {255.f, 0.f, 255.f},
- {85.f, 0.f, 255.f},
- {0.f, 0.f, 255.f},
- {0.f, 0.f, 255.f},
- {0.f, 0.f, 255.f},
- {0.f, 255.f, 255.f},
- {0.f, 255.f, 255.f},
- {0.f, 255.f, 255.f}
-};
-
-
-
-const std::vector<std::pair<unsigned int, unsigned int>>
- armarx::viz::HumanPoseBody25::pairs =
+namespace armarx::viz
{
- {1, 8}, {1, 2}, {1, 5}, {2, 3}, {3, 4}, {5, 6}, {6, 7}, {8, 9}, {9, 10}, {10, 11}, {8, 12},
- {12, 13}, {13, 14}, {1, 0}, {0, 15}, {15, 17}, {0, 16}, {16, 18}, {14, 19}, {19, 20}, {14, 21},
- {11, 22}, {22, 23}, {11, 24}
-};
+ using namespace armarx::human::pose::model::openpose_body_25;
-void
-armarx::viz::HumanPoseBody25::addPoseToLayer(const armarx::Keypoint3DMap& pose,
- armarx::viz::Layer& layer, const std::string& prefix)
-{
- // Segments.
- for (const auto& [index1, index2] : HumanPoseBody25::pairs)
+ void
+ body_25::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ armarx::viz::Layer& layer,
+ const std::string& prefix)
{
- const std::string name1 = parts.at(index1);
- const std::string name2 = parts.at(index2);
-
- // Only draw segment if start end end are in keypoint map.
- if (pose.find(name1) == pose.end() or pose.find(name2) == pose.end())
- {
- continue;
- }
+ ARMARX_TRACE;
+ static const int segmentThickness = 20;
+ static const int jointThickness = 30;
-
- const armarx::Keypoint3D p1 = pose.at(name1);
- const armarx::Keypoint3D p2 = pose.at(name2);
-
- if (std::isfinite(p1.globalX) && std::isfinite(p1.globalY) && std::isfinite(p1.globalZ)
- && std::isfinite(p2.globalX) && std::isfinite(p2.globalY) && std::isfinite(p2.globalZ))
+ // Segments.
+ for (const auto& [index1, index2] : Segments)
{
- const Eigen::Vector3f pos1(p1.globalX, p1.globalY, p1.globalZ);
- const Eigen::Vector3f pos2(p2.globalX, p2.globalY, p2.globalZ);
+ const std::string& name1 = JointNames.map().left.at(index1);
+ const std::string& name2 = JointNames.map().left.at(index2);
- if (pos1 == Eigen::Vector3f(0, 0, 0) || pos2 == Eigen::Vector3f(0, 0, 0))
+ // Only draw segment if start end end are in keypoint map.
+ if (pose.keypoints.find(name1) == pose.keypoints.end() or
+ pose.keypoints.find(name2) == pose.keypoints.end())
{
continue;
}
- float r, g, b;
- std::tie(r, g, b) = colors.at(index2);
- const armarx::viz::Color color = armarx::viz::Color::fromRGBA(r / 255.0f, g / 255.0f, b / 255.0f, 1.0);
+ const armarx::armem::human::PoseKeypoint p1 = pose.keypoints.at(name1);
+ const armarx::armem::human::PoseKeypoint p2 = pose.keypoints.at(name2);
- armarx::viz::Cylinder line = armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
- .fromTo(pos1, pos2)
- .radius(20)
- .color(color);
- layer.add(line);
- }
- }
+ const Eigen::Vector3f pos1 = p1.positionGlobal->toEigen();
+ const Eigen::Vector3f pos2 = p2.positionGlobal->toEigen();
- // Keypoints.
- for (auto const& [id, name] : parts)
- {
+ if (pos1.allFinite() and pos2.allFinite())
+ {
+ if (pos1 == Eigen::Vector3f(0, 0, 0) or pos2 == Eigen::Vector3f(0, 0, 0))
+ {
+ continue;
+ }
+
+ const armarx::viz::Color color = Colors.at(index2);
+
+ armarx::viz::Cylinder line =
+ armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
+ .fromTo(pos1, pos2)
+ .radius(segmentThickness)
+ .color(color);
+ layer.add(line);
+ }
+ }
- if (pose.find(name) != pose.end() && name != "Background")
+ // Keypoints.
+ for (auto const& [id, name] : JointNames.map())
{
- armarx::Keypoint3D kp = pose.at(name);
- const Eigen::Vector3f pos(kp.globalX, kp.globalY, kp.globalZ);
-
- float r, g, b;
- std::tie(r, g, b) = colors.at(id);
- const armarx::viz::Color color = armarx::viz::Color::fromRGBA(r / 255.0f, g / 255.0f, b / 255.0f, 1.0);
- armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
- .position(pos)
- .radius(30)
- .color(color);
- layer.add(sphere);
+ if (pose.keypoints.find(name) != pose.keypoints.end() and
+ name != JointNames.map().left.at(Joints::Background))
+ {
+ armarx::armem::human::PoseKeypoint kp = pose.keypoints.at(name);
+
+ const armarx::viz::Color color= Colors.at(id);
+ armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
+ .position(kp.positionGlobal->toEigen())
+ .radius(jointThickness)
+ .color(color);
+ layer.add(sphere);
+ }
}
}
-}
+
+} // namespace armarx::viz
diff --git a/source/VisionX/libraries/ArViz/HumanPoseBody25.h b/source/VisionX/libraries/ArViz/HumanPoseBody25.h
index 60b38f8c2fafafaa94c88ee6ef9314ac7801554e..c475ed51b652125bc947a2be6369b03fc865b720 100644
--- a/source/VisionX/libraries/ArViz/HumanPoseBody25.h
+++ b/source/VisionX/libraries/ArViz/HumanPoseBody25.h
@@ -1,40 +1,19 @@
#pragma once
-// STD/STL
#include <map>
#include <string>
#include <vector>
-// RobotAPI
#include <RobotAPI/components/ArViz/Client/Layer.h>
-// VisionX
-#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
+#include <VisionX/libraries/armem_human/types.h>
-namespace armarx::viz
+namespace armarx::viz::body_25
{
+ void addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ armarx::viz::Layer& layer,
+ const std::string& prefix);
- class HumanPoseBody25
- {
-
- public:
-
- static
- const std::map<unsigned int, std::string> parts;
-
- static
- const std::vector<std::pair<unsigned int, unsigned int>> pairs;
-
- static
- const std::vector<std::tuple<float, float, float>> colors;
-
- public:
-
- void
- static addPoseToLayer(const armarx::Keypoint3DMap& pose, armarx::viz::Layer& layer, const std::string& s);
-
- };
-
-}
+} // namespace armarx::viz::body_25
diff --git a/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.cpp b/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d4dfa3175e618ddc0359fe4b0500fa0f6091b82
--- /dev/null
+++ b/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.cpp
@@ -0,0 +1,87 @@
+#include "HumanPoseK4ABTBody32.h"
+#include <SimoxUtility/color/GlasbeyLUT.h>
+
+#include <VisionX/libraries/human/pose/model/k4a_bt_body_32.h>
+
+namespace armarx::viz
+{
+
+ using namespace armarx::human::pose::model::k4a_bt_body_32;
+
+ void
+ k4abt_body_32::addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix)
+ {
+ static const int segmentThickness = 20;
+ static const int jointThickness = 30;
+
+ //const armarx::viz::Color colorCylinder =
+ // armarx::viz::Color::fromRGBA(1, 1, 1, 1); // TODO: Random, seed with person ID.
+
+ // Segments.
+ for (const auto& [index1, index2] : Segments)
+ {
+ const std::string& name1 = JointNames.map().left.at(index1);
+ const std::string& name2 = JointNames.map().left.at(index2);
+
+ // Only draw segment if start end end are in keypoint map.
+ if (pose.keypoints.find(name1) == pose.keypoints.end() or
+ pose.keypoints.find(name2) == pose.keypoints.end())
+ {
+ continue;
+ }
+
+ const armarx::armem::human::PoseKeypoint p1 = pose.keypoints.at(name1);
+ const armarx::armem::human::PoseKeypoint p2 = pose.keypoints.at(name2);
+
+ const Eigen::Vector3f pos1 = p1.positionGlobal->toEigen();
+ const Eigen::Vector3f pos2 = p2.positionGlobal->toEigen();
+
+ if (pos1.allFinite() and pos2.allFinite())
+ {
+ if (pos1 == Eigen::Vector3f(0, 0, 0) or pos2 == Eigen::Vector3f(0, 0, 0))
+ {
+ continue;
+ }
+
+ armarx::viz::Color color = Colors.at(JointNames.from_name(p2.label));
+
+ armarx::viz::Cylinder line =
+ armarx::viz::Cylinder(prefix + "::" + p1.label + "_" + p2.label)
+ .fromTo(pos1, pos2)
+ .radius(segmentThickness)
+ .color(color);
+ layer.add(line);
+ }
+ }
+
+ // Keypoints.
+ for (auto const& [id, name] : JointNames.map())
+ {
+ const auto it = pose.keypoints.find(name);
+ if (it != pose.keypoints.end())
+ {
+ armarx::armem::human::PoseKeypoint kp = pose.keypoints.at(name);
+ armarx::viz::Color color = Colors.at(JointNames.from_name(kp.label));
+
+ armarx::viz::Sphere sphere = armarx::viz::Sphere(prefix + "::" + name)
+ .position(kp.positionGlobal->toEigen())
+ .radius(jointThickness)
+ .color(color);
+
+ if (kp.orientationGlobal.has_value())
+ {
+ armarx::viz::Pose pose = armarx::viz::Pose(prefix + "::" + name + "_pose")
+ .pose(kp.positionGlobal.value().toEigen(),
+ kp.orientationGlobal.value().toEigen());
+ layer.add(pose);
+ }
+
+ layer.add(sphere);
+ }
+ }
+ }
+
+
+} // namespace armarx::viz
diff --git a/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.h b/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.h
new file mode 100644
index 0000000000000000000000000000000000000000..e2697e61df7ef698e9ad499816e52ede06c9fbfe
--- /dev/null
+++ b/source/VisionX/libraries/ArViz/HumanPoseK4ABTBody32.h
@@ -0,0 +1,22 @@
+#pragma once
+
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+/**
+ * Body model taken from https://docs.microsoft.com/en-us/azure/kinect-dk/body-joints and used
+ * with Azure Kinect Body Tracking SDK 1.1.1.
+ */
+namespace armarx::viz::k4abt_body_32
+{
+ void addPoseToLayer(const armarx::armem::human::HumanPose& pose,
+ viz::Layer& layer,
+ const std::string& prefix);
+
+} // namespace armarx::viz::akbt_body_32
diff --git a/source/VisionX/libraries/CMakeLists.txt b/source/VisionX/libraries/CMakeLists.txt
index 56e2665c3614973da69a4d4cc2bfa55b3fc3270b..4b3d8d3128724d1bf73d7ccf11f72a5cb7b91e0a 100644
--- a/source/VisionX/libraries/CMakeLists.txt
+++ b/source/VisionX/libraries/CMakeLists.txt
@@ -1,4 +1,5 @@
add_subdirectory(AffordanceKitArmarX)
+add_subdirectory(human)
add_subdirectory(ArViz)
add_subdirectory(armem_idgraph)
add_subdirectory(armem_images_core)
diff --git a/source/VisionX/libraries/OpenPose/MonocularOpenPoseEstimation/OpenPoseAdapter.h b/source/VisionX/libraries/OpenPose/MonocularOpenPoseEstimation/OpenPoseAdapter.h
index 9552ea42de8a9eb7490fe56c79086b3246823dff..219ffbe676f3e4f64c74f0a12576942f5e2a369b 100644
--- a/source/VisionX/libraries/OpenPose/MonocularOpenPoseEstimation/OpenPoseAdapter.h
+++ b/source/VisionX/libraries/OpenPose/MonocularOpenPoseEstimation/OpenPoseAdapter.h
@@ -53,6 +53,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
+#include <VisionX/libraries/human/pose/model/openpose_body_25.h>
namespace armarx
{
@@ -66,7 +67,7 @@ namespace armarx
{
std::string net_resolution = "-1x368";
std::string output_resolution = "-1x-1";
- std::string model_pose = "BODY_25";
+ std::string model_pose = human::pose::model::openpose_body_25::ModelId;
std::string model_folder = "models/";
double scale_gap = 0.3;
int scale_number = 1;
diff --git a/source/VisionX/libraries/OpenPose/RGBDOpenPoseEstimation/RGBDOpenPoseEstimationComponent.cpp b/source/VisionX/libraries/OpenPose/RGBDOpenPoseEstimation/RGBDOpenPoseEstimationComponent.cpp
index afeb970054e4856a01087593966b5d250d8f3706..23fab3b3ce86fc676cbe0ba4f9b537b29475ac97 100644
--- a/source/VisionX/libraries/OpenPose/RGBDOpenPoseEstimation/RGBDOpenPoseEstimationComponent.cpp
+++ b/source/VisionX/libraries/OpenPose/RGBDOpenPoseEstimation/RGBDOpenPoseEstimationComponent.cpp
@@ -29,7 +29,7 @@
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
+//#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
using namespace armarx;
@@ -434,7 +434,7 @@ void RGBDOpenPoseEstimationComponentPluginUser::visualize()
for (const auto& [name, humanPose] : openposeResult3D)
{
std::string objectName = "human_" + name;
- armarx::viz::HumanPoseBody25::addPoseToLayer(humanPose.keypointMap, openPoseArVizLayer, objectName);
+ //armarx::viz::HumanPoseBody25::addPoseToLayer(humanPose.keypointMap, openPoseArVizLayer, objectName);
human++;
}
diff --git a/source/VisionX/libraries/armem_human/CMakeLists.txt b/source/VisionX/libraries/armem_human/CMakeLists.txt
index 8f5cccf7c8c2d498b781925a5683a32ebad1b395..8d047d1265ba2f4bf4e745dc0554502fd703e83d 100644
--- a/source/VisionX/libraries/armem_human/CMakeLists.txt
+++ b/source/VisionX/libraries/armem_human/CMakeLists.txt
@@ -17,12 +17,14 @@ armarx_add_library(
armem
# VisionX
VisionXInterfaces
+ armem_human_serverInterfaces
SOURCES
aron_conversions.cpp
json_conversions.cpp
memory_ids.cpp
client/HumanPoseReader.cpp
+ client/HumanPoseWriter.cpp
HEADERS
types.h
@@ -31,9 +33,10 @@ armarx_add_library(
json_conversions.h
memory_ids.h
client/HumanPoseReader.h
+ client/HumanPoseWriter.h
ARON_FILES
- aron/BODY_25Pose.xml
+ aron/HumanPose.xml
aron/FaceRecognition.xml
aron/PersonInstance.xml
aron/Profile.xml
diff --git a/source/VisionX/libraries/armem_human/aron/BODY_25Pose.xml b/source/VisionX/libraries/armem_human/aron/HumanPose.xml
similarity index 51%
rename from source/VisionX/libraries/armem_human/aron/BODY_25Pose.xml
rename to source/VisionX/libraries/armem_human/aron/HumanPose.xml
index 951912fefb13a96d4c9919da10c763521b2f4ba8..90259d1b75288f51d077bfd5a0360b005d174a56 100644
--- a/source/VisionX/libraries/armem_human/aron/BODY_25Pose.xml
+++ b/source/VisionX/libraries/armem_human/aron/HumanPose.xml
@@ -7,76 +7,87 @@
<AronIncludes>
<Include include="<RobotAPI/libraries/aron/common/aron/framed.xml>" autoinclude="true" />
- <Include include="<RobotAPI/libraries/aron/common/aron/color.xml>" autoinclude="true" />
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
</AronIncludes>
<GenerateTypes>
- <Object name='armarx::human::arondto::Keypoint2D'>
- <ObjectChild key="label">
- <string />
+ <Object name='armarx::human::arondto::PoseKeypoint'>
+ <ObjectChild key="positionCamera">
+ <armarx::arondto::FramedPosition />
</ObjectChild>
- <ObjectChild key="x">
- <float />
+ <ObjectChild key="orientationCamera">
+ <armarx::arondto::FramedOrientation optional="true" />
</ObjectChild>
- <ObjectChild key="y">
- <float />
+ <ObjectChild key="positionRobot">
+ <armarx::arondto::FramedPosition optional="true" />
</ObjectChild>
- <ObjectChild key="confidence">
- <float />
+ <ObjectChild key="orientationRobot">
+ <armarx::arondto::FramedOrientation optional="true" />
</ObjectChild>
- <ObjectChild key="dominantColor">
- <armarx::arondto::DrawColor24Bit />
+ <ObjectChild key="positionGlobal">
+ <armarx::arondto::FramedPosition optional="true" />
+ </ObjectChild>
+
+ <ObjectChild key="orientationGlobal">
+ <armarx::arondto::FramedOrientation optional="true" />
+ </ObjectChild>
+
+ <ObjectChild key="confidence">
+ <float />
</ObjectChild>
</Object>
- <Object name='armarx::human::arondto::Keypoint3D'>
- <ObjectChild key="label">
+ <Object name='armarx::human::arondto::HumanPose'>
+ <ObjectChild key="poseModelId">
<string />
</ObjectChild>
-
- <ObjectChild key="positionRobot">
- <armarx::arondto::FramedPosition />
+ <ObjectChild key="humanTrackingId">
+ <string optional="true" />
+ </ObjectChild>
+ <ObjectChild key="keypoints">
+ <Dict>
+ <armarx::human::arondto::PoseKeypoint />
+ </Dict>
</ObjectChild>
+ </Object>
- <ObjectChild key="positionGlobal">
- <armarx::arondto::FramedPosition />
+ <Object name='armarx::human::arondto::PoseKeypoint2D'>
+ <ObjectChild key="x">
+ <float />
</ObjectChild>
- <ObjectChild key="confidence">
+ <ObjectChild key="y">
<float />
</ObjectChild>
- <ObjectChild key="dominantColor">
- <armarx::arondto::DrawColor24Bit />
+ <ObjectChild key="depth">
+ <float optional="true" />
</ObjectChild>
- </Object>
- <Object name='armarx::human::aronto::Body25Pose2D'>
- <ObjectChild key="keypoints">
- <Dict>
- <armarx::human::arondto::Keypoint2D />
- </Dict>
+ <ObjectChild key="confidence">
+ <float />
</ObjectChild>
</Object>
- <Object name='armarx::human::arondto::Body25Pose3D'>
- <ObjectChild key="keypoints2D">
- <Dict>
- <armarx::human::arondto::Keypoint2D />
- </Dict>
+ <Object name='armarx::human::arondto::HumanPose2D'>
+ <ObjectChild key="poseModelId">
+ <string />
+ </ObjectChild>
+ <ObjectChild key="humanTrackingId">
+ <string optional="true" />
</ObjectChild>
- <ObjectChild key="keypoints3D">
+ <ObjectChild key="keypoints">
<Dict>
- <armarx::human::arondto::Keypoint3D />
+ <armarx::human::arondto::PoseKeypoint2D />
</Dict>
</ObjectChild>
</Object>
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/VisionX/libraries/armem_human/aron_conversions.cpp b/source/VisionX/libraries/armem_human/aron_conversions.cpp
index 71f407fd652d92c25ddc4b46754157fa3b318e9f..3896ec07dd44425d8b17b64de4abf4526dbc1e30 100644
--- a/source/VisionX/libraries/armem_human/aron_conversions.cpp
+++ b/source/VisionX/libraries/armem_human/aron_conversions.cpp
@@ -1,27 +1,11 @@
-/*
- * 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 VisionX::ArmarXObjects::armem_images_server
- * @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 "aron_conversions.h"
-#include "VisionX/libraries/armem_human/types.h"
+#include "ArmarXCore/util/CPPUtility/trace.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
namespace armarx::armem::server::human
{
@@ -29,61 +13,147 @@ namespace armarx::armem::server::human
}
-
namespace armarx::armem::human
{
- void fromAron(const ::armarx::human::arondto::Body25Pose3D& dto, HumanPose& bo)
+
+ void
+ fromAron(const armarx::human::arondto::HumanPose& dto, HumanPose& bo)
{
- fromAron(dto.keypoints2D, bo.keypoint2dMap);
- fromAron(dto.keypoints3D, bo.keypoint3dMap);
+ using armarx::aron::fromAron;
+
+ fromAron(dto.poseModelId, bo.poseModelId);
+ bo.humanTrackingId = dto.humanTrackingId;
+ fromAron(dto.keypoints, bo.keypoints);
}
- void toAron(::armarx::human::arondto::Body25Pose3D& dto, const HumanPose& bo)
+
+ void
+ toAron(armarx::human::arondto::HumanPose& dto, const HumanPose& bo)
{
- // FIXME implement
+ using armarx::aron::toAron;
+
+ toAron(dto.poseModelId, bo.poseModelId);
+ dto.humanTrackingId = bo.humanTrackingId;
+ toAron(dto.keypoints, bo.keypoints);
}
- void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint3D>& dto, armarx::armem::human::Keypoint3DIdMap& bo)
+
+ void
+ fromAron(const std::map<std::string, armarx::human::arondto::PoseKeypoint>& dto,
+ std::map<std::string, armarx::armem::human::PoseKeypoint>& bo)
{
- for(const auto& [k, v]: dto)
+ bo.clear();
+ for (const auto& [keypoint_name, keypoint] : dto)
{
- Keypoint3D kp;
- fromAron(v, kp);
- bo[k] = kp;
+ armarx::armem::human::PoseKeypoint keypoint_bo;
+ keypoint_bo.label = keypoint_name;
+ fromAron(keypoint, keypoint_bo);
+ bo[keypoint_name] = keypoint_bo;
}
}
- void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint2D>& dto, armarx::armem::human::Keypoint2DIdMap& bo)
+
+ void
+ toAron(std::map<std::string, armarx::human::arondto::PoseKeypoint>& dto,
+ const std::map<std::string, armarx::armem::human::PoseKeypoint>& bo)
{
- for(const auto& [k, v]: dto)
+ dto.clear();
+ for (const auto& [keypoint_name, keypoint] : bo)
{
- Keypoint2D kp;
- fromAron(v, kp);
- bo[k] = kp;
+ armarx::human::arondto::PoseKeypoint keypoint_dto;
+ toAron(keypoint_dto, keypoint);
+ dto[keypoint_name] = keypoint_dto;
}
}
- void fromAron(const armarx::human::arondto::Keypoint2D& dto, Keypoint2D& bo)
+
+ void
+ fromAron(const armarx::human::arondto::PoseKeypoint& dto, PoseKeypoint& bo)
{
- bo = Keypoint2D
- {
- .label = dto.label,
- .position = Eigen::Vector2f{dto.x, dto.y},
- .confidence = dto.confidence,
- .dominantColor = simox::color::Color{dto.dominantColor.r, dto.dominantColor.g, dto.dominantColor.b}
- };
+ bo.positionCamera = dto.positionCamera;
+ bo.orientationCamera = dto.orientationCamera;
+ bo.positionRobot = dto.positionRobot;
+ bo.orientationRobot = dto.orientationRobot;
+ bo.positionGlobal = dto.positionGlobal;
+ bo.orientationGlobal = dto.orientationGlobal;
+ bo.confidence = dto.confidence;
}
- void fromAron(const armarx::human::arondto::Keypoint3D& dto, Keypoint3D& bo)
+
+ void
+ toAron(armarx::human::arondto::PoseKeypoint& dto, const PoseKeypoint& bo)
{
- bo = Keypoint3D
- {
- .label = dto.label,
- .positionRobot = dto.positionRobot.toEigen(),
- .positionGlobal = dto.positionGlobal.toEigen(),
- .confidence = dto.confidence,
- .dominantColor = simox::color::Color{dto.dominantColor.r, dto.dominantColor.g, dto.dominantColor.b}
- };
+ dto.positionCamera = bo.positionCamera;
+ dto.orientationCamera = bo.orientationCamera;
+ dto.positionRobot = bo.positionRobot;
+ dto.orientationRobot = bo.orientationRobot;
+ dto.positionGlobal = bo.positionGlobal;
+ dto.orientationGlobal = bo.orientationGlobal;
+ dto.confidence = bo.confidence;
}
-}
+
+ void fromAron(const armarx::human::arondto::HumanPose2D& dto, HumanPose2D& bo)
+ {
+ using armarx::aron::fromAron;
+
+ ARMARX_TRACE;
+
+ fromAron(dto.poseModelId, bo.poseModelId);
+ bo.humanTrackingId = dto.humanTrackingId;
+ fromAron(dto.keypoints, bo.keypoints);
+ }
+
+
+ void toAron(armarx::human::arondto::HumanPose2D& dto, const HumanPose2D& bo)
+ {
+ using armarx::aron::toAron;
+
+ toAron(dto.poseModelId, bo.poseModelId);
+ dto.humanTrackingId = bo.humanTrackingId;
+ toAron(dto.keypoints, bo.keypoints);
+ }
+
+
+ void fromAron(const std::map<std::string, armarx::human::arondto::PoseKeypoint2D>& dto,
+ std::map<std::string, armarx::armem::human::PoseKeypoint2D>& bo)
+ {
+ bo.clear();
+ for (const auto& [keypoint_name, keypoint] : dto)
+ {
+ armarx::armem::human::PoseKeypoint2D keypoint_bo;
+ keypoint_bo.label = keypoint_name;
+ fromAron(keypoint, keypoint_bo);
+ bo[keypoint_name] = keypoint_bo;
+ }
+ }
+
+
+ void toAron(std::map<std::string, armarx::human::arondto::PoseKeypoint2D>& dto,
+ const std::map<std::string, armarx::armem::human::PoseKeypoint2D>& bo)
+ {
+ dto.clear();
+ for (const auto& [keypoint_name, keypoint] : bo)
+ {
+ armarx::human::arondto::PoseKeypoint2D keypoint_dto;
+ toAron(keypoint_dto, keypoint);
+ dto[keypoint_name] = keypoint_dto;
+ }
+ }
+
+
+ void fromAron(const armarx::human::arondto::PoseKeypoint2D& dto, PoseKeypoint2D& bo)
+ {
+ bo.position = Eigen::Vector2f(dto.x, dto.y);
+ bo.confidence = dto.confidence;
+ }
+
+
+ void toAron(armarx::human::arondto::PoseKeypoint2D& dto, const PoseKeypoint2D& bo)
+ {
+ dto.x = bo.position.x();
+ dto.y = bo.position.y();
+ dto.confidence = bo.confidence;
+ }
+
+} // namespace armarx::armem::human
diff --git a/source/VisionX/libraries/armem_human/aron_conversions.h b/source/VisionX/libraries/armem_human/aron_conversions.h
index f326df2111c74de5d3dd68e912521119d6ca3a2c..3e26e587727c93a0432022405125d0de972900b5 100644
--- a/source/VisionX/libraries/armem_human/aron_conversions.h
+++ b/source/VisionX/libraries/armem_human/aron_conversions.h
@@ -1,47 +1,43 @@
-/*
- * 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 VisionX::ArmarXObjects::armem_images_server
- * @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 "VisionX/libraries/armem_human/types.h"
+
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
-#include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
+
+#include "VisionX/libraries/armem_human/types.h"
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
+#include <VisionX/libraries/armem_human/aron/Profile.aron.generated.h>
-namespace armarx::armem::server::human
+namespace armarx::armem::human
{
-}
+ void fromAron(const armarx::human::arondto::HumanPose& dto, HumanPose& bo);
-namespace armarx::armem::human
-{
- void fromAron(const ::armarx::human::arondto::Body25Pose3D& dto, HumanPose& bo);
- void toAron(::armarx::human::arondto::Body25Pose3D& dto, const HumanPose& bo);
+ void toAron(armarx::human::arondto::HumanPose& dto, const HumanPose& bo);
+
+ void fromAron(const std::map<std::string, armarx::human::arondto::PoseKeypoint>& dto,
+ std::map<std::string, armarx::armem::human::PoseKeypoint>& bo);
+
+ void toAron(std::map<std::string, armarx::human::arondto::PoseKeypoint>& dto,
+ const std::map<std::string, armarx::armem::human::PoseKeypoint>& bo);
+
+ void fromAron(const armarx::human::arondto::PoseKeypoint& dto, PoseKeypoint& bo);
+
+ void toAron(armarx::human::arondto::PoseKeypoint& dto, const PoseKeypoint& bo);
+
+ void fromAron(const armarx::human::arondto::HumanPose2D& dto, HumanPose2D& bo);
+
+ void toAron(armarx::human::arondto::HumanPose2D& dto, const HumanPose2D& bo);
+
+ void fromAron(const std::map<std::string, armarx::human::arondto::PoseKeypoint2D>& dto,
+ std::map<std::string, armarx::armem::human::PoseKeypoint2D>& bo);
+
+ void toAron(std::map<std::string, armarx::human::arondto::PoseKeypoint2D>& dto,
+ const std::map<std::string, armarx::armem::human::PoseKeypoint2D>& bo);
+
+ void fromAron(const armarx::human::arondto::PoseKeypoint2D& dto, PoseKeypoint2D& bo);
- void fromAron(const armarx::human::arondto::Keypoint2D& dto, Keypoint2D& bo);
- void fromAron(const armarx::human::arondto::Keypoint3D& dto, Keypoint3D& bo);
+ void toAron(armarx::human::arondto::PoseKeypoint2D& dto, const PoseKeypoint2D& bo);
- void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint3D>& dto, armarx::armem::human::Keypoint3DIdMap& bo);
- void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint2D>& dto, armarx::armem::human::Keypoint2DIdMap& bo);
-
-}
+} // namespace armarx::armem::human
diff --git a/source/VisionX/libraries/armem_human/client/HumanPoseReader.cpp b/source/VisionX/libraries/armem_human/client/HumanPoseReader.cpp
index c3537751a8d90d75cbff1773c5847ca15ebf67d1..26c459110a7810728df8cc2a3a25895734492262 100644
--- a/source/VisionX/libraries/armem_human/client/HumanPoseReader.cpp
+++ b/source/VisionX/libraries/armem_human/client/HumanPoseReader.cpp
@@ -2,9 +2,12 @@
#include <iterator>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
#include <RobotAPI/libraries/armem/util/util.h>
-#include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
#include <VisionX/libraries/armem_human/aron_conversions.h>
@@ -30,10 +33,7 @@ namespace armarx::armem::human::client
return coreSegmentQuery.providerSegments().all();
}();
- providerQuery.entities()
- .withName("3DDetections")
- .snapshots()
- .beforeOrAtTime(query.timestamp);
+ providerQuery.entities().all().snapshots().beforeOrAtTime(query.timestamp);
return qb;
}
@@ -51,37 +51,56 @@ namespace armarx::armem::human::client
}
std::vector<HumanPose>
- asHumanPoses(const wm::ProviderSegment& providerSegment)
+ asHumanPoses(const wm::ProviderSegment& providerSegment,
+ const DateTime& timestamp,
+ const Duration& maxAge)
{
+ ARMARX_TRACE;
+
ARMARX_CHECK(not providerSegment.empty()) << "No entities";
- ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+ // ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
std::vector<HumanPose> humanPoses;
providerSegment.forEachEntity(
[&](const wm::Entity& entity)
{
+ ARMARX_TRACE;
+
const auto& entitySnapshot = entity.getLatestSnapshot();
ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
- const auto* entityInstance = &entitySnapshot.getInstance(0);
+ entitySnapshot.forEachInstance(
+ [&humanPoses, ×tamp, &maxAge](const auto& entityInstance)
+ {
+ ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(entityInstance);
+ const core::time::Duration dtToNow =
+ timestamp - entityInstance.metadata().timeCreated;
- const auto aronDto = tryCast<armarx::human::arondto::Body25Pose3D>(*entityInstance);
- ARMARX_CHECK(aronDto) << "Failed casting to Body25Pose3D";
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto aronDto =
+ tryCast<armarx::human::arondto::HumanPose>(entityInstance);
+ ARMARX_CHECK(aronDto) << "Failed casting to HumanPose";
- HumanPose humanPose;
- fromAron(*aronDto, humanPose);
+ HumanPose humanPose;
+ fromAron(*aronDto, humanPose);
- humanPoses.push_back(humanPose);
+ humanPoses.push_back(humanPose);
+ };
+ });
});
+ // when no humans are in the scene, this is empty
+ //ARMARX_CHECK_NOT_EMPTY(humanPoses);
+
return humanPoses;
}
Reader::Result
Reader::query(const Query& query) const
{
+ ARMARX_TRACE;
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
@@ -98,16 +117,19 @@ namespace armarx::armem::human::client
.errorMessage = qResult.errorMessage};
}
+ ARMARX_TRACE;
const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
if (query.providerName.empty())
{
+ ARMARX_TRACE;
+
std::vector<HumanPose> allHumanPoses;
coreSegment.forEachProviderSegment(
- [&allHumanPoses](const auto& providerSegment)
+ [&allHumanPoses, &query](const auto& providerSegment)
{
- const std::vector<HumanPose> humanPoses = asHumanPoses(providerSegment);
+ const std::vector<HumanPose> humanPoses = asHumanPoses(providerSegment, query.timestamp, query.maxAge);
std::copy(
humanPoses.begin(), humanPoses.end(), std::back_inserter(allHumanPoses));
});
@@ -123,14 +145,18 @@ namespace armarx::armem::human::client
return Result{.humanPoses = allHumanPoses, .status = Result::Status::Success};
}
+ ARMARX_TRACE;
+
// -> provider segment name is set
if (not coreSegment.hasProviderSegment(query.providerName))
{
- ARMARX_WARNING << "Provider segment `" << query.providerName
- << "` does not exist (yet).";
+ ARMARX_INFO << deactivateSpam(5) << "Provider segment `" << query.providerName
+ << "` does not exist (yet).";
return {.humanPoses = {}, .status = Result::Status::NoData};
}
+ ARMARX_TRACE;
+
const wm::ProviderSegment& providerSegment =
coreSegment.getProviderSegment(query.providerName);
@@ -143,7 +169,9 @@ namespace armarx::armem::human::client
try
{
- const auto humanPoses = asHumanPoses(providerSegment);
+ ARMARX_TRACE;
+
+ const auto humanPoses = asHumanPoses(providerSegment, query.timestamp, query.maxAge);
return Result{.humanPoses = humanPoses, .status = Result::Status::Success};
}
catch (...)
diff --git a/source/VisionX/libraries/armem_human/client/HumanPoseReader.h b/source/VisionX/libraries/armem_human/client/HumanPoseReader.h
index 4903bb90e240eccff6bb79000c1162e9d329c4c9..0cb2bc1b007233d5fbe330f1f186ebe738b90c53 100644
--- a/source/VisionX/libraries/armem_human/client/HumanPoseReader.h
+++ b/source/VisionX/libraries/armem_human/client/HumanPoseReader.h
@@ -41,6 +41,8 @@ namespace armarx::armem::human::client
// if empty, will query all providers
std::string providerName;
armem::Time timestamp;
+
+ Duration maxAge;
};
struct Result
diff --git a/source/VisionX/libraries/armem_human/client/HumanPoseWriter.cpp b/source/VisionX/libraries/armem_human/client/HumanPoseWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6cf3045384826f0639db1d74cff720471d2fe39
--- /dev/null
+++ b/source/VisionX/libraries/armem_human/client/HumanPoseWriter.cpp
@@ -0,0 +1,99 @@
+#include "HumanPoseWriter.h"
+
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+
+#include <VisionX/libraries/armem_human/aron_conversions.h>
+
+
+namespace armarx::armem::human::client
+{
+
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem,
+ const HumanMemoryServerInterfacePrx& humanMemory) :
+ armem::client::util::SimpleWriterBase(memoryNameSystem), humanMemory{humanMemory}
+ {
+ ;
+ }
+
+
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ;
+ }
+
+
+ void
+ Writer::connect()
+ {
+ ;
+ }
+
+
+ bool
+ Writer::commitHumanPose(const HumanPose& humanPose,
+ const std::string& provider,
+ const armem::Time& timeCreated)
+ {
+ ARMARX_WARNING << "NYI";
+ return true;
+ }
+
+
+ bool
+ Writer::commitHumanPoses(const std::vector<HumanPose>& humanPoses,
+ const std::string& provider,
+ const armem::Time& timeCreated)
+ {
+ ARMARX_WARNING << "NYI";
+ return true;
+ }
+
+
+ bool
+ Writer::commitHumanPoseInCameraFrame(const HumanPose& humanPose,
+ const std::string& provider,
+ const armem::Time& timeCreated)
+ {
+ return commitHumanPosesInCameraFrame({humanPose}, provider, timeCreated);
+ }
+
+
+ bool
+ Writer::commitHumanPosesInCameraFrame(const std::vector<HumanPose>& humanPoses,
+ const std::string& provider,
+ const armem::Time& timeCreated)
+ {
+ using armarx::armem::human::toAron;
+ using armarx::aron::toAron;
+
+ std::vector<armarx::aron::data::dto::DictPtr> humanPosesDto;
+ for (const HumanPose& humanPose : humanPoses)
+ {
+ armarx::human::arondto::HumanPose humanPoseAron;
+ toAron(humanPoseAron, humanPose);
+ humanPosesDto.push_back(humanPoseAron.toAronDTO());
+ }
+
+ core::time::dto::DateTime timeCreatedDto;
+ toIce(timeCreatedDto, timeCreated);
+
+ try
+ {
+ humanMemory->commitHumanPosesInCameraFrame(humanPosesDto, provider, timeCreatedDto);
+ return true;
+ }
+ catch (const Ice::Exception& e)
+ {
+ ARMARX_ERROR << "Failed to commit human poses: " << e;
+ return false;
+ }
+ }
+
+} // namespace armarx::armem::human::client
diff --git a/source/VisionX/libraries/armem_human/client/HumanPoseWriter.h b/source/VisionX/libraries/armem_human/client/HumanPoseWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e92dc34ee5e1ab14a0ff51457717b5ef2ca6da7
--- /dev/null
+++ b/source/VisionX/libraries/armem_human/client/HumanPoseWriter.h
@@ -0,0 +1,77 @@
+#pragma once
+
+
+#include <mutex>
+#include <optional>
+#include <string>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
+#include <VisionX/libraries/armem_human/server/HumanMemoryServerInterface.h>
+#include <VisionX/libraries/armem_human/types.h>
+
+
+namespace armarx::armem::human::client
+{
+
+ class Writer : virtual public armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armem::client::util::SimpleWriterBase::SimpleWriterBase;
+
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem,
+ const armem::human::HumanMemoryServerInterfacePrx& humanMemory);
+
+ ~Writer() override;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ std::string propertyPrefix() const override { return "mem.human.pose.";};
+ Properties defaultProperties() const override {
+ return Properties
+ {
+ .memoryName = "Human",
+ .coreSegmentName = "Pose",
+ .providerName = ""
+ };
+ };
+
+ // FIXME remove provider, add to c'tor
+ bool commitHumanPose(const HumanPose& humanPose,
+ const std::string& provider,
+ const armem::Time& timeCreated);
+
+ bool commitHumanPoses(const std::vector<HumanPose>& humanPoses,
+ const std::string& provider,
+ const armem::Time& timeCreated);
+
+ bool commitHumanPoseInCameraFrame(const HumanPose& humanPose,
+ const std::string& provider,
+ const armem::Time& timeCreated);
+
+ /**
+ * @brief Calls a dedicated interface of the human memory in order to resolve keypoints
+ * in several frames given the detection in the camera frame.
+ * @param humanPose
+ * @param provider
+ * @return
+ */
+ bool commitHumanPosesInCameraFrame(const std::vector<HumanPose>& humanPoses,
+ const std::string& provider,
+ const armem::Time& timeCreated);
+
+ private:
+ armem::client::Writer memoryWriter;
+
+ armem::human::HumanMemoryServerInterfacePrx humanMemory;
+
+ std::mutex memoryWriterMutex;
+ };
+
+} // namespace armarx::armem::human::client
diff --git a/source/VisionX/libraries/armem_human/server/CMakeLists.txt b/source/VisionX/libraries/armem_human/server/CMakeLists.txt
index 38de7e588d21b2843ff923ee78b269d6510be3d4..c06b8f7ba6bd4d989c3a6ff0d5c4cc963ffb9c9f 100644
--- a/source/VisionX/libraries/armem_human/server/CMakeLists.txt
+++ b/source/VisionX/libraries/armem_human/server/CMakeLists.txt
@@ -9,8 +9,20 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_build_if(OpenCV_FOUND "OpenCV not available")
+armarx_add_component_interface_lib(
+ SLICE_FILES
+ HumanMemoryServerInterface.ice
+ ICE_LIBS
+ ArmarXCore
+ RobotAPI
+ VisionX
+)
+
+
armarx_add_library(
LIBS
+ DebugObserverHelper
+ VisionXArViz
armem_human
armem_server
@@ -18,6 +30,7 @@ armarx_add_library(
IdentificationSegment.cpp
PoseSegment.cpp
PersonInstanceSegment.cpp
+ Visualization.cpp
face_recognition/Segment.cpp
@@ -29,6 +42,7 @@ armarx_add_library(
IdentificationSegment.h
PoseSegment.h
PersonInstanceSegment.h
+ Visualization.h
face_recognition/Segment.h
diff --git a/source/VisionX/libraries/armem_human/server/HumanMemoryServerInterface.ice b/source/VisionX/libraries/armem_human/server/HumanMemoryServerInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..8b63f61c1abb53cf6d1b32fabcc63a2d9e4b3a6d
--- /dev/null
+++ b/source/VisionX/libraries/armem_human/server/HumanMemoryServerInterface.ice
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <ArmarXCore/interface/core/time.ice>
+
+#include <RobotAPI/interface/aron.ice>
+
+module armarx
+{
+ module armem
+ {
+ module human{
+
+ interface HumanMemoryServerInterface{
+
+ /**
+ * @brief Commit a human pose.
+ *
+ * If the global pose information of the keypoints is not available,
+ * a lookup will be performed and the corresponding fields will be set.
+ *
+ * @param dto a vector of serialized variants of armarx::human::arondto::HumanPose
+ * @param providerName
+ * @param timestamp
+ */
+ void commitHumanPosesInCameraFrame(::armarx::aron::data::dto::AronDictSeq dictSeq,
+ string providerName,
+ ::armarx::core::time::dto::DateTime timestamp);
+
+ }
+ };
+ };
+
+};
diff --git a/source/VisionX/libraries/armem_human/server/PoseSegment.cpp b/source/VisionX/libraries/armem_human/server/PoseSegment.cpp
index 3b4d6f5ccad100ef0c5f1de0ba6eaf98493d862a..b7f3cd56f738e96ca9bbceee595a18e26ab6dc32 100644
--- a/source/VisionX/libraries/armem_human/server/PoseSegment.cpp
+++ b/source/VisionX/libraries/armem_human/server/PoseSegment.cpp
@@ -22,20 +22,20 @@
#include "PoseSegment.h"
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
-#include <VisionX/libraries/armem_human/memory_ids.h>
-
+#include "VisionX/libraries/armem_human/types.h"
+#include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
+#include <VisionX/libraries/armem_human/aron_conversions.h>
namespace armarx::armem::server::human
{
- const std::string PoseSegment::CORE_SEGMENT_NAME =
- armarx::human::PoseCoreSegmentID.coreSegmentName;
+ const std::string PoseSegment::CORE_SEGMENT_NAME = "Pose";
PoseSegment::PoseSegment(armem::server::MemoryToIceAdapter& iceMemory) :
- Base(iceMemory, CORE_SEGMENT_NAME, armarx::human::arondto::Body25Pose3D::ToAronType(), 256)
+ Base(iceMemory, CORE_SEGMENT_NAME, armarx::human::arondto::HumanPose::ToAronType(), 256)
{
}
@@ -46,4 +46,43 @@ namespace armarx::armem::server::human
Base::defineProperties(defs, prefix);
}
+
+ std::map<std::string, std::vector<armarx::armem::human::HumanPose>>
+ PoseSegment::getHumanPoseEntities(const Duration& maxAge) const
+ {
+ std::map<std::string, std::vector<armarx::armem::human::HumanPose>> humanPosesMap;
+
+ const auto now = armarx::Clock::Now();
+
+ segmentPtr->forEachProviderSegment(
+ [&](const wm::ProviderSegment& provSegment)
+ {
+ humanPosesMap[provSegment.name()] = {};
+
+ provSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ if (entity.empty())
+ {
+ return;
+ }
+ entity.getLatestSnapshot().forEachInstance(
+ [&](const wm::EntityInstance& instance)
+ {
+ armarx::human::arondto::HumanPose dto;
+ dto.fromAron(instance.data());
+
+ armarx::armem::human::HumanPose bo;
+ fromAron(dto, bo);
+
+ if( (now - instance.metadata().timeCreated) < maxAge )
+ {
+ humanPosesMap[provSegment.name()].push_back(bo);
+ }
+ });
+ });
+ });
+
+ return humanPosesMap;
+ }
} // namespace armarx::armem::server::human
diff --git a/source/VisionX/libraries/armem_human/server/PoseSegment.h b/source/VisionX/libraries/armem_human/server/PoseSegment.h
index bbd6983ca68b8936bd09f9fcb89e62f2cc3542f3..5789c040b0711408bb42ddb4fdbad7df01573a76 100644
--- a/source/VisionX/libraries/armem_human/server/PoseSegment.h
+++ b/source/VisionX/libraries/armem_human/server/PoseSegment.h
@@ -22,27 +22,32 @@
#pragma once
+#include <map>
#include <string>
+#include <ArmarXCore/core/time_minimal.h>
+
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <VisionX/libraries/armem_human/types.h>
+
namespace armarx::armem::server::human
{
class PoseSegment : public armem::server::segment::SpecializedCoreSegment
{
public:
-
using Base = armem::server::segment::SpecializedCoreSegment;
PoseSegment(armem::server::MemoryToIceAdapter& iceMemory);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
+ std::map<std::string, std::vector<armarx::armem::human::HumanPose>>
+ getHumanPoseEntities(const Duration& maxAge) const;
public:
-
static const std::string CORE_SEGMENT_NAME;
-
};
-}
+} // namespace armarx::armem::server::human
diff --git a/source/VisionX/libraries/armem_human/server/Visualization.cpp b/source/VisionX/libraries/armem_human/server/Visualization.cpp
index 264e2c890667a3fde764d867be33f4da9a234e58..cae6e3a7fbac8a61c683fb83677d183b1026868d 100644
--- a/source/VisionX/libraries/armem_human/server/Visualization.cpp
+++ b/source/VisionX/libraries/armem_human/server/Visualization.cpp
@@ -1,28 +1,184 @@
-/*
- * 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 VisionX::ArmarXObjects::armem_images_server
- * @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 "armem_human.h"
-
-namespace armarx
+#include "Visualization.h"
+
+#include <algorithm>
+#include <exception>
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/time/Metronome.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
+
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+
+#include <VisionX/libraries/ArViz/HumanPose.h>
+
+
+namespace armarx::armem::server::human
{
-}
+ Visu::Visu(const PoseSegment& poseSegment) : poseSegment(poseSegment)
+ {
+ Logging::setTag("HumanVisualization");
+ }
+
+
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+ }
+
+
+ void
+ Visu::init()
+ {
+ ;
+ }
+
+
+ void
+ Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ {
+ this->arviz = arviz;
+ if (debugObserver)
+ {
+ bool batchMode = true;
+ this->debugObserver = DebugObserverHelper("HumanMemory", debugObserver, batchMode);
+ }
+
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+ updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
+ updateTask->start();
+ }
+
+
+ void
+ Visu::visualizeHumanPoses(viz::Layer& layer,
+ const std::vector<armarx::armem::human::HumanPose>& humanPoses)
+ {
+ std::string providerName = "azure_kinect"; // TODO: Get actual value.
+
+ std::map<std::string, armarx::armem::human::HumanPose> poseMap;
+ for (const armarx::armem::human::HumanPose& humanPose : humanPoses)
+ {
+ ARMARX_CHECK(humanPose.humanTrackingId.has_value())
+ << "Can only visualize tracked humans at the moment.";
+ poseMap[humanPose.humanTrackingId.value()] = humanPose;
+ }
+
+ addPosesToLayer(poseMap, layer, providerName);
+ }
+
+
+ void
+ Visu::visualizeRun()
+ {
+ armarx::Metronome metronome{armarx::Frequency::Hertz(p.frequencyHz)};
+ while (updateTask and not updateTask->isStopped())
+ {
+ if (p.enabled)
+ {
+ const Time timestamp = Time::Now();
+ ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
+
+ try
+ {
+ visualizeOnce(timestamp);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
+ }
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ }
+ metronome.waitForNextTick();
+ }
+ }
+
+
+ void
+ Visu::visualizeOnce(const Time& timestamp)
+ {
+ TIMING_START(tVisuTotal);
+
+ // TODO(fabian.reister): use timestamp
+
+ const Duration maxAge = Duration::MilliSeconds(1000);
+
+ // Get data.
+ TIMING_START(tVisuGetData);
+
+ TIMING_START(tRobotDescriptions);
+ const std::map<std::string, std::vector<armarx::armem::human::HumanPose>>
+ humanPosesPerProvider = poseSegment.getHumanPoseEntities(maxAge);
+ TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
+
+ // Build layers.
+ TIMING_START(tVisuBuildLayers);
+
+ std::vector<viz::Layer> layers;
+ for (const auto& [providerName, humanPoses] : humanPosesPerProvider)
+ {
+ viz::Layer layer = arviz.layer("HumanPoses_" + providerName);
+ visualizeHumanPoses(layer, humanPoses);
+ layers.push_back(layer);
+ }
+
+ TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+
+
+ // Commit layers.
+
+ ARMARX_DEBUG << "Commit visualization ...";
+ TIMING_START(tVisuCommit);
+ arviz.commit(layers);
+ TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
+
+ TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
+
+ if (debugObserver.has_value())
+ {
+ const std::string p = "Visu | ";
+ debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
+ tVisuTotal.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
+ tVisuGetData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
+ tRobotDescriptions.toMilliSecondsDouble());
+
+ debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
+ tVisuBuildLayers.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
+ tVisuCommit.toMilliSecondsDouble());
+ }
+ }
+
+} // namespace armarx::armem::server::human
diff --git a/source/VisionX/libraries/armem_human/server/Visualization.h b/source/VisionX/libraries/armem_human/server/Visualization.h
index 34fc14ff5c9f2862bce83eddfa69e59a22cc597f..dd85541d92529981d0396eaa5facde5a32daa29c 100644
--- a/source/VisionX/libraries/armem_human/server/Visualization.h
+++ b/source/VisionX/libraries/armem_human/server/Visualization.h
@@ -1,34 +1,54 @@
-/*
- * 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 VisionX::ArmarXObjects::armem_images_server
- * @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
-namespace armarx
+#include <optional>
+#include <vector>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+#include <VisionX/libraries/armem_human/server/PoseSegment.h>
+#include <VisionX/libraries/armem_human/types.h>
+
+
+namespace armarx::armem::server::human
{
- class armem_human
+
+ class Visu : public armarx::Logging
{
public:
+ Visu(const PoseSegment& poseSegment);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
+ void init();
+ void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
+
+ private:
+ void visualizeRun();
+ void visualizeOnce(const Time& timestamp);
+
+ static void
+ visualizeHumanPoses(viz::Layer& layer,
+ const std::vector<armarx::armem::human::HumanPose>& humanPoses);
+
+ private:
+ viz::Client arviz;
+ std::optional<DebugObserverHelper> debugObserver;
+
+ const PoseSegment& poseSegment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+ SimpleRunningTask<>::pointer_type updateTask;
};
-}
+} // namespace armarx::armem::server::human
diff --git a/source/VisionX/libraries/armem_human/types.h b/source/VisionX/libraries/armem_human/types.h
index b83edaa6709219fd102d9b362447d8e6b4ee9a36..3bd48cbe55f40045c230cdceb0097d2c26916a47 100644
--- a/source/VisionX/libraries/armem_human/types.h
+++ b/source/VisionX/libraries/armem_human/types.h
@@ -1,38 +1,56 @@
#pragma once
-#include <Eigen/Core>
-#include <SimoxUtility/color/Color.h>
+
#include <map>
+#include <optional>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
-namespace armarx::armem::human {
+namespace armarx::armem::human
+{
-struct Keypoint2D {
- std::string label;
+ struct PoseKeypoint
+ {
+ std::string label;
+ float confidence;
- Eigen::Vector2f position; // which frame?
+ armarx::FramedPosition positionCamera;
+ std::optional<armarx::FramedOrientation> orientationCamera = std::nullopt;
+ std::optional<armarx::FramedPosition> positionRobot = std::nullopt;
+ std::optional<armarx::FramedOrientation> orientationRobot = std::nullopt;
+ std::optional<armarx::FramedPosition> positionGlobal = std::nullopt;
+ std::optional<armarx::FramedOrientation> orientationGlobal = std::nullopt;
+ };
- float confidence;
- simox::color::Color dominantColor;
-};
+ struct HumanPose
+ {
+ std::string poseModelId;
+
+ using KeyPointMap = std::map<std::string, PoseKeypoint>;
+ KeyPointMap keypoints;
+ std::optional<std::string> humanTrackingId = std::nullopt;
+ };
-struct Keypoint3D {
- std::string label;
- Eigen::Vector3f positionRobot;
- Eigen::Vector3f positionGlobal;
+ struct PoseKeypoint2D
+ {
+ std::string label;
- float confidence;
+ float confidence;
- simox::color::Color dominantColor;
-};
+ Eigen::Vector2f position;
+ };
-using Keypoint2DIdMap = std::map<std::string, Keypoint2D>;
-using Keypoint3DIdMap = std::map<std::string, Keypoint3D>;
-struct HumanPose {
- Keypoint2DIdMap keypoint2dMap;
- Keypoint3DIdMap keypoint3dMap;
-};
+ struct HumanPose2D
+ {
+ std::string poseModelId;
+ std::map<std::string, PoseKeypoint2D> keypoints;
+ std::optional<std::string> humanTrackingId = std::nullopt;
+ };
} // namespace armarx::armem::human
diff --git a/source/VisionX/libraries/human/CMakeLists.txt b/source/VisionX/libraries/human/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3e20b98dd87ca9386cf90a8d4da51bc6590480cb
--- /dev/null
+++ b/source/VisionX/libraries/human/CMakeLists.txt
@@ -0,0 +1,17 @@
+armarx_set_target("human")
+
+set(SOURCES
+ pose/model/foo.cpp
+)
+
+set(HEADERS
+ pose/model/k4a_bt_body_32.h
+ pose/model/openpose_body_25.h
+)
+
+set(COMPONENT_LIBS
+ VisionXCore
+ Simox::SimoxUtility
+)
+
+armarx_add_library(human "${SOURCES}" "${HEADERS}" "${COMPONENT_LIBS}")
diff --git a/source/VisionX/libraries/human/pose/model/foo.cpp b/source/VisionX/libraries/human/pose/model/foo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/VisionX/libraries/human/pose/model/k4a_bt_body_32.h b/source/VisionX/libraries/human/pose/model/k4a_bt_body_32.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0f0a294cdb567230ff28b27a7c04b67a8530e0e
--- /dev/null
+++ b/source/VisionX/libraries/human/pose/model/k4a_bt_body_32.h
@@ -0,0 +1,182 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+
+/**
+ * Body model taken from https://docs.microsoft.com/en-us/azure/kinect-dk/body-joints and used
+ * with Azure Kinect Body Tracking SDK 1.1.1.
+ */
+namespace armarx::human::pose::model::k4a_bt_body_32
+{
+ inline const std::string ModelId = "K4ABT_BODY_32";
+
+ /**
+ * Joints with index as defined in the body model.
+ */
+ enum class Joints
+ {
+ Pelvis,
+ SpineNaval,
+ SpineChest,
+ Neck,
+ ClavicleLeft,
+ ShoulderLeft,
+ ElbowLeft,
+ WristLeft,
+ HandLeft,
+ HandtipLeft,
+ ThumbLeft,
+ ClavicleRight,
+ ShoulderRight,
+ ElbowRight,
+ WristRight,
+ HandRight,
+ HandtipRight,
+ ThumbRight,
+ HipLeft,
+ KneeLeft,
+ AnkleLeft,
+ FootLeft,
+ HipRight,
+ KneeRight,
+ AnkleRight,
+ FootRight,
+ Head,
+ Nose,
+ EyeLeft,
+ EarLeft,
+ EyeRight,
+ EarRight
+ };
+
+ /**
+ * Names of the joints as defined in the body model.
+ */
+ inline const simox::meta::EnumNames<Joints> JointNames{{Joints::Pelvis, "Pelvis"},
+ {Joints::SpineNaval, "SpineNaval"},
+ {Joints::SpineChest, "SpineChest"},
+ {Joints::Neck, "Neck"},
+ {Joints::ClavicleLeft, "ClavicleLeft"},
+ {Joints::ShoulderLeft, "ShoulderLeft"},
+ {Joints::ElbowLeft, "ElbowLeft"},
+ {Joints::WristLeft, "WristLeft"},
+ {Joints::HandLeft, "HandLeft"},
+ {Joints::HandtipLeft, "HandtipLeft"},
+ {Joints::ThumbLeft, "ThumbLeft"},
+ {Joints::ClavicleRight, "ClavicleRight"},
+ {Joints::ShoulderRight, "ShoulderRight"},
+ {Joints::ElbowRight, "ElbowRight"},
+ {Joints::WristRight, "WristRight"},
+ {Joints::HandRight, "HandRight"},
+ {Joints::HandtipRight, "HandtipRight"},
+ {Joints::ThumbRight, "ThumbRight"},
+ {Joints::HipLeft, "HipLeft"},
+ {Joints::KneeLeft, "KneeLeft"},
+ {Joints::AnkleLeft, "AnkleLeft"},
+ {Joints::FootLeft, "FootLeft"},
+ {Joints::HipRight, "HipRight"},
+ {Joints::KneeRight, "KneeRight"},
+ {Joints::AnkleRight, "AnkleRight"},
+ {Joints::FootRight, "FootRight"},
+ {Joints::Head, "Head"},
+ {Joints::Nose, "Nose"},
+ {Joints::EyeLeft, "EyeLeft"},
+ {Joints::EarLeft, "EarLeft"},
+ {Joints::EyeRight, "EyeRight"},
+ {Joints::EarRight, "EarRight"}};
+
+ /**
+ * Segments (defined by pairs of joints) as defined in the body model.
+ */
+ inline const std::vector<std::pair<Joints, Joints>> Segments{
+ {Joints::SpineNaval, Joints::Pelvis},
+ {Joints::SpineChest, Joints::SpineNaval},
+ {Joints::Neck, Joints::SpineChest},
+ {Joints::ClavicleLeft, Joints::SpineChest},
+ {Joints::ShoulderLeft, Joints::ClavicleLeft},
+ {Joints::ElbowLeft, Joints::ShoulderLeft},
+ {Joints::WristLeft, Joints::ElbowLeft},
+ {Joints::HandLeft, Joints::WristLeft},
+ {Joints::HandtipLeft, Joints::HandLeft},
+ {Joints::ThumbLeft, Joints::WristLeft},
+ {Joints::ClavicleRight, Joints::SpineChest},
+ {Joints::ShoulderRight, Joints::ClavicleRight},
+ {Joints::ElbowRight, Joints::ShoulderRight},
+ {Joints::WristRight, Joints::ElbowRight},
+ {Joints::HandRight, Joints::WristRight},
+ {Joints::HandtipRight, Joints::HandRight},
+ {Joints::ThumbRight, Joints::WristRight},
+ {Joints::HipLeft, Joints::Pelvis},
+ {Joints::KneeLeft, Joints::HipLeft},
+ {Joints::AnkleLeft, Joints::KneeLeft},
+ {Joints::FootLeft, Joints::AnkleLeft},
+ {Joints::HipRight, Joints::Pelvis},
+ {Joints::KneeRight, Joints::HipRight},
+ {Joints::AnkleRight, Joints::KneeRight},
+ {Joints::FootRight, Joints::AnkleRight},
+ {Joints::Head, Joints::Neck},
+ {Joints::Nose, Joints::Head},
+ {Joints::EyeLeft, Joints::Head},
+ {Joints::EarLeft, Joints::Head},
+ {Joints::EyeRight, Joints::Head},
+ {Joints::EarRight, Joints::Head}};
+
+
+ inline const std::map<Joints, simox::Color> Colors = {
+ {Joints::Pelvis, {0.f, 1.f, 0.f}},
+ {Joints::SpineNaval, {0.f, 1.f, 0.f}},
+ {Joints::SpineChest, {1.f, 0.f, 85.f / 255.f}},
+ {Joints::Neck, {1.f, 0.f, 85.f / 255.f}},
+ {Joints::ClavicleLeft, {1.f, 1.f, 0.f}},
+ {Joints::ShoulderLeft, {1.f, 1.f, 0.f}},
+ {Joints::ElbowLeft, {170.f / 255.f, 1.f, 0.f}},
+ {Joints::WristLeft, {85.f / 255.f, 1.f, 0.f}},
+ {Joints::HandLeft, {1.f, 0.f, 0.f}},
+ {Joints::HandtipLeft, {0.f, 1.f, 0.f}},
+ {Joints::ThumbLeft, {0.f, 0.f, 1.f}},
+ {Joints::ClavicleRight, {1.f, 0.f, 0.f}},
+ {Joints::ShoulderRight, {1.f, 0.f, 0.f}},
+ {Joints::ElbowRight, {1.f, 85.f / 255.f, 0.f}},
+ {Joints::WristRight, {1.f, 170.f / 255.f, 0.f}},
+ {Joints::HandRight, {1.f, 0.f, 0.f}},
+ {Joints::HandtipRight, {0.f, 1.f, 0.f}},
+ {Joints::ThumbRight, {0.f, 0.f, 1.f}},
+ {Joints::HipLeft, {0.f, 1.f, 1.f}},
+ {Joints::KneeLeft, {0.f, 170.f / 255.f, 1.f}},
+ {Joints::AnkleLeft, {0.f, 85.f / 255.f, 1.f}},
+ {Joints::FootLeft, {85.f / 255.f, 0.f, 1.f}},
+ {Joints::HipRight, {1.f, 0.f, 0.f}},
+ {Joints::KneeRight, {0.f, 1.f, 85.f / 255.f}},
+ {Joints::AnkleRight, {0.f, 1.f, 170.f / 255.f}},
+ {Joints::FootRight, {0.f, 0.f, 1.f}},
+ {Joints::Head, {1.f, 0.f, 85.f / 255.f}},
+ {Joints::Nose, {1.f, 0.f, 85.f / 255.f}},
+ {Joints::EyeLeft, {1.f, 0.f, 170.f / 255.f}},
+ {Joints::EarLeft, {1.f, 0.f, 1.f}},
+ {Joints::EyeRight, {0.f, 0.f, 1.f}},
+ {Joints::EarRight, {170.f / 255.f, 0.f, 1.f}}};
+
+} // namespace armarx::human::pose::model::k4a_bt_body_32
diff --git a/source/VisionX/libraries/human/pose/model/openpose_body_25.h b/source/VisionX/libraries/human/pose/model/openpose_body_25.h
new file mode 100644
index 0000000000000000000000000000000000000000..18c5a091c896c3e1d3bdc414f97ab0a3088bda7a
--- /dev/null
+++ b/source/VisionX/libraries/human/pose/model/openpose_body_25.h
@@ -0,0 +1,146 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+
+namespace armarx::human::pose::model::openpose_body_25
+{
+ inline const std::string ModelId = "BODY_25";
+
+ enum class Joints
+ {
+ Nose,
+ Neck,
+ ShoulderRight,
+ ElbowRight,
+ WristRight,
+ ShoulderLeft,
+ ElbowLeft,
+ WristLeft,
+ Pelvis,
+ HipRight,
+ KneeRight,
+ AnkleRight,
+ HipLeft,
+ KneeLeft,
+ AnkleLeft,
+ EyeRight,
+ EyeLeft,
+ EarRight,
+ EarLeft,
+ BigToeLeft,
+ SmallToeLeft,
+ HeelLeft,
+ BigToeRight,
+ SmallToeRight,
+ HeelRight,
+ Background
+ };
+
+ inline const simox::meta::EnumNames<Joints> JointNames{{Joints::Nose, "Nose"},
+ {Joints::Neck, "Neck"},
+ {Joints::ShoulderRight, "RShoulder"},
+ {Joints::ElbowRight, "RElbow"},
+ {Joints::WristRight, "RWrist"},
+ {Joints::ShoulderLeft, "LShoulder"},
+ {Joints::ElbowLeft, "LElbow"},
+ {Joints::WristLeft, "LWrist"},
+ {Joints::Pelvis, "MidHip"},
+ {Joints::HipRight, "RHip"},
+ {Joints::KneeRight, "RKnee"},
+ {Joints::AnkleRight, "RAnkle"},
+ {Joints::HipLeft, "LHip"},
+ {Joints::KneeLeft, "LKnee"},
+ {Joints::AnkleLeft, "LAnkle"},
+ {Joints::EyeRight, "REye"},
+ {Joints::EyeLeft, "LEye"},
+ {Joints::EarRight, "REar"},
+ {Joints::EarLeft, "LEar"},
+ {Joints::BigToeLeft, "LBigToe"},
+ {Joints::SmallToeLeft, "LSmallToe"},
+ {Joints::HeelLeft, "LHeel"},
+ {Joints::BigToeRight, "RBigToe"},
+ {Joints::SmallToeRight, "RSmallToe"},
+ {Joints::HeelRight, "RHeel"},
+ {Joints::Background, "Background"}};
+
+
+ inline const std::vector<std::pair<Joints, Joints>> Segments = {
+ {Joints::Neck, Joints::Pelvis},
+ {Joints::Neck, Joints::ShoulderRight},
+ {Joints::Neck, Joints::ShoulderLeft},
+ {Joints::ShoulderRight, Joints::ElbowRight},
+ {Joints::ElbowRight, Joints::WristRight},
+ {Joints::ShoulderLeft, Joints::ElbowLeft},
+ {Joints::ElbowLeft, Joints::WristLeft},
+ {Joints::Pelvis, Joints::HipRight},
+ {Joints::HipRight, Joints::KneeRight},
+ {Joints::KneeRight, Joints::AnkleRight},
+ {Joints::Pelvis, Joints::HipLeft},
+ {Joints::HipLeft, Joints::KneeLeft},
+ {Joints::KneeLeft, Joints::AnkleLeft},
+ {Joints::Neck, Joints::Nose},
+ {Joints::Nose, Joints::EyeRight},
+ {Joints::EyeRight, Joints::EarRight},
+ {Joints::Nose, Joints::EyeLeft},
+ {Joints::EyeLeft, Joints::EarLeft},
+ {Joints::AnkleLeft, Joints::BigToeLeft},
+ {Joints::BigToeLeft, Joints::SmallToeLeft},
+ {Joints::AnkleLeft, Joints::HeelLeft},
+ {Joints::AnkleRight, Joints::BigToeRight},
+ {Joints::BigToeRight, Joints::SmallToeRight},
+ {Joints::AnkleRight, Joints::HeelRight}};
+
+
+ inline const std::map<Joints, simox::Color> Colors = {
+ {Joints::Nose, {1.f, 0.f, 85.f / 255.f}},
+ {Joints::Neck, {1.f, 0.f, 0.f}},
+ {Joints::ShoulderRight, {1.f, 85.f / 255.f, 0.f}},
+ {Joints::ElbowRight, {1.f, 170.f / 255.f, 0.f}},
+ {Joints::WristRight, {1.f, 1.f, 0.f}},
+ {Joints::ShoulderLeft, {170.f / 255.f, 1.f, 0.f}},
+ {Joints::ElbowLeft, {85.f / 255.f, 1.f, 0.f}},
+ {Joints::WristLeft, {0.f, 1.f, 0.f}},
+ {Joints::Pelvis, {1.f, 0.f, 0.f}},
+ {Joints::HipRight, {0.f, 1.f, 85.f / 255.f}},
+ {Joints::KneeRight, {0.f, 1.f, 170.f / 255.f}},
+ {Joints::AnkleRight, {0.f, 1.f, 1.f}},
+ {Joints::HipLeft, {0.f, 170.f / 255.f, 1.f}},
+ {Joints::KneeLeft, {0.f, 85.f / 255.f, 1.f}},
+ {Joints::AnkleLeft, {0.f, 0.f, 1.f}},
+ {Joints::EyeRight, {1.f, 0.f, 170.f / 255.f}},
+ {Joints::EyeLeft, {170.f / 255.f, 0.f, 1.f}},
+ {Joints::EarRight, {1.f, 0.f, 1.f}},
+ {Joints::EarLeft, {85.f / 255.f, 0.f, 1.f}},
+ {Joints::BigToeLeft, {0.f, 0.f, 1.f}},
+ {Joints::SmallToeLeft, {0.f, 0.f, 1.f}},
+ {Joints::HeelLeft, {0.f, 0.f, 1.f}},
+ {Joints::BigToeRight, {0.f, 1.f, 1.f}},
+ {Joints::SmallToeRight, {0.f, 1.f, 1.f}},
+ {Joints::HeelRight, {0.f, 1.f, 1.f}}};
+ // Joints::Background has no color assigned
+
+
+} // namespace armarx::human::pose::model::openpose_body_25