diff --git a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
index 3c4d6c5aa7cca9ac5f9e6fa1d4e5a65b84361392..570cffa4bdc3848ed6f32959981904d3bac7ecfc 100644
--- a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
+++ b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
@@ -11,5 +11,7 @@
<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="dynamic_scene_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg b/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..28099ebc83514ac57b5af9e5acba37e72a6f3846
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/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/HumanAwareNavigation/config/control_memory.cfg b/scenarios/HumanAwareNavigation/config/control_memory.cfg
index 4aad1c46be9d15f4fb20cee16506124bfc282af0..53efd6fedf80402149271390c3c93fca3e434c63 100644
--- a/scenarios/HumanAwareNavigation/config/control_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/control_memory.cfg
@@ -92,37 +92,21 @@
# ArmarX.ControlMemory.mem.MemoryName = Control
-# ArmarX.ControlMemory.mem.ltm..configuration:
+# ArmarX.ControlMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ControlMemory.mem.ltm..configuration = ""
+# ArmarX.ControlMemory.mem.ltm.configuration = {}
-# ArmarX.ControlMemory.mem.ltm..enabled:
+# ArmarX.ControlMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ControlMemory.mem.ltm..enabled = false
-
-
-# ArmarX.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ControlMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ControlMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ControlMemory.mem.ltm.enabled = false
# ArmarX.ControlMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6059c7a85d41e91d2c9daffbe3ea5a3913898b9b
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,262 @@
+# ==================================================================
+# distance_to_obstacle_costmap_provider 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.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
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName: Robot name.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+
+
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
index 27479d20a7500ec6433dbe92d239e04c8eff073a..565bb501258d5d9dc917d2a5cd2108937980babf 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -260,6 +260,45 @@
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name = Armar6
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name = distance_to_obstacles
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = ::_NOT_SET_::
+
+
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs:
# Attributes:
# - Default: 100
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
index 98cd68b74fc94662bd85fcba3ee7c6943fcd4eb0..c632df10adc981f508fe20503aa145291a06f4ee 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
@@ -191,7 +191,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.dynamic_scene_provider.MinimumLoggingLevel = Undefined
+ArmarX.dynamic_scene_provider.MinimumLoggingLevel = Verbose
# ArmarX.dynamic_scene_provider.ObjectMemoryName: Name of the object memory.
@@ -242,6 +242,30 @@
# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory = Navigation
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider = ""
+
+
# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
@@ -308,6 +332,14 @@
# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider:
+# Attributes:
+# - Default: OpenNIPointCloudProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider = OpenNIPointCloudProvider
+
+
# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name:
# Attributes:
# - Default: ""
diff --git a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
index c024b25654427649329bf33ff2f0574f6c807ccf..38cf4a09238d735ec3bf53f76b7a5ebdeb8589e7 100644
--- a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
@@ -152,10 +152,10 @@ ArmarX.NavigationMemory.MinimumLoggingLevel = Verbose
# ArmarX.NavigationMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.configuration = ""
+# ArmarX.NavigationMemory.mem.ltm.configuration = {}
# ArmarX.NavigationMemory.mem.ltm.enabled:
@@ -167,22 +167,6 @@ ArmarX.NavigationMemory.MinimumLoggingLevel = Verbose
# ArmarX.NavigationMemory.mem.ltm.enabled = false
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.NavigationMemory.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:
diff --git a/scenarios/HumanAwareNavigation/config/navigator.cfg b/scenarios/HumanAwareNavigation/config/navigator.cfg
index 0484c3f0c2af7c863310517a5cbbae12b1943545..5d03829068b253c3da8e6ce1d2277bebbab1743c 100644
--- a/scenarios/HumanAwareNavigation/config/navigator.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigator.cfg
@@ -181,14 +181,6 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.nav.costmap.Memory = Navigation
-# ArmarX.Navigator.mem.nav.costmap.Provider: Name of this provider
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Provider = ""
-
-
# ArmarX.Navigator.mem.nav.events.CoreSegment:
# Attributes:
# - Default: Events
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index dfd3bf35aa932b14d0087041740aa2c1f2bff40b..6093698656d8c8cd7f22d1c2e8e560b60b932d61 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(core)
add_subdirectory(util)
add_subdirectory(conversions)
add_subdirectory(algorithms)
+add_subdirectory(human)
add_subdirectory(dynamic_scene)
add_subdirectory(global_planning)
add_subdirectory(local_planning)
diff --git a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
index e2a2438387fbe50d1e96883fe79732388395ae2b..98a6934af3592c828b76ca66d3c1b80be8d7ca70 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
@@ -212,8 +212,8 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
const float cellSize = j.at("cell_size"); // [mm]
BOOST_REQUIRE_GT(cellSize, 0);
- std::vector<float> sceneBoundsMinV(j.at("scene_bounds"]["min"));
- std::vector<float> sceneBoundsMaxV(j.at("scene_bounds"]["max"));
+ std::vector<float> sceneBoundsMinV(j.at("scene_bounds").at("min"));
+ std::vector<float> sceneBoundsMaxV(j.at("scene_bounds").at("max"));
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 3f8da95e6b5031113f57c490131c7b1abc9d6e52..5f88e9cf859170337dd0eea66c110da31fc6a5de 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -18,6 +18,7 @@ armarx_add_component(navigation_memory
armarx_navigation::graph
armarx_navigation::location
armarx_navigation::algorithms
+ armarx_navigation::teb_human
SOURCES
NavigationMemory.cpp
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index a18764b02624ec4abf73fd5af497d82b2018aaa3..97d8fa3688c6d1d316446a5308cc1e162fa20b7e 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -48,6 +48,7 @@
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
@@ -127,6 +128,11 @@ namespace armarx::navigation
workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
navigation::core::arondto::Graph::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
+ navigation::human::arondto::Human::ToAronType());
+
+ // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ // navigation::human::arondto::Human::ToAronType());
if (not properties.snapshotToLoad.empty())
{
@@ -432,7 +438,8 @@ namespace armarx::navigation
memory::Visu visu{arviz,
workingMemory().getCoreSegment(navigation::location::coreSegmentID),
workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName)};
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
Properties::LocationGraph p;
@@ -462,6 +469,9 @@ namespace armarx::navigation
// Costmaps
visu.drawCostmaps(layers, p.visuCostmaps);
+ // Humans
+ visu.drawHumans(layers, p.visuHumans);
+
arviz.commit(layers);
metronome.waitForNextTick();
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index c67da7fc5981fd4044b04ab936c5c4126fd5ee32..e34aa21e949c6b77489edb3b13ed39b7c33f7d72 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -97,6 +97,7 @@ namespace armarx::navigation
bool visuLocations = true;
bool visuGraphEdges = true;
bool visuCostmaps = true;
+ bool visuHumans = true;
float visuFrequency = 2;
};
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 73df72debcb1eabe837036c877fd856d94029151..f3ae9cbc89f0d9646a61750f906fa18ac3d46bc3 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -22,18 +22,24 @@
#include "Visu.h"
+#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
+#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/human/aron/Human.aron.generated.h"
+#include "armarx/navigation/human/aron_conversions.h"
+#include "armarx/navigation/human/types.h"
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
namespace armarx::navigation::memory
@@ -42,11 +48,13 @@ namespace armarx::navigation::memory
Visu::Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment) :
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
+ humanSegment(humanSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -165,6 +173,24 @@ namespace armarx::navigation::memory
layer.add(mesh);
}
+ void
+ visualize(const human::Humans& humans, viz::Layer& layer)
+ {
+ ARMARX_INFO << "Visualizing " << humans.size() << " humans";
+ for (const auto& human : humans)
+ {
+ viz::Cylinder cylinder(std::to_string(layer.size()));
+ cylinder.fromTo(conv::to3D(human.pose.translation()),
+ conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+
+
+ cylinder.color(simox::Color::orange());
+ cylinder.radius(300);
+
+ layer.add(cylinder);
+ }
+ }
+
} // namespace
void
@@ -207,5 +233,45 @@ namespace armarx::navigation::memory
}
}
+ void
+ Visu::drawHumans(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::Humans> namedProviderHumans;
+
+ humanSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(instance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName]
+ .emplace_back(std::move(human));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humans] : namedProviderHumans)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ visualize(humans, layer);
+ }
+ }
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
index cfd732adeb4ab69ffe8cc284e830ccff0898a418..123a865dbfcc38db134f9b822f502a51d02152fe 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -28,6 +28,7 @@
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
#include "armarx/navigation/algorithms/Costmap.h"
@@ -45,13 +46,15 @@ namespace armarx::navigation::memory
Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment);
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment);
~Visu();
void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
+ void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
public:
@@ -60,6 +63,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment;
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
+ const armem::server::wm::CoreSegment& humanSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 146e5b7bb13f62cd0398b9624e893dd7828f225a..e061ea23b64b4980fe421fd0af0077b6b4fe727b 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -9,9 +9,11 @@ armarx_add_component(dynamic_scene_provider
SOURCES
Component.cpp
ArVizDrawer.cpp
+ HumanTracker.cpp
HEADERS
Component.h
ArVizDrawer.h
+ HumanTracker.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
@@ -29,9 +31,12 @@ armarx_add_component(dynamic_scene_provider
armarx_navigation::util
armarx_navigation::memory
armarx_navigation::dynamic_scene
+ armarx_navigation::teb_human
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
+ # DEPENDENCIES_PRIVATE
+ range-v3::range-v3
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
# FOO
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9f8cd9f7d970a0e4d5ccebbc2c5faf2412daf89c..4a1cd5e85110c42c1a150ab8cdcb1e442d3826ee 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -53,6 +53,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(virtualRobotReaderPlugin);
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
+ addPlugin(humanWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -86,6 +87,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
def->optional(properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
def->optional(properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
+
+ def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
return def;
}
@@ -137,14 +140,18 @@ namespace armarx::navigation::components::dynamic_scene_provider
robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
ARMARX_CHECK_NOT_NULL(robot);
+ humanTracker.reset();
+
task = new PeriodicTask<Component>(
this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+ task->start();
}
void
Component::onDisconnectComponent()
{
+ task->stop();
}
@@ -187,12 +194,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Human
//
- const armem::human::client::Reader::Query humanPoseQuery{.providerName = "", // all
+ ARMARX_INFO << "Querying humans";
+
+ const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
.timestamp = timestamp};
const armem::human::client::Reader::Result humanPoseResult =
humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success);
+ ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success) << humanPoseResult.errorMessage;
ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
@@ -200,6 +209,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
+ ARMARX_INFO << "Querying laser scanner features";
+
const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
@@ -217,6 +228,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Objects in the scene (both static and dynamic)
//
+ ARMARX_INFO << "Querying object poses";
+
const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
// remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
@@ -241,8 +254,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Costmaps
//
+ ARMARX_INFO << "Querying costmap";
+
const memory::client::costmap::Reader::Query costmapQuery{.providerName =
- "navigator", // TODO check
+ "distance_to_obstacle_costmap_provider", // TODO check
.name = "distance_to_obstacles",
.timestamp = timestamp};
@@ -314,6 +329,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
// arviz.commit({layer});
}
+
+ // here ends: data fetching
+
+ ARMARX_INFO << "Running human tracker";
+
+ humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
+ .humanPoses = humanPoseResult.humanPoses});
+
+
+ humanWriterPlugin->get().store(humanTracker.getTrackedHumans(),getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 1d7d92fc3291688e19160c571d44bf61930bb19c..bcc3ad7d5f8163380eeed9a5deb65429f6c9cd58 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -47,7 +47,9 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
+#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
@@ -150,6 +152,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
float freespaceThreshold = 0.45F;
float occupiedThreshold = 0.55;
} occupancyGrid;
+
+ std::string humanPoseProvider = "OpenNIPointCloudProvider";
};
Properties properties;
/* Use a mutex if you access variables from different threads
@@ -197,6 +201,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
occupancyGridReaderPlugin = nullptr;
+
+ ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
+
+ HumanTracker humanTracker;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4bdc8c4b33ce6c06bf6766729fc617432ce44333
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -0,0 +1,206 @@
+
+#include "HumanTracker.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+
+ HumanTracker::DetectedHuman
+ convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
+ {
+ const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+ Eigen::Vector3f centerPos;
+ int size = 0;
+ for (const auto& [_, v] : keypoints)
+ {
+ if (v.positionGlobal.has_value())
+ {
+ centerPos += v.positionGlobal.value().toEigen();
+ size++;
+ }
+ }
+ centerPos /= size;
+
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = conv::to2D(centerPos);
+ //TODO: angle
+ pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+
+ return {pose, humanPose.humanTrackingId, time, false};
+ }
+
+ void
+ HumanTracker::update(const Measurements& measurements)
+ {
+ trackedHumans.clear();
+
+ for (const armem::human::HumanPose& measurement : measurements.humanPoses)
+ {
+ human::Human human{
+ .pose = conv::to2D(
+ core::Pose(Eigen::Translation3f(measurement.keypoints.begin()->second.positionGlobal->toEigen()))),
+ .linearVelocity = Eigen::Vector2f::Zero(),
+ .detectionTime = 0};
+
+ trackedHumans.push_back(TrackedHuman{
+ .human = human, .trackingId = std::to_string(trackedHumans.size()), .associated = true});
+ }
+
+ return; // FIXME remove section above
+
+ for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+ {
+ auto& human = *it;
+ if ((measurements.detectionTime - human.human.detectionTime) >=
+ parameters.maxTrackingAge)
+ {
+ it = trackedHumans.erase(it);
+ }
+ else
+ {
+ human.associated = false;
+ human.human.pose = human.human.estimateAt(measurements.detectionTime);
+ it++;
+ }
+ }
+
+ std::vector<DetectedHuman> newPoses =
+ measurements.humanPoses |
+ ranges::views::transform(
+ [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
+ { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
+ ranges::to_vector;
+
+ associateHumans(newPoses);
+ }
+
+ struct PosDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
+ };
+
+ std::vector<PosDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosDistance> posDistances;
+
+ for (auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
+ {
+ continue;
+ }
+ for (auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
+ }
+ }
+
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosDistance& a, const PosDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
+ void
+ HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
+ {
+ // associate humans by their tracking id
+ for (auto& oldHuman : trackedHumans)
+ {
+ if (oldHuman.associated || !oldHuman.trackingId)
+ {
+ continue;
+ }
+ for (auto& newHuman : detectedHumans)
+ {
+ if (newHuman.associated || !newHuman.trackingId)
+ {
+ continue;
+ }
+ if (oldHuman.trackingId.value() == newHuman.trackingId.value())
+ {
+ associate(&oldHuman, &newHuman);
+ }
+ }
+ }
+
+
+ // associate leftover humans by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
+
+ for (auto& posDistance : sortedDistances)
+ {
+ if (posDistance.distance > parameters.maxAssociationDistance)
+ {
+ break;
+ }
+ if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
+ {
+ continue;
+ }
+ associate(posDistance.oldHuman, posDistance.newHuman);
+ }
+ }
+
+ void
+ HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
+ {
+ ARMARX_CHECK(!trackedHuman->associated);
+ ARMARX_CHECK(!detectedHuman->associated);
+
+ trackedHuman->associated = true;
+ detectedHuman->associated = true;
+
+ float dt =
+ (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
+ Eigen::Vector2f ds =
+ (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
+ Eigen::Vector2f linVelocity = ds / dt;
+
+ Eigen::Vector2f velocity =
+ parameters.velocityAlpha * linVelocity +
+ (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
+
+ trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
+ trackedHuman->trackingId = detectedHuman->trackingId;
+ }
+
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.human; }) |
+ ranges::to_vector;
+ }
+
+
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
new file mode 100644
index 0000000000000000000000000000000000000000..9146a798d208fb0c0a2e8e9925d1f1176d7a440b
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -0,0 +1,64 @@
+#pragma once
+
+
+#include <ArmarXCore/core/time.h>
+
+#include "VisionX/libraries/armem_human/types.h"
+
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ class HumanTracker
+ {
+ public:
+ HumanTracker() = default;
+
+ struct Measurements
+ {
+ DateTime detectionTime;
+ std::vector<armem::human::HumanPose> humanPoses;
+ };
+
+ struct DetectedHuman
+ {
+ core::Pose2D pose;
+ std::optional<std::string> trackingId;
+ DateTime detectionTime;
+ bool associated;
+ };
+
+ struct TrackedHuman
+ {
+ human::Human human;
+ std::optional<std::string> trackingId = std::nullopt;
+ bool associated;
+ };
+
+ struct Parameters
+ {
+ // the duration after which tracked humans will be erased if no new measurement for this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(500);
+ // the maximum distance in millimeters of two human measurements to be associated with each other
+ float maxAssociationDistance = 600;
+ // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
+ float velocityAlpha = 0.7;
+ };
+
+ void update(const Measurements& measurements);
+
+ std::vector<human::Human> getTrackedHumans() const;
+
+ void reset();
+
+ private:
+ void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ void associate(TrackedHuman* tracked, DetectedHuman* detected);
+
+ private:
+ std::vector<TrackedHuman> trackedHumans;
+ Parameters parameters;
+ };
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..76a447309ac1cbe27a3a74edb58b20c0d1cd81ab
--- /dev/null
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -0,0 +1,23 @@
+# TODO remove teb_ once the ArmarX CMake migration is complete.
+# Note: "human" is a library defined in VisionX
+
+armarx_add_aron_library(teb_human_aron
+ ARON_FILES
+ aron/Human.xml
+)
+
+armarx_add_library(teb_human
+ DEPENDENCIES_PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::conversions
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
+ SOURCES
+ types.cpp
+ aron_conversions.cpp
+ HEADERS
+ types.h
+ aron_conversions.h
+ shapes.h
+)
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
new file mode 100644
index 0000000000000000000000000000000000000000..57d104d858b8b346531c61b73069aba168ce374d
--- /dev/null
+++ b/source/armarx/navigation/human/aron/Human.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::human::arondto::Human'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='linearVelocity'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::human::arondto::HumanGroup'>
+ <ObjectChild key='shape'>
+ <List>
+ <Position />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='humans'>
+ <List>
+ <armarx::navigation::human::arondto::Human />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e09a3ca376d5f75790c8adada1038e627c968161
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::human
+{
+ void
+ toAron(arondto::Human& dto, const Human& bo)
+ {
+ dto.pose = conv::to3D(bo.pose).matrix();
+ dto.linearVelocity = conv::to3D(bo.linearVelocity);
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::Human& dto, Human& bo)
+ {
+ bo.pose = conv::to2D(core::Pose(dto.pose));
+ bo.linearVelocity = conv::to2D(dto.linearVelocity);
+ bo.detectionTime = dto.detectionTime;
+ }
+
+
+ void
+ toAron(arondto::HumanGroup& dto, const HumanGroup& bo)
+ {
+ dto.shape = bo.shape.vertices |
+ ranges::views::transform([](const Eigen::Vector2f& boVer) -> Eigen::Vector3f
+ { return conv::to3D(boVer); }) |
+ ranges::to_vector;
+
+ dto.humans = bo.humans |
+ ranges::views::transform(
+ [](const Human& boHuman) -> arondto::Human
+ {
+ arondto::Human dtoHuman;
+ toAron(dtoHuman, boHuman);
+ return dtoHuman;
+ }) |
+ ranges::to_vector;
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::HumanGroup& dto, HumanGroup& bo)
+ {
+ bo.shape.vertices =
+ dto.shape |
+ ranges::views::transform([](const Eigen::Vector3f& dtoVer) -> Eigen::Vector2f
+ { return conv::to2D(dtoVer); }) |
+ ranges::to_vector;
+
+ bo.humans = dto.humans |
+ ranges::views::transform(
+ [](const arondto::Human& dtoHuman) -> Human
+ {
+ Human boHuman;
+ fromAron(dtoHuman, boHuman);
+ return boHuman;
+ }) |
+ ranges::to_vector;
+ bo.detectionTime = dto.detectionTime;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..71159748a75ed5d44312650a530c80cc2e25e1ab
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.h
@@ -0,0 +1,46 @@
+/**
+ * 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 Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ struct Human;
+ struct HumanGroup;
+
+ namespace arondto
+ {
+ struct Human;
+ struct HumanGroup;
+
+ } // namespace arondto
+
+} // namespace armarx::navigation::human
+
+namespace armarx::navigation::human
+{
+ void toAron(arondto::Human& dto, const Human& bo);
+ void fromAron(const arondto::Human& dto, Human& bo);
+
+ void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
+ void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/shapes.h b/source/armarx/navigation/human/shapes.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6fd25a5156684648c7abf4139ebe04f27188a32
--- /dev/null
+++ b/source/armarx/navigation/human/shapes.h
@@ -0,0 +1,48 @@
+/**
+ * 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 Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human::shapes
+{
+
+ /**
+ * @brief An axis oriented ellipse with half-axes a and b along the x- and y-axis respectively.
+ */
+ struct Ellipse
+ {
+ float a;
+ float b;
+ };
+
+ /**
+ * @brief A polygon with arbitrarily many vertices. The polygon will always be closed automatically.
+ */
+ struct Polygon
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ };
+
+} // namespace armarx::navigation::human::shapes
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6296ba6e4c974706ab957fd9229291694ec7d5a9
--- /dev/null
+++ b/source/armarx/navigation/human/types.cpp
@@ -0,0 +1,14 @@
+#include "types.h"
+
+namespace armarx::navigation::human
+{
+ core::Pose2D
+ Human::estimateAt(const DateTime& time) const
+ {
+ double dt = (time - detectionTime).toSecondsDouble();
+ core::Pose2D estimation{pose};
+ estimation.translation() += linearVelocity * dt;
+ return estimation;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..b447df92e90ce26706eda63ed2d752343b49b011
--- /dev/null
+++ b/source/armarx/navigation/human/types.h
@@ -0,0 +1,59 @@
+/**
+ * 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 Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/shapes.h>
+
+namespace armarx::navigation::human
+{
+ struct Human
+ {
+ core::Pose2D pose;
+ Eigen::Vector2f linearVelocity;
+ DateTime detectionTime;
+
+ core::Pose2D estimateAt(const DateTime& time) const;
+ };
+
+ using Humans = std::vector<Human>;
+
+
+ struct HumanGroup
+ {
+ shapes::Polygon shape;
+ Humans humans;
+ DateTime detectionTime;
+ };
+
+ using HumanGroups = std::vector<HumanGroup>;
+
+
+ struct ProxemicZone
+ {
+ core::Pose2D pose;
+ shapes::Ellipse shape;
+ };
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 8f4483b1f1cc1b78bf0faa13835afe2a59c2228b..eba21c61a48965c16f4c0fe0d69a7504f6a98ef1 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -8,6 +8,8 @@ armarx_add_library(memory
client/events/Writer.cpp
client/costmap/Writer.cpp
client/costmap/Reader.cpp
+ client/human/Reader.cpp
+ client/human/Writer.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
@@ -18,6 +20,8 @@ armarx_add_library(memory
client/events/Writer.h
client/costmap/Writer.h
client/costmap/Reader.h
+ client/human/Reader.h
+ client/human/Writer.h
# ./client/events/Reader.h
DEPENDENCIES
ArmarXCoreInterfaces
@@ -27,6 +31,7 @@ armarx_add_library(memory
armarx_navigation::algorithms
armarx_navigation::graph
armarx_navigation::location
+ armarx_navigation::teb_human
)
armarx_add_test(memory_test
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e3bee9f25cf9c39cb69cbe02ee0577e62131ceae
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -0,0 +1,236 @@
+#include "Reader.h"
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::human
+{
+ Reader::~Reader() = default;
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumansQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("humans")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumanGroupsQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("groups")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.human.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::HumanCoreSegmentName};
+ }
+
+ navigation::human::Humans
+ asHumans(const armem::wm::ProviderSegment& providerSegment)
+ {
+ navigation::human::Humans humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humans](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&humans](const armem::wm::EntityInstance& entityInstance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(entityInstance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+ humans.push_back(human);
+ });
+ });
+
+ return humans;
+ }
+
+ navigation::human::HumanGroups
+ asGroups(const armem::wm::ProviderSegment& providerSegment)
+ {
+ navigation::human::HumanGroups humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humans](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&humans](const armem::wm::EntityInstance& entityInstance)
+ {
+ const auto dto =
+ navigation::human::arondto::HumanGroup::FromAron(entityInstance.data());
+
+ navigation::human::HumanGroup human;
+ fromAron(dto, human);
+ humans.push_back(human);
+ });
+ });
+
+ return humans;
+ }
+
+ Reader::HumanGroupResult
+ Reader::queryHumanGroups(const Query& query) const
+ {
+ const auto qb = buildHumansQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
+ << "` does not exist (yet).";
+ return {.groups = {}, .status = HumanGroupResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_VERBOSE << "No entities.";
+ return {.groups = {},
+ .status = HumanGroupResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanGroupResult{.groups = asGroups(providerSegment),
+ .status = HumanGroupResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanGroupResult{.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+
+ Reader::HumanResult
+ Reader::queryHumans(const Query& query) const
+ {
+ const auto qb = buildHumansQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
+ << "` does not exist (yet).";
+ return {.humans = {}, .status = HumanResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_VERBOSE << "No entities.";
+ return {
+ .humans = {}, .status = HumanResult::Status::NoData, .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanResult{.humans = asHumans(providerSegment),
+ .status = HumanResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanResult{.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e69323702944d3c58ea231ca1b7ad3514383907
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -0,0 +1,97 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::memory::client::human
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ };
+
+ struct HumanResult
+ {
+ armarx::navigation::human::Humans humans;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ struct HumanGroupResult
+ {
+ armarx::navigation::human::HumanGroups groups;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ HumanResult queryHumans(const Query& query) const;
+ HumanGroupResult queryHumanGroups(const Query& query) const;
+
+ protected:
+ ::armarx::armem::client::query::Builder buildHumansQuery(const Query& query) const;
+ ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c38c2ac775e064890eecbf12d95c55ce017fb4a8
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -0,0 +1,133 @@
+#include "Writer.h"
+
+#include <iterator>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/memory/constants.h>
+#include <armarx/navigation/human/aron_conversions.h>
+
+
+namespace armarx::navigation::memory::client::human
+{
+ Writer::~Writer() = default;
+
+ bool
+ Writer::store(const armarx::navigation::human::Humans& humans,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("humans").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(humans.begin(),
+ humans.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
+ navigation::human::arondto::Human dto;
+ toAron(dto, human);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ bool
+ Writer::store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(groups.begin(),
+ groups.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
+ navigation::human::arondto::HumanGroup dto;
+ toAron(dto, group);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.human.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::HumanCoreSegmentName};
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..b224149c4aeb3c4858530f19ed051700060fc179
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -0,0 +1,69 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::memory::client::human
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const armarx::navigation::human::Humans& humans,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ bool store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index fe57b76c0ec31fb41ee3768affbda060f09752da..2b3364a8335c4caef6fc347d43bfde87d49a9b98 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -32,5 +32,6 @@ namespace armarx::navigation::memory::constants
inline const std::string GraphCoreSegmentName = "Graph";
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
+ inline const std::string HumanCoreSegmentName = "Human";
} // namespace armarx::navigation::memory::constants