diff --git a/CMakeLists.txt b/CMakeLists.txt
index 494d9723528425a5413069441c067276bcd6efa0..a79cf08d167397002ade8cbd28a2b5440ccbcb68 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,10 @@ armarx_find_package(PUBLIC RVO QUIET)
armarx_find_package(PUBLIC teb_local_planner QUIET)
armarx_find_package(PUBLIC teb_extension QUIET)
+# laser scanner feature extraction
+armarx_find_package(PUBLIC RobotComponents QUIET)
+armarx_find_package(PUBLIC PCL QUIET COMPONENTS io common)
+
add_subdirectory(etc)
# FetchContent_Declare(
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 67fe56d7d947b1dc69af3c3610fef86db1aaae17..6ff029f563010d24f639ae209fc40351705d6493 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,12 +1,12 @@
{
"robot_footprint_radius": 0.5,
- "planning_distance": 3,
+ "planning_distance": 5,
"teb_config": {
"pse": {
"pse_costum_obstacle_penalties": true,
"pse_costum_obstacle_penalties_dynamic": true,
- "weight_costmap": 1,
+ "weight_costmap": 0,
"weight_global_path_position": 0.3,
"weight_global_path_orientation": 0.3,
"lrk_use_alternative_approach": false,
@@ -26,20 +26,20 @@
"exact_arc_length": false,
"force_reinit_new_goal_dist": 1,
"force_reinit_new_goal_angular": 1.5707963267948966,
- "feasibility_check_no_poses": 5,
+ "feasibility_check_no_poses": -1,
"feasibility_check_lookahead_distance": -1,
"min_resolution_collision_check_angular": 3.141592653589793
},
"robot": {
- "max_vel_x": 0.5,
- "max_vel_x_backwards": 0.5,
- "max_vel_y": 0.5,
- "max_vel_trans": 0.5,
- "max_vel_theta": 0.5,
- "acc_lim_x": 0.5,
- "acc_lim_y": 0.5,
- "acc_lim_theta": 0.5,
+ "max_vel_x": 0.3,
+ "max_vel_x_backwards": 0.3,
+ "max_vel_y": 0.3,
+ "max_vel_trans": 0.3,
+ "max_vel_theta": 0.3,
+ "acc_lim_x": 0.3,
+ "acc_lim_y": 0.3,
+ "acc_lim_theta": 0.3,
"min_turning_radius": 0
},
@@ -74,8 +74,8 @@
"weight_kinematics_nh": 1,
"weight_kinematics_forward_drive": 1,
"weight_kinematics_turning_radius": 1,
- "weight_optimaltime": 1,
- "weight_shortest_path": 1,
+ "weight_optimaltime": 0.5,
+ "weight_shortest_path": 0.5,
"weight_obstacle": 50,
"weight_inflation": 0.1,
"weight_dynamic_obstacle": 50,
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 5bce30456bff46714c9d10d30f69ec792f322e42..15bced3c9328a1a2c7550f72079386ccdfb041a9 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -13,5 +13,8 @@
<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="laser_scanner_feature_extraction" 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"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/HumanMemoryApp.cfg b/scenarios/PlatformNavigation/config/HumanMemoryApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..28099ebc83514ac57b5af9e5acba37e72a6f3846
--- /dev/null
+++ b/scenarios/PlatformNavigation/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/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
index 81d7cebf824840bbff1c4d28da4cebcfabc95977..48635a5e84d486b2fa2ddbf520ed4229c3c8c2af 100644
--- a/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -226,20 +226,20 @@
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment: Name of the mapping memory core segment to use.
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.CoreSegment:
# Attributes:
# - Default: LaserScannerFeatures
# - Case sensitivity: yes
# - Required: no
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment = LaserScannerFeatures
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName:
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.Memory:
# Attributes:
-# - Default: Vision
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName = Vision
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.Memory = Navigation
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg b/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8b2592fd6f0220e70e263489cae101f67179b7af
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg
@@ -0,0 +1,439 @@
+# ==================================================================
+# dynamic_scene_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.dynamic_scene_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.dynamic_scene_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.dynamic_scene_provider.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.EnableProfiling = false
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.dynamic_scene_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.dynamic_scene_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectName = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment:
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment = Pose
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# 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.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Provider = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment:
+# Attributes:
+# - Default: OccupancyGrid
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory = Vision
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.dynamic_scene_provider.p.humanPoses.enabled: Whether human poses are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.dynamic_scene_provider.p.humanPoses.enabled = true
+
+
+# ArmarX.dynamic_scene_provider.p.humanPoses.providerName:
+# Attributes:
+# - Default: AzureKinectPointCloudProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.humanPoses.providerName = AzureKinectPointCloudProvider
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.enabled: Whether laser scanner features are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.enabled = true
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: global
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name = global
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold:
+# Attributes:
+# - Default: 0.449999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold = 0.449999988
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name = ""
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold:
+# Attributes:
+# - Default: 0.550000012
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold = 0.550000012
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName:
+# Attributes:
+# - Default: CartographerMappingAndLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName = CartographerMappingAndLocalization
+
+
+# ArmarX.dynamic_scene_provider.p.robot.name:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.dynamic_scene_provider.p.robot.name = ArmarDE
+
+
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs: Update rate of the running task.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs = 100
+
+
diff --git a/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg b/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..9c77a56d13d6ddd4d0e9ad2bdf5a4442d92a4ed9
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg
@@ -0,0 +1,395 @@
+# ==================================================================
+# laser_scanner_feature_extraction 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.LaserScannerFeatureExtraction.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.LaserScannerFeatureExtraction.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.LaserScannerFeatureExtraction.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.EnableProfiling = false
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.LaserScannerFeatureExtraction.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ObjectName = ""
+
+
+# ArmarX.LaserScannerFeatureExtraction.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Memory = Navigation
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Provider = ""
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.drawRawPoints: If true, the laser scans will be drawn.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.drawRawPoints = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeMergedFeatures: If true, the merged features from all sensors will be drawn.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeMergedFeatures = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeSeparateFeatures: If true, the features from each sensor will be drawn.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeSeparateFeatures = false
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.cableAreaWidth: Width of the area where to search for the cable.
+# Attributes:
+# - Default: 400
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.cableAreaWidth = 400
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.enabled: Try to supress clusters belonging to the power supply cable.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.enabled = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.maxAreaTh: The cable will only be removed if the cluster area is below this threshold.
+# Attributes:
+# - Default: 2500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.maxAreaTh = 2500
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.distanceTh:
+# Attributes:
+# - Default: 40
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.distanceTh = 40
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.maxIterations:
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.maxIterations = -1
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.radius: The radius of the robot when using the circle shape.
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.radius = 500
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.robotConvexHullMargin: Parameter to increase the robot's convex hull when using the rectangle shape.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.LaserScannerFeatureExtraction.p.robotHull.robotConvexHullMargin = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.shape: Shape of the robot area.
+# Attributes:
+# - Default: Rectangle
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Circle, Rectangle}
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.shape = Rectangle
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.angleThreshold: Angular distance between consecutive points in laser scan for clustering.
+# Attributes:
+# - Default: 0.0314159282
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.angleThreshold = 0.0314159282
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.distanceThreshold: Radial distance between consecutive points in laser scan for clustering.
+# Attributes:
+# - Default: 30
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.LaserScannerFeatureExtraction.p.scanClustering.distanceThreshold = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.maxDistance: Maximum radius around sensors to detect clusters.
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.maxDistance = 3000
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.taskPeriodMs: Update rate of the running task.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.taskPeriodMs = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.tpc.pub.LaserScannerFeatures: Name of the `LaserScannerFeatures` topic to publish data to.
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.tpc.pub.LaserScannerFeatures = LaserScannerFeatures
+
+
+# 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/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 2d35b33823bf62434321f6649cf6f2185ab1959e..37185c4b371621ce86949c611502121a45c1ad46 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -302,7 +302,15 @@ ArmarX.navigation_memory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-grap
# - Default: 2
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigation_memory.p.visuFrequency = 2
+ArmarX.navigation_memory.p.visuFrequency = 5
+
+
+# ArmarX.navigation_memory.p.visuHumanMaxAgeMs: The maximum age of humans to be drawn in ms.
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.p.visuHumanMaxAgeMs = 1000
# ArmarX.navigation_memory.p.visuHumans: Enable visualization of humans.
@@ -330,5 +338,3 @@ ArmarX.navigation_memory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-grap
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.navigation_memory.p.visuTransparent = false
-
-
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 0bacc0c34adcb6a461452a27b2ad788a15963837..0496b5dcc39a9c7f7293bc48974c16a6492bbba4 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -181,7 +181,7 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
# ArmarX.navigator.ArVizStorageName: Name of the ArViz storage
@@ -329,6 +329,22 @@ ArmarX.navigator.RobotUnitName = ArmarDEUnit
# ArmarX.navigator.mem.nav.human.Memory = Navigation
+# ArmarX.navigator.mem.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.navigator.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.laser_scanner_features.Memory = Navigation
+
+
# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
# - Default: Parameterization
@@ -436,6 +452,14 @@ ArmarX.navigator.RobotUnitName = ArmarDEUnit
# ArmarX.navigator.p.scene.humanProviderName = dynamic_scene_provider
+# ArmarX.navigator.p.scene.laserScannerFeaturesProviderName:
+# Attributes:
+# - Default: dynamic_scene_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.laserScannerFeaturesProviderName = dynamic_scene_provider
+
+
# ArmarX.navigator.p.scene.robotName:
# Attributes:
# - Case sensitivity: yes
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index efa6ec80e5a10ff6f1a4667c6c4792b29ef71f7f..e65951f6cdada3248f25aea846787eed6840de31 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -21,3 +21,5 @@ add_subdirectory(dynamic_scene_provider)
add_subdirectory(human_simulator)
add_subdirectory(navigation_skill_provider)
+
+add_subdirectory(laser_scanner_feature_extraction)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index ab0d58102f6d3514940cabac0de4e8bea7d22f4f..5bab4b0deb3d568fde424ff1c32b6dc0bc47fdc2 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -35,13 +35,12 @@
#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
-#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
-#include <armarx/navigation/util/geometry.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/util/geometry.h>
// Include headers you only need in function definitions in the .cpp.
@@ -55,7 +54,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
const std::string Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
-
armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
@@ -97,7 +95,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
return def;
}
-
Component::Component() :
robotReader(memoryNameSystem()),
costmapReader(memoryNameSystem()),
@@ -116,7 +113,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// setDebugObserverBatchModeEnabled(true);
}
-
void
Component::onConnectComponent()
{
@@ -191,11 +187,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// return false;
}
-
-
-
void
- fillCostmap(const std::vector<armem::vision::LaserScannerFeatures>& features,
+ fillCostmap(const std::vector<memory::LaserScannerFeatures>& features,
algorithms::Costmap& costmap)
{
@@ -257,7 +250,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
}
-
void
Component::drawCostmap(const armarx::navigation::algorithms::Costmap& costmap,
const std::string& name,
@@ -325,7 +317,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
const auto timestamp = armarx::core::time::Clock::Now();
- const armem::vision::laser_scanner_features::client::Reader::Query query{
+ const memory::client::laser_scanner_features::Reader::Query query{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
.timestamp = timestamp};
@@ -405,33 +397,28 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
return navigationPlanningCostmap;
}
-
void
Component::onDisconnectComponent()
{
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
return Component::defaultName;
}
-
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 1837d8ac9cf76718fef5978213a5786235a50a08..92179ba82dc43eb2f989f8ea6cc92bae11307b4c 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -23,19 +23,18 @@
#pragma once
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
-#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/costmap/Writer.h>
-#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
-
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
{
@@ -114,7 +113,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
static const std::string defaultName;
-
// Private member variables go here.
@@ -141,6 +139,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
int updatePeriodMs = 100;
};
+
Properties properties;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
@@ -172,7 +171,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
memory::client::costmap::Reader costmapReader;
memory::client::costmap::Writer costmapWriter;
- armem::vision::laser_scanner_features::client::Reader laserScannerFeaturesReader;
+ memory::client::laser_scanner_features::Reader laserScannerFeaturesReader;
std::optional<algorithms::Costmap> staticCostmap;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 52e4efdccb9d044cc6262f8953838d00e53b3043..7cc6fb586fdf6cd42625b5daf80bcc622bcdef89 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -15,7 +15,7 @@ armarx_add_component(dynamic_scene_provider
DEPENDENCIES
# ArmarXCore
ArmarXCore
- ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 77d1fd7f77f556efc19e553500ab14cd57d609c3..a5302a5070a26fd15c629df0e8e5fa182761e113 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/time.h>
@@ -40,13 +41,11 @@
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
const std::string Component::defaultName = "dynamic_scene_provider";
-
Component::Component()
{
addPlugin(humanPoseReaderPlugin);
@@ -55,6 +54,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
addPlugin(humanWriterPlugin);
+ addPlugin(laserScannerFeaturesWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -80,12 +80,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(
properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+ def->optional(properties.laserScannerFeatures.enabled,
+ "p.laserScannerFeatures.enabled",
+ "Whether laser scanner features are used.");
def->optional(properties.laserScannerFeatures.providerName,
"p.laserScannerFeatures.providerName",
"");
def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
- def->optional(properties.robot.name, "p.robot.name", "");
+ def->required(properties.robot.name, "p.robot.name", "");
def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
@@ -94,12 +97,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(
properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
- def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
+ def->optional(
+ properties.humanPoses.enabled, "p.humanPoses.enabled", "Whether human poses are used.");
+ def->optional(properties.humanPoses.providerName, "p.humanPoses.providerName", "");
return def;
}
-
void
Component::onInitComponent()
{
@@ -107,10 +111,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
+ setDebugObserverBatchModeEnabled(true);
}
-
void
Component::onConnectComponent()
{
@@ -162,27 +165,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
task->start();
}
-
void
Component::onDisconnectComponent()
{
task->stop();
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
@@ -192,76 +191,92 @@ namespace armarx::navigation::components::dynamic_scene_provider
void
Component::runPeriodically()
{
- TIMING_START(FULL_DYNAMIC_SCENE_PROVIDER);
+ const auto logDuration = [this](const std::string& name, const Duration& duration)
+ { setDebugObserverDatafield("timing." + name + " [ms]", duration.toMilliSeconds()); };
- // obtain data from perception
+ const auto makeSWlog = [&](const std::string& name)
+ { return [=](const Duration& duration) { logDuration(name, duration); }; };
- const DateTime timestamp = Clock::Now();
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.full"));
+ // obtain data from perception
+ const DateTime timestamp = Clock::Now();
//
// Robot
//
+ ARMARX_TRACE;
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_robot"));
- TIMING_START(READ_ROBOT_FROM_MEMORY);
-
- ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
- const core::Pose global_T_robot(robot->getGlobalPose());
-
- ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
-
- TIMING_END_COMMENT_STREAM(
- READ_ROBOT_FROM_MEMORY, "Timer: reading robot from memory", ARMARX_VERBOSE);
+ ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+ const core::Pose global_T_robot(robot->getGlobalPose());
+ ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
+ }
//
// Human
//
+ ARMARX_TRACE;
+ const std::vector<armem::human::HumanPose> humanPoses = [&]
+ {
+ if (!properties.humanPoses.enabled)
+ {
+ return std::vector<armem::human::HumanPose>{};
+ }
- TIMING_START(READ_HUMANS_FROM_MEMORY);
-
- ARMARX_VERBOSE << "Querying humans";
-
- const armem::human::client::Reader::Query humanPoseQuery{
- .providerName = properties.humanPoseProvider,
- .timestamp = timestamp,
- .maxAge = Duration::MilliSeconds(500)};
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_human"));
- const armem::human::client::Reader::Result humanPoseResult =
- humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_NOT_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Error)
- << humanPoseResult.errorMessage;
+ ARMARX_VERBOSE << "Querying humans";
+ const armem::human::client::Reader::Query humanPoseQuery{
+ .providerName = properties.humanPoses.providerName,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
- ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
+ const armem::human::client::Reader::Result humanPoseResult =
+ humanPoseReaderPlugin->get().query(humanPoseQuery);
+ ARMARX_CHECK_NOT_EQUAL(humanPoseResult.status,
+ armem::human::client::Reader::Result::Error)
+ << humanPoseResult.errorMessage;
- TIMING_END_COMMENT_STREAM(
- READ_HUMANS_FROM_MEMORY, "Timer: reading humans from memory", ARMARX_VERBOSE);
+ ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
+ return humanPoseResult.humanPoses;
+ }();
//
// Laser scanner features
//
+ ARMARX_TRACE;
+ const memory::LaserScannerFeatures laserFeatures = [&]
+ {
+ if (!properties.laserScannerFeatures.enabled)
+ {
+ return memory::LaserScannerFeatures{};
+ }
- /*
- TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
- ARMARX_INFO << "Querying laser scanner features";
- const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
- .providerName = properties.laserScannerFeatures.providerName,
- .name = properties.laserScannerFeatures.name,
- .timestamp = timestamp};
+ ARMARX_VERBOSE << "Querying laser scanner features";
+ const memory::client::laser_scanner_features::Reader::Query laserFeaturesQuery{
+ .providerName = properties.laserScannerFeatures.providerName,
+ .name = properties.laserScannerFeatures.name,
+ .timestamp = timestamp};
- const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
- laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ const memory::client::laser_scanner_features::Reader::Result laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+ memory::client::laser_scanner_features::Reader::Result::Success)
+ << laserFeaturesResult.errorMessage;
- ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success);
+ ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
- ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.features.size(), 1)
+ << "We request the merged instance with only one result";
- TIMING_END_COMMENT_STREAM(
- READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
- */
+ return laserFeaturesResult.features.front();
+ }();
/* we don't need this at the moment
@@ -370,49 +385,44 @@ namespace armarx::navigation::components::dynamic_scene_provider
// arviz.commit({layer});
}
+ */
// here ends: data fetching
- */
-
// process data from perception and write information back to memory
//
// Human tracking
//
+ ARMARX_TRACE;
+ if (properties.humanPoses.enabled)
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
- TIMING_START(RUNNING_HUMAN_TRACKER_WITH_CAMERA);
-
- ARMARX_VERBOSE << "Running human tracker with camera measurements";
+ ARMARX_VERBOSE << "Running human tracker with camera measurements";
+ humanTracker.update(human::HumanTracker::CameraMeasurement{.detectionTime = timestamp,
+ .humanPoses = humanPoses});
+ }
- humanTracker.update(human::HumanTracker::CameraMeasurement{
- .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
- TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_CAMERA,
- "Timer: running human tracker with camera",
- ARMARX_VERBOSE);
+ ARMARX_TRACE;
+ const std::vector<memory::LaserScannerFeature> unusedFeatures = [&]
+ {
+ if (!properties.laserScannerFeatures.enabled)
+ {
+ return std::vector<memory::LaserScannerFeature>{};
+ }
+ armarx::core::time::ScopedStopWatch sw(
+ makeSWlog("dynamic_scene.human_tracker.laserscanner"));
- /*
- ARMARX_INFO << "Running human tracker with lasersensor measurements";
+ ARMARX_VERBOSE << "Running human tracker with lasersensor measurements";
- TIMING_START(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER);
+ return humanTracker.update(human::HumanTracker::LaserMeasurement{
+ .detectionTime = timestamp, .clusters = laserFeatures.features});
+ }();
- //TODO why is result a vector of LSFs and not a vector of LSF?
- std::vector<armem::vision::LaserScannerFeature> flattened;
- for (auto const& fs : laserFeaturesResult.features)
- {
- flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
- }
- std::vector<armem::vision::LaserScannerFeature> clusters =
- humanTracker.update(human::HumanTracker::LaserMeasurement{.detectionTime = timestamp,
- .clusters = flattened});
-
- TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER,
- "Timer: running human tracker with laserscanner",
- ARMARX_VERBOSE);
- */
ARMARX_VERBOSE << "Human tracking done";
@@ -420,30 +430,36 @@ namespace armarx::navigation::components::dynamic_scene_provider
//
// Write dynamic scene back to memory
//
-
- TIMING_START(WRITE_BACK_HUMANS);
-
- //TODO use clusters for obstacle creation
-
- std::vector<human::Human> humans = humanTracker.getTrackedHumans();
-
-
- if(not humans.empty())
+ ARMARX_TRACE;
{
- ARMARX_INFO << "Detected " << humans.size() << " humans";
- humanWriterPlugin->get().store(humans, getName(), timestamp);
- }
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.write_back_human"));
+ //TODO use clusters for obstacle creation
+ std::vector<human::Human> humans = humanTracker.getTrackedHumans();
- TIMING_END_COMMENT_STREAM(
- WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
+ if (not humans.empty())
+ {
+ ARMARX_VERBOSE << "Detected " << humans.size() << " humans";
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
+ }
+ if (not unusedFeatures.empty())
+ {
+ ARMARX_VERBOSE << "Detected " << unusedFeatures.size()
+ << " laser scanner features not associated with humans";
+ //TODO(groeger): check frame, do we need it?
+ laserScannerFeaturesWriterPlugin->get().store(
+ memory::LaserScannerFeatures{.frame = "global",
+ .frameGlobalPose = Eigen::Isometry3f::Identity(),
+ .features = unusedFeatures},
+ getName(),
+ timestamp);
+ }
+ }
- TIMING_END_COMMENT_STREAM(
- FULL_DYNAMIC_SCENE_PROVIDER, "Timer: complete dynamic scene provider", ARMARX_VERBOSE);
+ sendDebugObserverBatch();
}
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
Component::createRemoteGuiTab()
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 11e0d65404a2c24d6179fe6327ee3e7b32fcc5c6..5c36522d1b0fdb069d84241fe61cf3c44a82c30b 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -30,15 +30,15 @@
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
-#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -47,11 +47,11 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
+#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
#include <armarx/navigation/human/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>
-
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -59,7 +59,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
class Component :
virtual public armarx::Component,
virtual public armarx::navigation::components::dynamic_scene_provider::ComponentInterface,
- // , virtual public armarx::DebugObserverComponentPluginUser
+ virtual public armarx::DebugObserverComponentPluginUser,
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
virtual public armarx::ArVizComponentPluginUser,
virtual public armarx::armem::client::plugins::PluginUser,
@@ -123,7 +123,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
static const std::string defaultName;
-
// Private member variables go here.
@@ -134,14 +133,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct
{
+ bool enabled = true;
std::string providerName = "LaserScannerFeatureExtraction";
- std::string name = ""; // all
+ std::string name = "global"; // the merged entity of all sensors
} laserScannerFeatures;
-
struct
{
- std::string name = "Armar6";
+ std::string name;
} robot;
struct
@@ -153,8 +152,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
float occupiedThreshold = 0.55;
} occupancyGrid;
- std::string humanPoseProvider = "AzureKinectPointCloudProvider";
+ struct
+ {
+ bool enabled = true;
+ std::string providerName = "AzureKinectPointCloudProvider";
+ } humanPoses;
};
+
Properties properties;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
@@ -191,7 +195,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::human::client::Reader>* humanPoseReaderPlugin = nullptr;
- ReaderWriterPlugin<armem::vision::laser_scanner_features::client::Reader>*
+ ReaderWriterPlugin<memory::client::laser_scanner_features::Reader>*
laserScannerFeaturesReaderPlugin = nullptr;
ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
@@ -203,6 +207,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
occupancyGridReaderPlugin = nullptr;
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
+ laserScannerFeaturesWriterPlugin = nullptr;
human::HumanTracker humanTracker;
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef07b9caa91d017e9de510c8f86112583b451913
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -0,0 +1,237 @@
+#include "ArVizDrawer.h"
+
+#include <iterator>
+#include <string>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/impl/point_types.hpp>
+#include <pcl/point_cloud.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/Line.h"
+#include "RobotAPI/components/ArViz/Client/elements/Path.h"
+#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
+#include "RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h"
+
+#include "RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h"
+
+#include "FeatureExtractor.h"
+#include "conversions/eigen.h"
+#include "conversions/pcl.h"
+#include "conversions/pcl_eigen.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ void
+ ArVizDrawer::draw(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ drawCircles(features, frame, globalSensorPose);
+ drawConvexHulls(features, frame, globalSensorPose);
+ drawEllipsoids(features, frame, globalSensorPose);
+ drawChains(features, frame, globalSensorPose);
+ }
+
+ void
+ ArVizDrawer::draw(const armem::laser_scans::LaserScanStamped& msg,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer("points_" + msg.header.frame);
+
+ const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.data));
+
+ layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
+ .pointCloud(pointCloud, viz::Color(color))
+ .pointSizeInPixels(5)
+ .pose(globalSensorPose));
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::draw(const std::string& layerName,
+ const Circle& circle,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer(layerName);
+
+ drawCircle(layer, circle, robotGlobalPose, color);
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawCircle(viz::Layer& layer,
+ const Circle& circle,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+
+ const Eigen::Vector3f position =
+ globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), 10.F);
+
+ layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
+ .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
+ .position(position)
+ .color(color));
+ }
+
+ void
+ ArVizDrawer::drawCircles(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("circles_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features& f)
+ {
+ if (not f.circle)
+ {
+ return;
+ }
+ drawCircle(
+ layer, *f.circle, globalSensorPose, simox::Color::red(200, 100));
+ });
+
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("convex_hulls_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features& f)
+ {
+ if (not f.convexHull)
+ {
+ return;
+ }
+ drawConvexHull(
+ layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
+ });
+
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::draw(const std::string& layerName,
+ const VirtualRobot::MathTools::ConvexHull2D& robotHull,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer(layerName);
+
+ drawConvexHull(layer, robotHull, robotGlobalPose, color);
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawConvexHull(viz::Layer& layer,
+ const VirtualRobot::MathTools::ConvexHull2D& hull,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+ const auto points = conversions::to3D(hull.vertices);
+
+ layer.add(viz::Polygon("convex_hull_" + std::to_string(layer.size()))
+ .points(points)
+ .color(color)
+ .pose(globalSensorPose));
+ }
+
+ void
+ ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("ellipsoids_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features& f)
+ {
+ if (not f.ellipsoid)
+ {
+ return;
+ }
+ drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
+ });
+
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawEllipsoid(viz::Layer& layer,
+ const Ellipsoid& ellipsoid,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+
+ const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
+
+ layer.add(viz::Ellipsoid("ellipsoid_" + std::to_string(layer.size()))
+ .axisLengths(conversions::to3D(ellipsoid.radii))
+ .pose(pose)
+ .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
+ }
+
+ void
+ ArVizDrawer::drawChains(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("chains_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features& f)
+ {
+ if (not f.chain)
+ {
+ return;
+ }
+ drawChain(layer, *f.chain, globalSensorPose);
+
+ // const auto ellipsoids = f.linesAsEllipsoids(50);
+ // for (const auto& ellipsoid : ellipsoids)
+ // {
+ // drawEllipsoid(layer, ellipsoid, globalSensorPose);
+ // }
+ });
+
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawChain(viz::Layer& layer,
+ const Points& chain,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ if (chain.size() < 2)
+ {
+ return;
+ }
+
+ const auto cloud = conversions::to3D(chain);
+
+ layer.add(viz::Path("chain_" + std::to_string(layer.size()))
+ .points(cloud)
+ .width(7.F)
+ .color(viz::Color::blue())
+ .pose(globalSensorPose));
+ }
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5668778ea2eb0151612f4b52dd69909c0945c2e
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -0,0 +1,98 @@
+/*
+ * 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 <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ class ArVizDrawer
+ {
+ public:
+ using Points = std::vector<Eigen::Vector2f>;
+
+ ArVizDrawer(armarx::viz::Client& arviz) : arviz(arviz)
+ {
+ }
+
+ void draw(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void draw(const armem::laser_scans::LaserScanStamped& msg,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void draw(const std::string& layerName,
+ const Circle& circle,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color);
+
+ void draw(const std::string& layerName,
+ const VirtualRobot::MathTools::ConvexHull2D& robotHull,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color = simox::Color::red(100, 80));
+
+ private:
+ void drawCircles(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+ void drawCircle(viz::Layer& layer,
+ const Circle& circle,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void drawConvexHulls(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+ void drawConvexHull(viz::Layer& layer,
+ const VirtualRobot::MathTools::ConvexHull2D& hull,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void drawEllipsoids(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void drawEllipsoid(viz::Layer& layer,
+ const Ellipsoid& ellipsoid,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void drawChains(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+ void drawChain(viz::Layer& layer,
+ const Points& chain,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ armarx::viz::Client arviz;
+ };
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7148135ec31b777b44005b4c1e8c3f067a2f4c67
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -0,0 +1,56 @@
+armarx_add_component(laser_scanner_feature_extraction
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ SOURCES
+ Component.cpp
+ FeatureExtractor.cpp
+ FeatureMerger.cpp
+ ArVizDrawer.cpp
+ ScanClustering.cpp
+ ChainApproximation.cpp
+ #conversions
+ conversions/eigen.cpp
+ conversions/pcl.cpp
+ conversions/features.cpp
+ EnclosingEllipsoid.cpp
+ Path.cpp
+ UnionFind.cpp
+ HEADERS
+ Component.h
+ FeatureExtractor.h
+ FeatureMerger.h
+ ArVizDrawer.h
+ ScanClustering.h
+ ChainApproximation.h
+ EnclosingEllipsoid.h
+ Path.h
+ geometry.h
+ #conversions
+ conversions/eigen.h
+ conversions/pcl_eigen.h
+ conversions/opencv_eigen.h
+ conversions/opencv_pcl.h
+ conversions/pcl.h
+ conversions/features.h
+ UnionFind.h
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ ArmarXCoreLogging
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotComponentsInterfaces # For legacy LaserScannerFeatureExtraction topic
+ armarx_navigation::memory
+ armem_laser_scans
+ range-v3::range-v3
+ DEPENDENCIES_LEGACY
+ PCL
+)
+
+add_subdirectory(test)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..905cf765c2c9f9c7c207f270c02b3d27b511992b
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp
@@ -0,0 +1,214 @@
+#include "ChainApproximation.h"
+
+#include <iterator>
+#include <numeric>
+
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx
+{
+
+ ChainApproximation::ChainApproximation(const Points& points, const Params& params) :
+ points(points), params(params)
+ {
+ // fill indices
+ indices.resize(points.size());
+ std::iota(indices.begin(), indices.end(), 0);
+ }
+
+ ChainApproximation::ApproximationResult ChainApproximation::approximate()
+ {
+
+ int iterations = 0;
+
+ const auto maxIterConditionReached = [&]()
+ {
+ // inactive?
+ if (params.maxIterations <= 0)
+ {
+ return false;
+ }
+
+ return iterations >= params.maxIterations;
+ };
+
+ while (true)
+ {
+ if (maxIterConditionReached())
+ {
+ return ApproximationResult
+ {
+ .condition = ApproximationResult::TerminationCondition::IterationLimit,
+ .iterations = iterations};
+ }
+
+ if (not approximateStep())
+ {
+ return ApproximationResult
+ {
+ .condition = ApproximationResult::TerminationCondition::Converged,
+ .iterations = iterations,
+ .reductionFactor = 1.F - static_cast<float>(indices.size()) /
+ static_cast<float>(points.size())};
+ }
+
+ iterations++;
+ }
+ }
+
+ ChainApproximation::Triplets ChainApproximation::getTriplets() const
+ {
+ const int nIndices = static_cast<int>(indices.size());
+
+ if (nIndices < 3)
+ {
+ return {};
+ }
+
+ Triplets triplets;
+ triplets.reserve(indices.size());
+
+ // Here, we iterate over all elements under consideration.
+ // The aim is to create a view - a sliding window - over the
+ // indices. i will always point to the centered element.
+
+ // the first element
+ triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+ // intermediate elements
+ for (int i = 1; i < (nIndices - 1); i++)
+ {
+ triplets.emplace_back(indices.at(i - 1), indices.at(i), indices.at(i + 1));
+ }
+
+ // the last element
+ triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+ return triplets;
+ }
+
+ std::vector<float>
+ ChainApproximation::computeDistances(const ChainApproximation::Triplets& triplets)
+ {
+ std::vector<float> distances;
+ distances.reserve(triplets.size());
+
+ std::transform(triplets.begin(),
+ triplets.end(),
+ std::back_inserter(distances),
+ [&](const auto & triplet)
+ {
+ return computeDistance(triplet);
+ });
+
+ return distances;
+ }
+ float ChainApproximation::computeDistance(const ChainApproximation::Triplet& triplet) const
+ {
+ using Line = Eigen::ParametrizedLine<float, 2>;
+
+ const Eigen::Vector2f& ptBefore = points.at(triplet.a);
+ const Eigen::Vector2f& ptPivot = points.at(triplet.b);
+ const Eigen::Vector2f& ptAfter = points.at(triplet.c);
+
+ const auto line = Line::Through(ptBefore, ptAfter);
+ return line.distance(ptPivot);
+ }
+
+ bool ChainApproximation::approximateStep()
+ {
+ const size_t nIndices = indices.size();
+ if (nIndices <= 3)
+ {
+ return false;
+ }
+
+ const Triplets triplets = getTriplets();
+ const std::vector<float> distances = computeDistances(triplets);
+
+ ARMARX_CHECK_EQUAL(triplets.size(), distances.size());
+ const int n = static_cast<int>(triplets.size());
+
+ std::vector<int> indicesToBeRemoved;
+
+ // TODO(fabian.reister): consider boundaries
+ for (int i = 1; i < n - 1; i++)
+ {
+ const auto& distance = distances.at(i);
+
+ // check distance criterion (necessary conditio)
+ if (distance >= params.distanceTh)
+ {
+ continue;
+ }
+
+ // better remove this element than those left and right (sufficient condition)
+ if (distance < std::min(distances.at(i - 1), distances.at(i + 1)))
+ {
+ indicesToBeRemoved.emplace_back(triplets.at(i).b);
+ }
+ }
+
+ // termination condition
+ if (indicesToBeRemoved.empty())
+ {
+ return false;
+ }
+
+ const auto isMatch = [&](const int& idx) -> bool
+ {
+ return std::find(indicesToBeRemoved.begin(), indicesToBeRemoved.end(), idx) !=
+ indicesToBeRemoved.end();
+ };
+
+ indices.erase(std::remove_if(indices.begin(), indices.end(), isMatch), indices.end());
+
+ return true;
+ }
+ ChainApproximation::Points ChainApproximation::approximatedChain() const
+ {
+ Points extractedPoints;
+ extractedPoints.reserve(indices.size());
+
+ std::transform(indices.begin(),
+ indices.end(),
+ std::back_inserter(extractedPoints),
+ [&](const auto & idx)
+ {
+ return points.at(idx);
+ });
+
+ return extractedPoints;
+ }
+
+ std::ostream& detail::operator<<(std::ostream& str, const ApproximationResult& res)
+ {
+ using TerminationCondition = ApproximationResult::TerminationCondition;
+
+ const std::string condStr = [&res]() -> std::string
+ {
+ std::string repr;
+
+ switch (res.condition)
+ {
+ case TerminationCondition::Converged:
+ repr = "Converged";
+ break;
+ case TerminationCondition::IterationLimit:
+ repr = "IterationLimit";
+ break;
+ }
+ return repr;
+ }();
+
+ str << "ApproximationResult: ["
+ << "condition: " << condStr << " | "
+ << "iterations: " << res.iterations << " | "
+ << "reduction: " << res.reductionFactor * 100 << "%]";
+
+ return str;
+ }
+} // namespace armarx
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7b5e6cdaeb46a8909f4f572863e08c92751a649
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.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 <Eigen/Core>
+
+namespace armarx
+{
+
+ namespace detail
+ {
+ struct ChainApproximationParams
+ {
+ float distanceTh; // [mm]
+
+ int maxIterations{-1};
+ };
+
+
+ struct ApproximationResult
+ {
+ enum class TerminationCondition
+ {
+ Converged,
+ IterationLimit
+ };
+ TerminationCondition condition;
+
+ int iterations;
+
+ float reductionFactor{0.F};
+ };
+
+ std::ostream& operator<<(std::ostream& str, const ApproximationResult& res);
+ } // namespace detail
+
+ class ChainApproximation
+ {
+ public:
+ using Point = Eigen::Vector2f;
+ using Points = std::vector<Point>;
+
+ using Params = detail::ChainApproximationParams;
+ using ApproximationResult = detail::ApproximationResult;
+
+ ChainApproximation(const Points& points, const Params& params);
+
+ ApproximationResult approximate();
+
+ [[nodiscard]] Points approximatedChain() const;
+
+ private:
+ struct Triplet
+ {
+ Triplet(const int& a, const int& b, const int& c) : a(a), b(b), c(c) {}
+
+ int a;
+ int b;
+ int c;
+ };
+
+ using Triplets = std::vector<Triplet>;
+
+ Triplets getTriplets() const;
+
+ std::vector<float> computeDistances(const Triplets& triplets);
+ float computeDistance(const Triplet& triplet) const;
+
+ bool approximateStep();
+
+ Points points;
+
+ std::vector<int> indices;
+
+ const Params params;
+ };
+
+} // namespace armarx
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa5ef3315f48fdb684d9e5c786f929ca65ae37aa
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -0,0 +1,649 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
+ */
+
+#include "Component.h"
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <utility>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/BoundingBox.h>
+#include <VirtualRobot/MathTools.h>
+
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
+
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h>
+
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+#include "conversions/eigen.h"
+#include "conversions/pcl_eigen.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ const std::string LaserScannerFeatureExtraction::defaultName = "LaserScannerFeatureExtraction";
+
+ armarx::PropertyDefinitionsPtr
+ LaserScannerFeatureExtraction::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ def->topic(featuresTopic);
+
+ // // Add an optionalproperty.
+ def->optional(
+ properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+
+ def->optional(properties.robotHull.shape, "p.robotHull.shape", "Shape of the robot area.")
+ .map({std::make_pair("Rectangle", Properties::RobotHull::RECTANGLE),
+ std::make_pair("Circle", Properties::RobotHull::CIRCLE)});
+ def->optional(properties.robotHull.radius,
+ "p.robotHull.radius",
+ "The radius of the robot when using the circle shape.");
+ def->optional(
+ properties.robotHull.robotConvexHullMargin,
+ "p.robotHull.robotConvexHullMargin",
+ "Parameter to increase the robot's convex hull when using the rectangle shape.");
+
+ def->optional(properties.cableFix.enabled,
+ "p.cableFix.enabled",
+ "Try to supress clusters belonging to the power supply cable.");
+ def->optional(properties.cableFix.cableAreaWidth,
+ "p.cableFix.cableAreaWidth",
+ "Width of the area where to search for the cable.");
+
+ def->optional(
+ properties.cableFix.maxAreaTh,
+ "p.cableFix.maxAreaTh",
+ "The cable will only be removed if the cluster area is below this threshold.");
+
+ def->optional(
+ properties.chainApproximationParams.distanceTh, "p.chainApproximation.distanceTh", "");
+ def->optional(properties.chainApproximationParams.maxIterations,
+ "p.chainApproximation.maxIterations",
+ "");
+
+ def->optional(properties.scanClusteringParams.angleThreshold,
+ "p.scanClustering.angleThreshold",
+ "Angular distance between consecutive points in laser scan for clustering.");
+ def->optional(properties.scanClusteringParams.distanceThreshold,
+ "p.scanClustering.distanceThreshold",
+ "Radial distance between consecutive points in laser scan for clustering.");
+ def->optional(properties.scanClusteringParams.maxDistance,
+ "p.scanClustering.maxDistance",
+ "Maximum radius around sensors to detect clusters.");
+
+ def->optional(properties.arviz.drawRawPoints,
+ "p.arviz.drawRawPoints",
+ "If true, the laser scans will be drawn.");
+
+ def->optional(properties.arviz.visualizeSeparateFeatures,
+ "p.arviz.visualizeSeparateFeatures",
+ "If true, the features from each sensor will be drawn.");
+
+ def->optional(properties.arviz.visualizeMergedFeatures,
+ "p.arviz.visualizeMergedFeatures",
+ "If true, the merged features from all sensors will be drawn.");
+
+ return def;
+ }
+
+ LaserScannerFeatureExtraction::LaserScannerFeatureExtraction()
+ {
+ addPlugin(laserScannerReaderPlugin);
+ addPlugin(laserScannerFeatureWriterPlugin);
+ }
+
+ void
+ LaserScannerFeatureExtraction::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+ void
+ LaserScannerFeatureExtraction::onConnectComponent()
+ {
+ ARMARX_INFO << "Connecting";
+
+ frequencyReporterPublish = std::make_unique<FrequencyReporter>(
+ getDebugObserver(), "LaserScannerFeatureExtraction_publish");
+ frequencyReporterRun = std::make_unique<FrequencyReporter>(
+ getDebugObserver(), "LaserScannerFeatureExtraction_run");
+
+ arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient());
+
+ featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
+ properties.chainApproximationParams);
+
+ ARMARX_INFO << "Connected";
+
+ robot = RemoteRobot::createLocalClone(getRobotStateComponent());
+ RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ switch (properties.robotHull.shape)
+ {
+ case Properties::RobotHull::RECTANGLE:
+ {
+ // initialize robot platform convex hull
+ const Eigen::Vector2f ptFrontLeft = conversions::to2D(
+ robot->getRobotNode("PlatformCornerFrontLeft")->getPositionInRootFrame());
+ const Eigen::Vector2f ptFrontRight = conversions::to2D(
+ robot->getRobotNode("PlatformCornerFrontRight")->getPositionInRootFrame());
+ const Eigen::Vector2f ptBackRight = conversions::to2D(
+ robot->getRobotNode("PlatformCornerBackRight")->getPositionInRootFrame());
+ const Eigen::Vector2f ptBackLeft = conversions::to2D(
+ robot->getRobotNode("PlatformCornerBackLeft")->getPositionInRootFrame());
+
+ const auto addMargin = [&](const Eigen::Vector2f& pt) -> Eigen::Vector2f
+ {
+ Eigen::Vector2f ptWithMargin = pt;
+ ptWithMargin.x() +=
+ std::copysignf(properties.robotHull.robotConvexHullMargin, pt.x());
+ ptWithMargin.y() +=
+ std::copysignf(properties.robotHull.robotConvexHullMargin, pt.y());
+
+ return ptWithMargin;
+ };
+
+ {
+ std::vector<Eigen::Vector2f> hullPoints;
+ hullPoints.push_back(addMargin(ptFrontLeft));
+ hullPoints.push_back(addMargin(ptFrontRight));
+ hullPoints.push_back(addMargin(ptBackRight));
+ hullPoints.push_back(addMargin(ptBackLeft));
+
+ robotHull = VirtualRobot::MathTools::createConvexHull2D(hullPoints);
+ }
+
+ // cable fix is only applicable when the robot shape is a rectangle
+ if (properties.cableFix.enabled)
+ {
+ // the cable area and the robot hull overlap slightly. as all points must be within one region,
+ // the cable area should start at approx. the position of the wheels
+
+ std::vector<Eigen::Vector2f> cableAreaPoints;
+ cableAreaPoints.emplace_back(addMargin(ptBackRight) +
+ 100 * Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackLeft) +
+ 100 * Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackRight) -
+ properties.cableFix.cableAreaWidth *
+ Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackLeft) -
+ properties.cableFix.cableAreaWidth *
+ Eigen::Vector2f::UnitY());
+
+ cableArea = VirtualRobot::MathTools::createConvexHull2D(cableAreaPoints);
+ }
+
+ break;
+ }
+ case Properties::RobotHull::CIRCLE:
+ {
+ // cable fix is not supported for circular robots
+ if (properties.cableFix.enabled)
+ {
+ ARMARX_ERROR << "Cable fix is not supported for circular robots!";
+ }
+ const Eigen::Vector2f root =
+ conversions::to2D(robot->getRootNode()->getPositionInRootFrame());
+ robotHull = root;
+ break;
+ }
+ }
+
+
+ task = new PeriodicTask<LaserScannerFeatureExtraction>(
+ this,
+ &LaserScannerFeatureExtraction::runPeriodically,
+ properties.taskPeriodMs,
+ false,
+ "runningTask");
+ task->start();
+ }
+
+ memory::LaserScannerFeature
+ toArmemFeature(const Features& features)
+ {
+ memory::LaserScannerFeature armemFeature;
+
+ if (features.chain)
+ {
+ armemFeature.chain = features.chain.value();
+ }
+
+ if (features.circle)
+ {
+ armemFeature.circle = features.circle.value();
+ }
+
+ if (features.convexHull)
+ {
+ armemFeature.convexHull = features.convexHull->vertices;
+ }
+
+ if (features.ellipsoid)
+ {
+ armemFeature.ellipsoid = features.ellipsoid.value();
+ }
+
+ armemFeature.points = features.points;
+
+ return armemFeature;
+ }
+
+ memory::LaserScannerFeatures
+ toArmemFeatures(const std::vector<Features>& features,
+ const Eigen::Isometry3f& global_T_sensor,
+ const std::string& sensorFrame)
+ {
+ memory::LaserScannerFeatures armemFeatures;
+ armemFeatures.frame = sensorFrame;
+ armemFeatures.frameGlobalPose = global_T_sensor;
+
+
+ armemFeatures.features = simox::alg::apply(features, toArmemFeature);
+
+ return armemFeatures;
+ }
+
+ std::vector<Features>
+ removeInvalidFeatures(std::vector<Features> features)
+ {
+ const auto isInvalid = [](const Features& features) -> bool
+ {
+ if (features.points.size() < 2)
+ {
+ return true;
+ }
+
+ if (not features.convexHull.has_value())
+ {
+ return true;
+ }
+
+ // if(not features.circle.has_value())
+ // {
+ // return true;
+ // }
+
+ // if(not features.ellipsoid.has_value())
+ // {
+ // return true;
+ // }
+
+ return false;
+ };
+
+
+ features.erase(std::remove_if(features.begin(), features.end(), isInvalid), features.end());
+
+ return features;
+ }
+
+ std::vector<Features>
+ LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
+ const std::vector<Features>& featuresFromExtractor)
+ {
+ ARMARX_DEBUG << "Publishing data";
+
+ // obtain sensor pose
+ RemoteRobot::synchronizeLocalCloneToTimestamp(
+ robot, getRobotStateComponent(), data.header.timestamp.toMicroSecondsSinceEpoch());
+ const Eigen::Isometry3f global_T_sensor(
+ robot->getRobotNode(data.header.frame)->getGlobalPose());
+ const Eigen::Isometry3f robot_T_sensor(
+ robot->getRobotNode(data.header.frame)->getPoseInRootFrame());
+
+ //Eigen::AlignedBox2f box;
+ //box.extend(pt1).extend(pt2).extend(pt3).extend(pt4);
+
+ const auto transformPoints =
+ [](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& tf)
+ {
+ std::vector<Eigen::Vector2f> out;
+ out.reserve(points.size());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(out),
+ [&tf](const auto& pt)
+ { return conversions::to2D(tf * conversions::to3D(pt)); });
+
+ return out;
+ };
+
+ const auto isPointInsideRobot = [&](const Eigen::Vector2f& pt) -> bool
+ {
+ if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
+ {
+ return VirtualRobot::MathTools::isInside(
+ pt, std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull));
+ }
+ else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
+ {
+ return (std::get<Eigen::Vector2f>(robotHull) - pt).norm() <
+ properties.robotHull.radius;
+ }
+ return false;
+ };
+ const auto isPointInsideCableArea = [&](const Eigen::Vector2f& pt) -> bool
+ { return VirtualRobot::MathTools::isInside(pt, cableArea); };
+ const auto isClusterInvalid = [&](const Features& features) -> bool
+ {
+ // either use convex hull (compact representation) or raw points as fallbacck
+ const auto points = [&]()
+ {
+ if (features.convexHull)
+ {
+ return transformPoints(features.convexHull->vertices, robot_T_sensor);
+ }
+
+ return transformPoints(features.points, robot_T_sensor);
+ }();
+
+ const bool allPointsInsideRobot =
+ std::all_of(points.begin(), points.end(), isPointInsideRobot);
+ if (allPointsInsideRobot)
+ {
+ return true;
+ }
+
+ if (properties.cableFix.enabled)
+ {
+
+ const bool allPointsInCableArea =
+ std::all_of(points.begin(), points.end(), isPointInsideCableArea);
+ if (allPointsInCableArea)
+ {
+ if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
+ {
+ return true;
+ }
+ }
+ }
+
+ return false;
+ };
+
+ const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
+ {
+ features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
+ features.end());
+ return features;
+ };
+
+ ARMARX_VERBOSE << featuresFromExtractor.size() << " features from extractor";
+
+ const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
+ ARMARX_VERBOSE << features.size() << " features without cable region";
+
+ const auto validFeatures = removeInvalidFeatures(features);
+ ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
+
+ const auto armemFeatures =
+ toArmemFeatures(validFeatures, global_T_sensor, data.header.frame);
+
+ ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
+
+ // report the features
+ publishFeatures(armemFeatures, data.header.timestamp);
+
+ if (properties.arviz.visualizeSeparateFeatures)
+ {
+ drawFeatures(
+ features, &data, data.header.frame, data.header.timestamp, global_T_sensor);
+ }
+
+ return validFeatures;
+ }
+
+ Throttler&
+ LaserScannerFeatureExtraction::getOrCreateThrottler(std::string frame)
+ {
+ const auto it = throttlers.find(frame);
+ if (it == throttlers.end())
+ {
+ throttlers.emplace(frame, Throttler(10.F));
+ }
+
+ return throttlers.at(frame);
+ }
+
+ void
+ LaserScannerFeatureExtraction::drawRobot(const armem::Time& timestamp)
+ {
+ // check if arviz should be triggered
+ if (getOrCreateThrottler(robot->getName()).check(timestamp))
+ {
+ const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
+
+ if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
+ {
+ VirtualRobot::MathTools::ConvexHull2DPtr& convexHullPtr =
+ std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull);
+ if (convexHullPtr != nullptr)
+ {
+ arVizDrawer->draw("robot_convex_hull",
+ *convexHullPtr,
+ global_T_robot,
+ simox::Color::azure(255, 80));
+ }
+ }
+ else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
+ {
+ Eigen::Vector2f& root = std::get<Eigen::Vector2f>(robotHull);
+ arVizDrawer->draw("robot_circle_hull",
+ Circle{root, properties.robotHull.radius},
+ global_T_robot,
+ simox::Color::azure(255, 80));
+ }
+
+ if (cableArea != nullptr)
+ {
+ arVizDrawer->draw(
+ "cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
+ }
+ }
+ }
+
+ void
+ LaserScannerFeatureExtraction::drawFeatures(
+ const std::vector<Features>& features,
+ const armem::laser_scans::LaserScanStamped* laserPoints,
+ const std::string frame,
+ const armem::Time& timestamp,
+ const Eigen::Isometry3f& global_T_sensor)
+ {
+ // check if arviz should be triggered
+ if (getOrCreateThrottler(frame).check(timestamp))
+ {
+ arVizDrawer->draw(features, frame, global_T_sensor);
+
+ if (properties.arviz.drawRawPoints && laserPoints != nullptr)
+ {
+ arVizDrawer->draw(*laserPoints, global_T_sensor, simox::Color::magenta(255, 100));
+ }
+ }
+ }
+
+ void
+ LaserScannerFeatureExtraction::publishFeatures(const memory::LaserScannerFeatures& features,
+ const armem::Time& timestamp)
+ {
+
+ // store in memory
+ laserScannerFeatureWriterPlugin->get().store(features, getName(), timestamp);
+
+ // legacy - topic
+ LineSegment2DChainSeq chains;
+ for (const auto& feature : features.features)
+ {
+ if (not feature.chain.empty())
+ {
+ LineSegment2DChain chain;
+ for (const auto& pt : feature.chain)
+ {
+ chain.push_back(
+ conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
+ }
+ chains.push_back(chain);
+ }
+ }
+
+ featuresTopic->reportExtractedLineSegments(chains);
+ }
+
+ void
+ LaserScannerFeatureExtraction::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void
+ LaserScannerFeatureExtraction::onExitComponent()
+ {
+ }
+
+ std::string
+ LaserScannerFeatureExtraction::getDefaultName() const
+ {
+ return defaultName;
+ }
+
+ std::string
+ LaserScannerFeatureExtraction::GetDefaultName()
+ {
+ return defaultName;
+ }
+
+ void
+ LaserScannerFeatureExtraction::runPeriodically()
+ {
+ const armem::laser_scans::client::Reader::Query laserScanQuery{
+ .agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
+
+ const armem::laser_scans::client::Reader::Result laserScanResult =
+ laserScannerReaderPlugin->get().queryData(laserScanQuery);
+ ARMARX_CHECK_EQUAL(laserScanResult.status,
+ armem::laser_scans::client::Reader::Result::Success)
+ << laserScanResult.errorMessage;
+
+ ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
+ << " sensors";
+
+ frequencyReporterRun->add(IceUtil::Time::now().toMicroSeconds());
+
+ std::vector<FramedFeatures> allFeatures;
+ allFeatures.reserve(laserScanResult.laserScans.size());
+ armem::Time mostRecentTimestamp{0};
+
+ for (const auto& scan : laserScanResult.laserScans)
+ {
+ mostRecentTimestamp = std::max(mostRecentTimestamp, scan.header.timestamp);
+
+ // run clustering for every sensor
+ setDebugObserverDatafield(getDefaultName() + scan.header.frame, scan.data.size());
+ const auto features = featureExtractor->onData(scan);
+
+ auto& framedFeatures = allFeatures.emplace_back();
+
+ // onFeatures filters out some clusters (cable area, inside robot hull etc.), we only want to keep the filtered points
+ framedFeatures.features = onFeatures(scan, features);
+ framedFeatures.frame = scan.header.frame;
+ }
+
+ if (!allFeatures.empty())
+ {
+ mergeFeatures(allFeatures, mostRecentTimestamp);
+ }
+
+ // visualize robot with bounding box/cable area etc.
+ drawRobot(mostRecentTimestamp);
+
+ // some debugobserver reporting
+ frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
+ }
+
+ void
+ LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
+ const armem::Time& timestamp)
+ {
+ // convert features to global coordinate system
+ std::vector<Features> converted;
+ for (const auto& features : framedFeatures)
+ {
+ converted.reserve(converted.size() + features.features.size());
+ Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
+
+ for (const auto& singleFeature : features.features)
+ {
+ converted.push_back(conversions::transformFeature(singleFeature, global_T_frame));
+ }
+ }
+
+ // merge overlapping features
+ FeatureMerger merger{std::move(converted), properties.chainApproximationParams};
+ std::vector<Features> merged = merger.merge();
+
+ // save merged features in another entity with name 'global'
+ std::string frame = "global";
+ const Eigen::Isometry3f global_T_global = Eigen::Isometry3f::Identity();
+
+ const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
+
+ laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
+
+ setDebugObserverDatafield("numMergedClusters", merged.size());
+
+ if (properties.arviz.visualizeMergedFeatures)
+ {
+ // visualize the features above the floor
+ auto global_T_vis = global_T_global;
+ global_T_vis.translation() = Eigen::Vector3f{0, 0, 20};
+ drawFeatures(merged, nullptr, frame, timestamp, global_T_vis);
+ }
+ }
+
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
+ LaserScannerFeatureExtraction::GetDefaultName());
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..e634dd27072aa06e37195638da03bc28a4fc4986
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -0,0 +1,255 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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 <variant>
+
+#include "ArmarXCore/core/util/Throttler.h"
+#include "ArmarXCore/libraries/logging/FrequencyReporter.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h"
+#include "RobotAPI/libraries/armem/client/forward_declarations.h"
+#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
+
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
+
+#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ using LaserScannerFeaturesListenerPrx =
+ armarx::laser_scanner_feature_extraction::LaserScannerFeaturesListenerPrx;
+ using LineSegment2DChain = armarx::laser_scanner_feature_extraction::LineSegment2DChain;
+ using LineSegment2DChainSeq = armarx::laser_scanner_feature_extraction::LineSegment2DChainSeq;
+
+ // class FeatureExtractor;
+ // class ArVizDrawer;
+
+ /**
+ * @defgroup Component-LaserScannerFeatureExtraction LaserScannerFeatureExtraction
+ * @ingroup RobotComponents-Components
+ * A description of the component LaserScannerFeatureExtraction.
+ *
+ * @class LaserScannerFeatureExtraction
+ * @ingroup Component-LaserScannerFeatureExtraction
+ * @brief Brief description of class LaserScannerFeatureExtraction.
+ *
+ * Detailed description of class LaserScannerFeatureExtraction.
+ */
+ class LaserScannerFeatureExtraction :
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ ,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public RobotStateComponentPluginUser,
+ virtual public armem::ClientPluginUser
+
+ {
+ public:
+ LaserScannerFeatureExtraction();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+ protected:
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+ private:
+ struct FramedFeatures
+ {
+ std::vector<Features> features;
+ std::string frame;
+ };
+
+ void runPeriodically();
+
+ std::vector<Features> onFeatures(const armem::laser_scans::LaserScanStamped& data,
+ const std::vector<Features>& features);
+
+ Throttler& getOrCreateThrottler(std::string frame);
+ void drawRobot(const armem::Time& timestamp);
+ void drawFeatures(const std::vector<Features>& features,
+ const armem::laser_scans::LaserScanStamped* laserPoints,
+ const std::string frame,
+ const armem::Time& timestamp,
+ const Eigen::Isometry3f& global_T_sensor);
+
+ void mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
+ const armem::Time& timestamp);
+
+ void publishFeatures(const memory::LaserScannerFeatures& features,
+ const armem::Time& timestamp);
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+ // Private member variables go here.
+ static const std::string defaultName;
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ int taskPeriodMs = 100;
+
+ //
+ struct RobotHull
+ {
+ enum Shape
+ {
+ RECTANGLE,
+ CIRCLE
+ };
+
+ Shape shape = RECTANGLE;
+ // for shape circle
+ float radius = 500.F; // [mm]
+ // for shape rectangle
+ float robotConvexHullMargin{50.F};
+ } robotHull;
+
+ //
+ struct CableFix
+ {
+ bool enabled = {true};
+ float cableAreaWidth{400};
+ float maxAreaTh{50 * 50};
+ } cableFix;
+
+ ScanClustering::Params scanClusteringParams{
+ .distanceThreshold = 30.F, // [mm]
+ // we know the scan resolution which produces a bit more than 1000 points
+ .angleThreshold = 2 * M_PIf32 / 1000.F * 5.F, // [rad]
+ .maxDistance = 3'000.F // [mm]
+ };
+
+ ChainApproximation::Params chainApproximationParams{
+ .distanceTh = 40.F // [mm]
+ };
+
+ struct ArViz
+ {
+ bool drawRawPoints = {true};
+ bool visualizeSeparateFeatures = {false};
+ bool visualizeMergedFeatures = {true};
+ } arviz;
+ };
+
+ Properties properties;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
+
+ PeriodicTask<LaserScannerFeatureExtraction>::pointer_type task;
+
+ std::unique_ptr<FeatureExtractor> featureExtractor;
+ std::unique_ptr<ArVizDrawer> arVizDrawer;
+
+ std::unique_ptr<FrequencyReporter> frequencyReporterPublish;
+ std::unique_ptr<FrequencyReporter> frequencyReporterRun;
+
+ std::unordered_map<std::string, Throttler> throttlers;
+
+ VirtualRobot::RobotPtr robot;
+
+ LaserScannerFeaturesListenerPrx featuresTopic;
+
+ std::variant<std::monostate, VirtualRobot::MathTools::ConvexHull2DPtr, Eigen::Vector2f>
+ robotHull;
+ VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
+
+
+ armem::client::plugins::ReaderWriterPlugin<armem::laser_scans::client::Reader>*
+ laserScannerReaderPlugin = nullptr;
+
+
+ armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
+ laserScannerFeatureWriterPlugin = nullptr;
+ };
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice b/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..2c38043810363d0b8273a1e50e6234a2de2d9cae
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
@@ -0,0 +1,35 @@
+/*
+ * 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 navigation::LaserScannerFeatureExtraction
+ * author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * date 2023
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armarx { module navigation { module components { module laser_scanner_feature_extraction
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eea10eeb503434a73080dc9b1092b42a767293ba
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
@@ -0,0 +1,123 @@
+#include "EnclosingEllipsoid.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+#include <VirtualRobot/math/Helpers.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // Eigen::Affine2f Ellipsoid::pose() const noexcept
+ // {
+ // Eigen::Affine2f pose;
+ // pose.translation() = center;
+ // pose.linear() = Eigen::Rotation2Df(angle).toRotationMatrix();
+
+ // return pose;
+ // }
+
+ // TODO(fabian.reister): use Eigen built-in stuff.
+ size_t
+ argmin(const Eigen::VectorXf& v)
+ {
+ const std::vector<float> vec(v.data(), v.data() + v.rows() * v.cols());
+ return std::distance(std::begin(vec), std::max_element(vec.begin(), vec.end()));
+ }
+
+ // https://github.com/minillinim/ellipsoid/blob/master/ellipsoid.py
+
+ EnclosingEllipsoid::EnclosingEllipsoid(const Points& points)
+ {
+ ARMARX_CHECK(compute(points));
+ }
+
+ bool
+ EnclosingEllipsoid::compute(const Points& points)
+ {
+ const float tolerance = 0.01;
+
+ const int N = static_cast<int>(points.size());
+ const int d = 2;
+
+ Eigen::MatrixXf P(N, d);
+
+ for (int i = 0; i < N; i++)
+ {
+ P.row(i) = points.at(i);
+ }
+
+ Eigen::MatrixXf Q(d + 1, N);
+ Q.topRows(d) = P.transpose();
+ Q.bottomRows(1).setOnes(); // last row
+
+ Eigen::MatrixXf Qt = Q.transpose();
+
+ float err = 1.F + tolerance;
+ Eigen::VectorXf u = (1.F / N) * Eigen::VectorXf::Ones(N);
+
+ // Khachiyan Algorithm
+ while (err > tolerance)
+ {
+ const Eigen::MatrixXf V = Q * (u.asDiagonal() * Qt);
+ const Eigen::VectorXf M = (Qt * (V.inverse() * Q)).diagonal();
+
+ const int j = static_cast<int>(argmin(M));
+ const float maximum = M(j);
+
+ const float stepSize = (maximum - d - 1.0) / ((d + 1.0) * (maximum - 1.0));
+
+ Eigen::VectorXf uNew = (1.0 - stepSize) * u;
+ uNew(j) += stepSize;
+
+ err = (uNew - u).norm();
+ u = uNew;
+ }
+
+ const Eigen::VectorXf center = P.transpose() * u;
+
+ Eigen::MatrixXf K(d, d);
+ for (int i = 0; i < d; i++)
+ {
+ for (int j = 0; j < d; j++)
+ {
+ K(i, j) = center(i) * center(j);
+ }
+ }
+
+ const Eigen::MatrixXf H = (P.transpose() * (u.asDiagonal() * P)) - K;
+ const Eigen::MatrixXf A = H.inverse() / d;
+
+ const Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
+
+ // angle
+ const Eigen::VectorXf v0 = svd.matrixV().col(0);
+ const float angle = math::Helpers::Angle(v0);
+
+ // radii
+ const Eigen::Vector2f radii = svd.singularValues().cwiseSqrt().cwiseInverse();
+
+ // As the singular values are sorted descending, the radii are sorted ascending.
+ // To fix this, the vector is reversed.
+ this->radii = radii.reverse();
+
+ // Thus, the angle does no longer match the radii. It has to be altered by 90°.
+ const float angle1 = simox::math::periodic_clamp(angle + M_PI_2f32, -M_PIf32, M_PIf32);
+
+ this->pose.setIdentity();
+ this->pose.translation().head<2>() = center;
+ this->pose.linear() =
+ Eigen::AngleAxisf(angle1, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ return true;
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
new file mode 100644
index 0000000000000000000000000000000000000000..277531e7ab181ad3e5a21572ce16e13eaae967de
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
@@ -0,0 +1,71 @@
+/*
+ * 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 <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // struct Ellipsoid
+ // {
+ // Eigen::Vector2f center;
+ // float angle;
+
+ // Eigen::Vector2f radii;
+
+ // Eigen::Affine2f pose() const noexcept;
+ // };
+
+ using Ellipsoid = memory::Ellipsoid;
+
+ /**
+ * @brief Minimum volume enclosing ellipsoid (MVEE) for a set of points.
+ *
+ * See https://de.mathworks.com/matlabcentral/fileexchange/9542-minimum-volume-enclosing-ellipsoid
+ *
+ */
+ class EnclosingEllipsoid : public Ellipsoid
+ {
+ public:
+ using Point = Eigen::Vector2f;
+ using Points = std::vector<Point>;
+
+ EnclosingEllipsoid(const Points& points);
+
+ private:
+ /**
+ * @brief Computes the enclosing ellipsoid for the given points by using Khachiyan's Algorithm.
+ *
+ * The implementation is based on
+ * https://github.com/minillinim/ellipsoid/blob/master/ellipsoid.py
+ *
+ * @return true
+ * @return false
+ */
+ bool compute(const Points& points);
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a7ab54b425c722a6531bc2d5b7920f5bb1ac6bc
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -0,0 +1,237 @@
+#include "FeatureExtractor.h"
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <numeric>
+#include <optional>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Transform.h>
+
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
+
+#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
+
+#include "Path.h"
+#include "conversions/eigen.h"
+#include "conversions/opencv.h"
+#include "conversions/opencv_eigen.h"
+#include "conversions/opencv_pcl.h"
+#include "conversions/pcl_eigen.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // Features
+
+ std::vector<Ellipsoid>
+ Features::linesAsEllipsoids(const float axisLength) const
+ {
+ if (not chain)
+ {
+ return {};
+ }
+
+ const auto asEllipsoid = [&](const Path::Segment& segment) -> Ellipsoid
+ {
+ const Eigen::Vector2f center = (segment.first + segment.second) / 2;
+ const Eigen::Vector2f v = segment.second - center;
+ const float angle = math::Helpers::Angle(v);
+ const float r = v.norm();
+
+ Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
+ globalPose.translation().head<2>() = center;
+ globalPose.linear() =
+ Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ return Ellipsoid{.pose = globalPose, .radii = Eigen::Vector2f{r, axisLength}};
+ };
+
+ const auto segments = Path{.points = *chain}.segments();
+
+ std::vector<Ellipsoid> ellipsoids;
+ std::transform(
+ segments.begin(), segments.end(), std::back_inserter(ellipsoids), asEllipsoid);
+
+ return ellipsoids;
+ }
+
+ // FeatureExtractor
+
+ FeatureExtractor::FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
+ const ChainApproximation::Params& chainApproximationParams) :
+ scanClusteringParams(scanClusteringParams),
+ chainApproximationParams(chainApproximationParams)
+ {
+ }
+
+ std::vector<Features>
+ FeatureExtractor::onData(const armem::laser_scans::LaserScanStamped& data)
+ {
+ ARMARX_DEBUG << "on data";
+ const auto clustersWithFeatures = features(data.data);
+
+ ARMARX_DEBUG << "callback";
+ return clustersWithFeatures;
+ }
+
+ std::vector<Features>
+ FeatureExtractor::features(const LaserScan& scan) const
+ {
+ const auto clusters = detectClusters(scan);
+
+ std::vector<Features> fs;
+ std::transform(clusters.begin(),
+ clusters.end(),
+ std::back_inserter(fs),
+ [&](const LaserScan& cluster)
+ {
+ const auto points = toCartesian<Eigen::Vector2f>(cluster);
+ const auto hull = convexHull(points);
+
+ return Features{
+ .convexHull = hull,
+ .circle = circle(points),
+ .ellipsoid = std::nullopt, //ellipsoid(hull),
+ .chain = chainApproximation(points, hull, chainApproximationParams),
+ .points = points};
+ });
+
+ return fs;
+ }
+
+ std::vector<LaserScan>
+ FeatureExtractor::detectClusters(const LaserScan& scan) const
+ {
+ ScanClustering clustering(scanClusteringParams);
+ return clustering.detectClusters(scan);
+ }
+
+ std::optional<VirtualRobot::MathTools::ConvexHull2D>
+ FeatureExtractor::convexHull(const Points& points)
+ {
+
+ if (points.size() < 3) // no meaningful area
+ {
+ return std::nullopt;
+ }
+
+ try
+ {
+ return *VirtualRobot::MathTools::createConvexHull2D(points);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_WARNING << "Couldn't create convex hull: " << e.what();
+ }
+ return std::nullopt;
+ }
+
+ // Legacy OpenCV implementation. Didn't work as expected.
+ // Also, tries to fit an ellipsoid which is NOT the enclosing ellipsoid!
+
+ // std::optional<Ellipsoid> FeatureExtractor::ellipsoid(const PointCloud &pointCloud) const
+ // {
+ // // OpenCV::fitEllipse requirement
+ // // "There should be at least 5 points to fit the ellipse"
+ // // => Too few points will cause algorithmic issues
+ // if (pointCloud.size() < 100)
+ // {
+ // return std::nullopt;
+ // }
+
+ // const auto points2f = conversions::pcl2cv(pointCloud);
+ // const auto points2i = conversions::cast<cv::Point2i>(points2f);
+
+ // cv::RotatedRect rect = cv::fitEllipse(points2i);
+
+ // Eigen::Affine2f pose;
+ // pose.translation() = conversions::cv2eigen(rect.center);
+ // pose.linear() =
+ // Eigen::Rotation2Df(VirtualRobot::MathTools::deg2rad(rect.angle)).toRotationMatrix();
+
+ // return Ellipsoid{.axisLengths = conversions::cv2eigen(rect.size), .pose = pose};
+ // }
+
+ std::optional<Ellipsoid>
+ FeatureExtractor::ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull)
+ {
+
+ if (not hull)
+ {
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): it might result in multiple ellipsoids if hull->segments is considered
+
+ // If there are not enough points, don't even try to fit an ellipse ...
+ if (hull->vertices.size() < 5)
+ {
+ return std::nullopt;
+ }
+
+ EnclosingEllipsoid mvee(hull->vertices);
+ return mvee;
+ }
+
+ std::optional<Circle>
+ FeatureExtractor::circle(const Points& points)
+ {
+ // Too few points will cause algorithmic issues
+ if (points.size() < 5)
+ {
+ return std::nullopt;
+ }
+
+ const auto points2f = conversions::eigen2cv(points);
+ const auto points2i = conversions::cast<cv::Point2i>(points2f);
+
+ cv::Point2f center;
+ float radius = NAN;
+ cv::minEnclosingCircle(points2i, center, radius);
+
+ return Circle{.center = conversions::cv2eigen(center), .radius = radius};
+ }
+
+ std::optional<FeatureExtractor::Points>
+ FeatureExtractor::chainApproximation(
+ const Points& points,
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull,
+ const ChainApproximation::Params& params)
+ {
+ if (not convexHull)
+ {
+ return std::nullopt;
+ }
+
+ ChainApproximation chApprx(points, params);
+ const auto result = chApprx.approximate();
+
+ if (result.condition !=
+ ChainApproximation::ApproximationResult::TerminationCondition::Converged)
+ {
+ return std::nullopt;
+ }
+
+ return chApprx.approximatedChain();
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8273931d2a6a0583df348e25202a0b616026b191
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -0,0 +1,90 @@
+/*
+ * 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 <optional>
+
+#include <pcl/point_types.h>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include <armarx/navigation/memory/types.h>
+
+#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
+
+#include "EnclosingEllipsoid.h"
+#include "ScanClustering.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ using Circle = memory::Circle;
+
+ struct Features
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ std::optional<VirtualRobot::MathTools::ConvexHull2D> convexHull;
+
+ std::optional<Circle> circle;
+ std::optional<Ellipsoid> ellipsoid;
+
+ std::optional<Chain> chain;
+
+ Points points;
+ std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
+ class FeatureExtractor
+ {
+ public:
+ using Points = std::vector<Eigen::Vector2f>;
+
+ FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
+ const ChainApproximation::Params& chainApproximationParams);
+
+ std::vector<Features> onData(const armem::laser_scans::LaserScanStamped& data);
+
+ public:
+ static std::optional<VirtualRobot::MathTools::ConvexHull2D>
+ convexHull(const Points& points);
+ static std::optional<Ellipsoid>
+ ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull);
+ static std::optional<Circle> circle(const Points& circle);
+ static std::optional<Points>
+ chainApproximation(const Points& points,
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull,
+ const ChainApproximation::Params& params);
+
+
+ private:
+ std::vector<Features> features(const LaserScan& scan) const;
+
+ std::vector<LaserScan> detectClusters(const LaserScan& scan) const;
+
+ const ScanClustering::Params scanClusteringParams;
+ const ChainApproximation::Params chainApproximationParams;
+ };
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..173e3aef164f529a2d103d3e32a9e827d5a57fe7
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
@@ -0,0 +1,174 @@
+#include "FeatureMerger.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h>
+
+#include <range/v3/view/enumerate.hpp>
+#include <range/v3/view/sliding.hpp>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+
+ FeatureMerger::FeatureMerger(std::vector<Features>&& features,
+ const ChainApproximation::Params& chainApproximationParams) :
+ inputFeatures(std::move(features)), chainApproximationParams(chainApproximationParams)
+ {
+ }
+
+ FeatureMerger::FeatureMerger(const std::vector<Features>& features,
+ const ChainApproximation::Params& chainApproximationParams) :
+ inputFeatures(features), chainApproximationParams(chainApproximationParams)
+ {
+ }
+
+ std::vector<Features>
+ FeatureMerger::merge()
+ {
+ ARMARX_DEBUG << "Starting merge of " << inputFeatures.size() << " features";
+ UnionFind uf{inputFeatures.size()};
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ std::optional<std::size_t> lastFeature = std::nullopt;
+ for (const auto& p : f.points)
+ {
+ if (auto res = checkPoint(p, i, lastFeature); res)
+ {
+ uf.unite(i, res.value());
+ lastFeature = res.value();
+ }
+ }
+ }
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ std::size_t representative = uf.find(i);
+ if (i != representative)
+ {
+ ARMARX_DEBUG << "add feature " << i << " to feature " << representative;
+ addToFeature(inputFeatures[representative], f);
+ }
+ }
+
+ std::vector<Features> merged;
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ if (i == uf.find(i))
+ {
+ ARMARX_DEBUG << "feature " << i << " is representative";
+ merged.push_back(f);
+ }
+ }
+ // recalculate convex hull etc. for all merged features
+ recalculateFeatures(merged);
+
+ ARMARX_DEBUG << "Merged " << inputFeatures.size() << " features into " << merged.size();
+ return merged;
+ }
+
+ bool
+ FeatureMerger::insideConvexPoly(const Eigen::Vector2f& p,
+ const std::vector<Eigen::Vector2f>& vertices)
+ {
+ // algorithm to check whether a point is inside a convex polygon
+ // see https://stackoverflow.com/questions/1119627/how-to-test-if-a-point-is-inside-of-a-convex-polygon-in-2d-integer-coordinates
+
+ const auto signum = [](float x)
+ {
+ if (x == 0)
+ return 0;
+ return x > 0 ? 1 : -1;
+ };
+
+ const auto checkSide = [&](const Eigen::Vector2f& a, const Eigen::Vector2f& b)
+ { return signum((p.x() - a.x()) * (b.y() - a.y()) - (p.y() - a.y()) * (b.x() - a.x())); };
+
+ float previousSide = 0;
+ for (const auto edge : vertices | ranges::views::sliding(2))
+ {
+ int d = checkSide(edge[0], edge[1]);
+ if (d == 0)
+ return true;
+
+ if (previousSide != 0 && d != previousSide)
+ {
+ return false;
+ }
+ previousSide = d;
+ }
+ int d = checkSide(vertices.back(), vertices.front());
+ if (d == 0)
+ return true;
+ return d == previousSide;
+ }
+
+ std::optional<std::size_t>
+ FeatureMerger::checkPoint(const Eigen::Vector2f p,
+ std::size_t ignore,
+ const std::optional<std::size_t> checkFirst)
+ {
+ // when try first is given check this feature before all others
+ if (checkFirst)
+ {
+ if (insideConvexPoly(p, inputFeatures[checkFirst.value()].convexHull->vertices))
+ {
+ return checkFirst.value();
+ }
+ }
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ if (i == ignore || (checkFirst && i == checkFirst.value()))
+ continue;
+
+ if (insideConvexPoly(p, f.convexHull->vertices))
+ {
+ return i;
+ }
+ }
+ return std::nullopt;
+ }
+
+ void
+ FeatureMerger::addToFeature(Features& f, const Features& toAdd)
+ {
+ f.points.insert(f.points.end(), toAdd.points.begin(), toAdd.points.end());
+
+ f.convexHull = std::nullopt;
+ f.circle = std::nullopt;
+ f.ellipsoid = std::nullopt;
+ f.chain = std::nullopt;
+ }
+
+ void
+ FeatureMerger::recalculateFeatures(std::vector<Features>& features)
+ {
+ for (auto& f : features)
+ {
+ if (!f.convexHull)
+ {
+ f.convexHull = FeatureExtractor::convexHull(f.points);
+ }
+
+ if (!f.circle)
+ {
+ f.circle = FeatureExtractor::circle(f.points);
+ }
+
+ // if (!f.ellipsoid)
+ // {
+ // f.ellipsoid = FeatureExtractor::ellipsoid(f.convexHull);
+ // }
+
+ if (!f.chain)
+ {
+ f.chain = FeatureExtractor::chainApproximation(
+ f.points, f.convexHull, chainApproximationParams);
+ }
+ }
+ }
+
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a71c66d0e87e488d8138ad85311b455002f8772
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
@@ -0,0 +1,56 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ class FeatureMerger
+ {
+ public:
+ FeatureMerger(std::vector<Features>&& features,
+ const ChainApproximation::Params& chainApproximationParams);
+ FeatureMerger(const std::vector<Features>& features,
+ const ChainApproximation::Params& chainApproximationParams);
+
+ std::vector<Features> merge();
+
+ public:
+ static bool insideConvexPoly(const Eigen::Vector2f& p,
+ const std::vector<Eigen::Vector2f>& vertices);
+
+ private:
+ std::optional<std::size_t>
+ checkPoint(const Eigen::Vector2f p,
+ std::size_t ignore,
+ const std::optional<std::size_t> checkFirst = std::nullopt);
+ void addToFeature(Features& f, const Features& toAdd);
+ void recalculateFeatures(std::vector<Features>& features);
+
+ private:
+ std::vector<Features> inputFeatures;
+ const ChainApproximation::Params chainApproximationParams;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d57cae5caa509a13b7271d43b9c1adb7e8ea377c
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
@@ -0,0 +1,24 @@
+#include "Path.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ std::vector<Path::Segment>
+ Path::segments() const noexcept
+ {
+ if (points.size() <= 1)
+ {
+ return {};
+ }
+
+ std::vector<Segment> segments;
+ segments.reserve(static_cast<int>(points.size()) - 1);
+
+ for (int i = 0; i < static_cast<int>(points.size()) - 1; i++)
+ {
+ segments.emplace_back(points.at(i), points.at(i + 1));
+ }
+
+ return segments;
+ }
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
new file mode 100644
index 0000000000000000000000000000000000000000..2cf3a4134e580ee2e7558e42e5a36dff9fa8aae5
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ struct Path
+ {
+ std::vector<Eigen::Vector2f> points;
+
+ using Segment = std::pair<Eigen::Vector2f, Eigen::Vector2f>;
+
+ std::vector<Segment> segments() const noexcept;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32503968e0ba7e865cea541f25cc03c9a4bbde59
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
@@ -0,0 +1,84 @@
+#include "ScanClustering.h"
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ ScanClustering::ScanClustering(const Params& params) : params(params)
+ {
+ }
+
+ bool
+ ScanClustering::add(const LaserScanStep& scanStep)
+ {
+ if (scan.empty())
+ {
+ scan.push_back(scanStep);
+ return true;
+ }
+
+ if (not supports(scanStep))
+ {
+ return false;
+ }
+
+ scan.push_back(scanStep);
+ return true;
+ }
+
+ std::vector<LaserScan>
+ ScanClustering::detectClusters(const LaserScan& scan)
+ {
+ const auto isInvalid = [&](const LaserScanStep& step) -> bool
+ { return step.distance > params.maxDistance; };
+
+ std::vector<LaserScan> clusters;
+ for (const auto& laserScanStep : scan)
+ {
+ // cluster finished
+ if (isInvalid(laserScanStep) or (not add(laserScanStep)))
+ {
+ if (not cluster().empty())
+ {
+ clusters.push_back(cluster());
+ clear();
+
+ // work on a new cluster
+ add(laserScanStep);
+ }
+ }
+ }
+
+ return clusters;
+ }
+
+ const LaserScan&
+ ScanClustering::cluster() const
+ {
+ return scan;
+ }
+
+ void
+ ScanClustering::clear()
+ {
+ scan.clear();
+ }
+
+ bool
+ ScanClustering::supports(const LaserScanStep& scanStep)
+ {
+ // OK to create a new cluster if it's empty
+ if (scan.empty())
+ {
+ return true;
+ }
+
+ const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
+ const bool isWithinDistanceRange = distanceDiff < params.distanceThreshold;
+
+ const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
+ const bool isWithinAngleRange = angleDiff < params.angleThreshold;
+
+ return (isWithinDistanceRange and isWithinAngleRange);
+ }
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
new file mode 100644
index 0000000000000000000000000000000000000000..1159c2c648057ae5275a5989d90bf150f09a8893
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
@@ -0,0 +1,71 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ namespace detail
+ {
+ struct ScanClusteringParams
+ {
+ // Clustering options to decide whether a point belongs to the cluster
+ float distanceThreshold; // [mm]
+ float angleThreshold; // [rad]
+
+ /// Range filter: only consider points that are closer than maxDistance
+ float maxDistance; // [mm]
+ };
+ } // namespace detail
+
+ class ScanClustering
+ {
+ public:
+ using Params = detail::ScanClusteringParams;
+ using Clusters = std::vector<LaserScan>;
+
+ ScanClustering(const Params& params);
+
+ /**
+ * @brief Performs cluster detection on a full laser scan.
+ *
+ * @param scan A full scan
+ * @return The input scan split into clusters.
+ */
+ Clusters detectClusters(const LaserScan& scan);
+
+ protected:
+ const LaserScan& cluster() const;
+
+ bool add(const LaserScanStep& scanStep);
+ bool supports(const LaserScanStep& scanStep);
+
+ void clear();
+
+ private:
+ LaserScan scan;
+
+ const Params params;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e994dae56df1f4c92d65fa17bfd9725a663b2f9
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp
@@ -0,0 +1,70 @@
+#include "UnionFind.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ UnionFind::UnionFind(std::size_t n)
+ {
+ rank.reserve(n);
+ parent.reserve(n);
+ for (std::size_t i = 0; i < n; i++)
+ {
+ rank.push_back(0);
+ parent.push_back(i);
+ }
+ }
+
+ std::size_t
+ UnionFind::find(std::size_t x)
+ {
+ // Finds the representative of the set
+ // that x is an element of
+ if (parent[x] != x)
+ {
+
+ // if x is not the parent of itself
+ // Then x is not the representative of
+ // his set,
+ parent[x] = find(parent[x]);
+
+ // so we recursively call Find on its parent
+ // and move i's node directly under the
+ // representative of this set
+ }
+
+ return parent[x];
+ }
+
+ void
+ UnionFind::unite(std::size_t a, std::size_t b)
+ {
+ // Find current sets of x and y
+ std::size_t xset = find(a);
+ std::size_t yset = find(b);
+
+ // If they are already in same set
+ if (xset == yset)
+ return;
+
+ // Put smaller ranked item under
+ // bigger ranked item if ranks are
+ // different
+ if (rank[xset] < rank[yset])
+ {
+ parent[xset] = yset;
+ }
+ else if (rank[xset] > rank[yset])
+ {
+ parent[yset] = xset;
+ }
+
+ // If ranks are same, then increment
+ // rank.
+ else
+ {
+ parent[yset] = xset;
+ rank[xset] = rank[xset] + 1;
+ }
+ }
+
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7d6ab8582d7031baf291c67001e98c20a64c7ed
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h
@@ -0,0 +1,65 @@
+/*
+ * 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 )
+ * @author Corvin Navarro ( corvin dot ecker at student dot kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <vector>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // The implementation of this class is taken (and modified) from
+ // https://www.geeksforgeeks.org/disjoint-set-data-structures/
+
+ /*! \brief A simple union find data structure
+ *
+ * The data structure supports a fixed size of n elements which must be the
+ * integers from [0, n).
+ */
+ class UnionFind
+ {
+ public:
+ /*! \brief Construct a new union find data structure with n elements
+ * \param n the number of elements
+ *
+ * After construction each element will be in its own individual set.
+ */
+ UnionFind(std::size_t n);
+
+ /*! \brief find the representative of x
+ * \return the representative of x
+ */
+ std::size_t find(std::size_t x);
+
+ /*! \brief unite the sets of the elements a and b
+ * \param a
+ * \param b
+ */
+ void unite(std::size_t a, std::size_t b);
+
+ private:
+ std::vector<std::size_t> rank;
+ std::vector<std::size_t> parent;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..41af90f4e15b87cecec5d046697ba5897e9e8c5d
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp
@@ -0,0 +1,23 @@
+#include "eigen.h"
+
+#include <algorithm>
+
+namespace armarx::conversions
+{
+
+ std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v)
+
+ {
+ std::vector<Eigen::Vector3f> v3;
+ v3.reserve(v.size());
+
+ std::transform(
+ v.begin(),
+ v.end(),
+ std::back_inserter(v3),
+ static_cast<Eigen::Vector3f (*)(const Eigen::Vector2f&)>(&to3D));
+
+ return v3;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..615d41c6d8e80a0f5ff2390079f3627d5281206a
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h
@@ -0,0 +1,53 @@
+/*
+ * 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 <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace armarx::conversions
+{
+
+ inline Eigen::Vector2f to2D(const Eigen::Vector3f& v2)
+ {
+ return Eigen::Vector2f{v2.x(), v2.y()};
+ }
+
+ inline Eigen::Vector3f to3D(const Eigen::Vector2f& v2)
+ {
+ return Eigen::Vector3f{v2.x(), v2.y(), 0.F};
+ }
+
+ inline Eigen::Isometry3f to3D(const Eigen::Affine2f& p2)
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+ pose.linear().block<2, 2>(0, 0) = p2.linear();
+ pose.translation().head<2>() = p2.translation();
+
+ return pose;
+ }
+
+ std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v);
+
+} // namespace armarx::conversions
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4121bb6d4219cb478f13d0469ad6f49b81d1ac3
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp
@@ -0,0 +1,53 @@
+#include "features.h"
+
+#include <SimoxUtility/algorithm/apply.hpp>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h>
+
+namespace armarx::conversions
+{
+ using Features = armarx::navigation::components::laser_scanner_feature_extraction::Features;
+
+ Features
+ transformFeature(const Features& feature, const Eigen::Isometry3f& transformation)
+ {
+ const auto transformPoint =
+ [](const Eigen::Vector2f& point, const Eigen::Isometry3f& global_T_frame)
+ { return to2D(global_T_frame * to3D(point)); };
+
+ const auto transformPoints =
+ [&](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& global_T_frame)
+ {
+ return simox::alg::apply(points,
+ [&](const Eigen::Vector2f& point)
+ { return transformPoint(point, global_T_frame); });
+ };
+
+ Features transformed;
+
+ if (feature.convexHull)
+ {
+ transformed.convexHull = {
+ .vertices = transformPoints(feature.convexHull->vertices, transformation),
+ .segments = feature.convexHull->segments};
+ }
+ if (feature.circle)
+ {
+ transformed.circle = {.center = transformPoint(feature.circle->center, transformation),
+ .radius = feature.circle->radius};
+ }
+ if (feature.ellipsoid)
+ {
+ transformed.ellipsoid = {.pose = transformation * feature.ellipsoid->pose,
+ .radii = feature.ellipsoid->radii};
+ }
+ if (feature.chain)
+ {
+ transformed.chain = transformPoints(feature.chain.value(), transformation);
+ }
+ transformed.points = transformPoints(feature.points, transformation);
+
+ return transformed;
+ }
+
+} // namespace armarx::conversions
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h
new file mode 100644
index 0000000000000000000000000000000000000000..b77546bef54eea0a0ae6cdd6d8bdd7980aa4749e
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h
@@ -0,0 +1,33 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
+
+namespace armarx::conversions
+{
+
+ navigation::components::laser_scanner_feature_extraction::Features transformFeature(
+ const navigation::components::laser_scanner_feature_extraction::Features& feature,
+ const Eigen::Isometry3f& transformation);
+
+} // namespace armarx::conversions
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae12d627f1438237d4cc05069ba544ae3648073b
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h
@@ -0,0 +1,47 @@
+/*
+ * 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 <vector>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+
+ // TODO(fabian.reister): this is a specialized method
+ template <typename CvT> CvT cast(const auto &pt) { return CvT(pt.x, pt.y); }
+
+ template <typename PointOutT, typename PointInT>
+ std::vector<PointOutT> cast(const std::vector<PointInT> &points)
+ {
+ std::vector<PointOutT> v;
+ v.reserve(points.size());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(v),
+ static_cast<PointOutT (*)(const PointInT &)>(cast));
+
+ return v;
+ }
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e6b818219a8f143fc88f523800a850f275eba53
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h
@@ -0,0 +1,52 @@
+/*
+ * 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 <Eigen/Core>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+ inline cv::Point2f eigen2cv(const Eigen::Vector2f& pt)
+ {
+ return {pt.x(), pt.y()};
+ }
+
+ template <typename EigenT, typename CvT = decltype(eigen2cv(EigenT()))>
+ auto eigen2cv(const std::vector<EigenT>& points)
+ {
+ std::vector<CvT> v;
+ v.reserve(points.size());
+
+ std::transform(
+ points.begin(), points.end(), std::back_inserter(v), static_cast<CvT(*)(const EigenT&)>(eigen2cv));
+
+ return v;
+ }
+
+ inline Eigen::Vector2f cv2eigen(const cv::Point2f& pt)
+ {
+ return Eigen::Vector2f{pt.x, pt.y};
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h
new file mode 100644
index 0000000000000000000000000000000000000000..375f166f08b391a9462b7bb8fc2ebf7d8b7ff530
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h
@@ -0,0 +1,57 @@
+/*
+ * 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 <Eigen/Core>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+
+ inline cv::Point2f pcl2cv(const pcl::PointXY& pt)
+ {
+ return cv::Point2f{pt.x, pt.y};
+ }
+ inline cv::Point3f pcl2cv(const pcl::PointXYZ& pt)
+ {
+ return cv::Point3f{pt.x, pt.y, pt.z};
+ }
+
+ template <typename PointT, typename CvT = decltype(pcl2cv(PointT()))>
+ auto pcl2cv(const pcl::PointCloud<PointT>& pointCloud)
+ {
+ std::vector<CvT> v;
+ v.reserve(pointCloud.points.size());
+
+ std::transform(pointCloud.points.begin(),
+ pointCloud.points.end(),
+ std::back_inserter(v),
+ static_cast<CvT (*)(const PointT&)>(pcl2cv));
+
+ return v;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8ed47f21519ffcf26d40a55bd8a288118a0a25f
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp
@@ -0,0 +1,27 @@
+#include "pcl.h"
+
+#include <algorithm>
+
+namespace armarx::conversions
+{
+ pcl::PointCloud<pcl::PointXYZ> to3D(const pcl::PointCloud<pcl::PointXY>& v)
+ {
+ pcl::PointCloud<pcl::PointXYZ> pc;
+ pc.points.reserve(v.size());
+
+ std::transform(
+ v.begin(),
+ v.end(),
+ std::back_inserter(pc),
+ static_cast<pcl::PointXYZ (*)(const pcl::PointXY&)>(&to3D));
+
+ pc.width = v.height;
+ pc.height = v.width;
+ pc.is_dense = v.is_dense;
+
+ pc.header = v.header;
+
+ return pc;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h
new file mode 100644
index 0000000000000000000000000000000000000000..2caf92b95541f850b0db004c78fa8751e8faf751
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h
@@ -0,0 +1,39 @@
+/*
+ * 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 <vector>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace armarx::conversions
+{
+
+ inline pcl::PointXYZ to3D(const pcl::PointXY& v2)
+ {
+ return pcl::PointXYZ{v2.x, v2.y, 0.F};
+ }
+
+ pcl::PointCloud<pcl::PointXYZ> to3D(const pcl::PointCloud<pcl::PointXY>& v);
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..b72277e033c4a51e374d0e9ba87cc12aad48b6b7
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h
@@ -0,0 +1,92 @@
+/*
+ * 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 <algorithm>
+
+#include <Eigen/Core>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace armarx::conversions
+{
+ // pcl2eigen
+
+ inline Eigen::Vector2f pcl2eigen(const pcl::PointXY& pt)
+ {
+ return Eigen::Vector2f{pt.x, pt.y};
+ }
+ inline Eigen::Vector3f pcl2eigen(const pcl::PointXYZ& pt)
+ {
+ return Eigen::Vector3f{pt.x, pt.y, pt.z};
+ }
+
+ template <typename PointT,
+ typename EigenVector = decltype(pcl2eigen(PointT()))>
+ auto pcl2eigen(const pcl::PointCloud<PointT>& pointCloud)
+ -> std::vector<EigenVector>
+ {
+ std::vector<EigenVector> v;
+ v.reserve(pointCloud.points.size());
+
+ std::transform(pointCloud.points.begin(),
+ pointCloud.points.end(),
+ std::back_inserter(v),
+ static_cast<EigenVector(*)(const PointT&)>(pcl2eigen));
+
+ return v;
+ }
+
+ // eigen2pcl
+
+ inline pcl::PointXY eigen2pcl(const Eigen::Vector2f& pt)
+ {
+ return pcl::PointXY{pt.x(), pt.y()};
+ }
+
+ inline pcl::PointXYZ eigen2pcl(const Eigen::Vector3f& pt)
+ {
+ return pcl::PointXYZ{pt.x(), pt.y(), 0.F};
+ }
+
+ template <typename EigenVector,
+ typename PointT = decltype(eigen2pcl(EigenVector()))>
+ auto eigen2pcl(const std::vector<EigenVector>& points)
+ -> pcl::PointCloud<PointT>
+ {
+ pcl::PointCloud<PointT> pointCloud;
+ pointCloud.points.reserve(points.size());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(pointCloud.points),
+ static_cast<PointT(*)(const EigenVector&)>(eigen2pcl));
+
+ pointCloud.width = pointCloud.points.size();
+ pointCloud.height = 1;
+ pointCloud.is_dense = true;
+
+ return pointCloud;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b70a56da0dcd6ba78b76b3c3ec187872d66f0c1
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h
@@ -0,0 +1,57 @@
+#pragma once
+
+// TODO To be moved to Simox after ROBDEKON demo
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/assign.hpp>
+#include <boost/geometry/algorithms/detail/within/interface.hpp>
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/make.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/strategies/strategies.hpp>
+
+#include <VirtualRobot/MathTools.h>
+
+BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2f, float, cs::cartesian, x(), y())
+
+namespace armarx::geometry
+{
+ namespace bg = boost::geometry;
+
+ class ConvexHull
+ {
+ public:
+ using Point = bg::model::d2::point_xy<float>;
+
+ ConvexHull(const VirtualRobot::MathTools::ConvexHull2D& hull) : ConvexHull(hull.vertices)
+ {
+ }
+
+ ConvexHull(const std::vector<Eigen::Vector2f>& hull)
+ {
+ boost::geometry::assign_points(polygon, hull);
+ }
+
+ bool contains(const Eigen::Vector2f& point) const
+ {
+ return bg::within(Point(point.x(), point.y()), polygon);
+ }
+
+ float area() const
+ {
+ return static_cast<float>(bg::area(polygon));
+ }
+
+ private:
+ bg::model::polygon<Point> polygon;
+ };
+
+ // inline float area()
+ // {
+
+ // return bg::area(polygon);
+ // }
+
+} // namespace armarx::geometry
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dee3f4382e762c79f8bb92c93114f404fd5f7030
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
@@ -0,0 +1,61 @@
+# we can't let tests depend on the component so we create a dummy library
+# with the same code (except the component itself)
+armarx_add_library(laser_scanner_feature_extraction_test_library
+ SOURCES
+ #../Component.cpp
+ ../FeatureExtractor.cpp
+ ../FeatureMerger.cpp
+ ../ArVizDrawer.cpp
+ ../ScanClustering.cpp
+ ../ChainApproximation.cpp
+ ../conversions/eigen.cpp
+ ../conversions/pcl.cpp
+ ../conversions/features.cpp
+ ../EnclosingEllipsoid.cpp
+ ../Path.cpp
+ ../UnionFind.cpp
+ HEADERS
+ #../Component.h
+ ../FeatureExtractor.h
+ ../FeatureMerger.h
+ ../ArVizDrawer.h
+ ../ScanClustering.h
+ ../ChainApproximation.h
+ ../EnclosingEllipsoid.h
+ ../Path.h
+ ../geometry.h
+ ../conversions/eigen.h
+ ../conversions/pcl_eigen.h
+ ../conversions/opencv_eigen.h
+ ../conversions/opencv_pcl.h
+ ../conversions/pcl.h
+ ../conversions/features.h
+ ../UnionFind.h
+ DEPENDENCIES
+ ArmarXCore
+ ArmarXCoreComponentPlugins
+ ArmarXCoreLogging
+ RobotAPIComponentPlugins
+ RobotComponentsInterfaces
+ armarx_navigation::memory
+ armem_laser_scans
+ range-v3::range-v3
+ DEPENDENCIES_LEGACY
+ PCL
+)
+
+armarx_add_test(laser_scanner_feature_extraction_test
+ TEST_FILES
+ LaserScannerFeatureExtractionTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::laser_scanner_feature_extraction_test_library
+)
+
+armarx_add_test(laser_scanner_feature_extraction_merger_test
+ TEST_FILES
+ MergerTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::laser_scanner_feature_extraction_test_library
+)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6efd4ed998a9508ce6271a79990cdc8a5e9c106
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
@@ -0,0 +1,71 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
+ */
+
+#include <iostream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
+
+#include "../EnclosingEllipsoid.h"
+
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXObjects::LaserScannerFeatureExtraction
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ BOOST_AUTO_TEST_CASE(testExample)
+ {
+ // Ground truth (GT) ellipsoid with params:
+ // center = (0,0)
+ // radii = (4,2)
+ // angle = 0
+
+ FeatureExtractor::Points points;
+ points.push_back(Eigen::Vector2f{-4.F, 0});
+ points.push_back(Eigen::Vector2f{0, 2.F});
+ points.push_back(Eigen::Vector2f{4.F, 0});
+ points.push_back(Eigen::Vector2f{0, -2.F});
+
+ const Eigen::Vector2f radii(4, 2);
+
+ EnclosingEllipsoid ee(points);
+
+ // The computed ellipsoid must contain the GT ellipsoid
+ BOOST_CHECK_GE(ee.radii.x(), 4.F);
+ BOOST_CHECK_GE(ee.radii.y(), 2.F);
+
+ // ... but should not be too large
+ BOOST_CHECK_LE(ee.radii.x(), 4.1F);
+ BOOST_CHECK_LE(ee.radii.y(), 2.1F);
+
+ // The computed ellipsoid must be roughly at the position of the GT ellipsoid
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().x()), 0.1);
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().y()), 0.1);
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..399a5d15c1a7c019ffa90a093029b26339747b1d
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp
@@ -0,0 +1,66 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
+ */
+
+#include <iostream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
+
+#include "../EnclosingEllipsoid.h"
+
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXObjects::LaserScannerFeatureExtraction
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ BOOST_AUTO_TEST_CASE(insideConvexPolyTest)
+ {
+ std::vector<Eigen::Vector2f> polygon;
+ polygon.push_back({0, 0});
+ polygon.push_back({1, 1});
+ polygon.push_back({0, 2});
+ polygon.push_back({-1, 1});
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0, 1}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0.5, 1}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 1}, polygon));
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0, 2}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({1, 1}, polygon));
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 0.5}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 0.51}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({-0.5, 0.49}, polygon));
+
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({1.1, 1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({0, 2.1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({0, -0.1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({-1, 2}, polygon));
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index 4323fffee55112d4259a7050295c5d1d3ef3428d..d33d52d921c106acecf44654447408b9bb4fe5a9 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -55,6 +55,7 @@
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/rooms/aron/Room.aron.generated.h>
@@ -105,6 +106,10 @@ namespace armarx::navigation::components::navigation_memory
"p.visuTransparent",
"Enable visualization of humans a bit transparent.");
+ def->optional(properties.visuHumanMaxAgeMs,
+ "p.visuHumanMaxAgeMs",
+ "The maximum age of humans to be drawn in ms.");
+
def->optional(properties.visuRooms,
"p.visuRooms",
"Enable visualization of rooms.");
@@ -153,6 +158,11 @@ namespace armarx::navigation::components::navigation_memory
workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
navigation::human::arondto::Human::ToAronType());
+ workingMemory()
+ .addCoreSegment(memory::constants::LaserScannerFeaturesCoreSegment,
+ navigation::memory::arondto::LaserScannerFeatures::ToAronType())
+ .setMaxHistorySize(properties.laserScannerFeaturesMaxHistorySize);
+
// workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
// navigation::human::arondto::Human::ToAronType());
workingMemory().addCoreSegment(navigation::rooms::coreSegmentID.coreSegmentName,
@@ -566,7 +576,7 @@ namespace armarx::navigation::components::navigation_memory
visu.drawCostmaps(layers, p.visuCostmaps);
// Humans
- visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
+ visu.drawHumans(layers, p.visuHumans, p.visuTransparent, Duration::MilliSeconds(p.visuHumanMaxAgeMs));
// Rooms
visu.drawRooms(layers, p.visuRooms);
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index 77220afebd7eafae930ab19c7a436f35ee641d08..32ca3ad9bc5b69d921fbf08a43c3cf79a74caf7d 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -99,6 +99,8 @@ namespace armarx::navigation::components::navigation_memory
{
std::string snapshotToLoad = "";
+ long laserScannerFeaturesMaxHistorySize = 20;
+
struct LocationGraph
{
bool visuLocations = true;
@@ -112,6 +114,7 @@ namespace armarx::navigation::components::navigation_memory
bool visuHumans = true;
bool visuTransparent = false;
+ int visuHumanMaxAgeMs = 1000;
bool visuRooms = true;
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index c450b9534bc6574c743bd1287f366d7d29f549f2..06c56ea0bc2f726c18c7a2ca3662fad0a1a97797 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -156,22 +156,24 @@ namespace armarx::navigation::memory
// cylinder.radius(300);
// layer.add(cylinder);
+ core::Pose human3d = conv::to3D(human.pose) * human_T_mmm;
viz::Robot mmm(std::to_string(layer.size()));
mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
- mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.pose(human3d);
mmm.scale(1.7); // 1.7m
mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
layer.add(mmm);
- core::Pose pose3d = conv::to3D(human.pose);
- pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
- auto arrow = viz::Arrow(std::to_string(layer.size()))
- .pose(pose3d)
- .length(200)
- .color(simox::Color::red());
- layer.add(arrow);
+ if(human.linearVelocity != Eigen::Vector2f::Zero()) {
+ core::Pose vel3d = human3d;
+ vel3d.translation().head<2>() += human.linearVelocity * 2;
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .fromTo(human3d.translation(), vel3d.translation())
+ .color(simox::Color::red());
+ layer.add(arrow);
+ }
}
}
@@ -285,7 +287,8 @@ namespace armarx::navigation::memory
void
Visu::drawHumans(std::vector<viz::Layer>& layers,
const bool enabled,
- const bool visuTransparent)
+ const bool visuTransparent,
+ const Duration maxAge)
{
if (not enabled)
{
@@ -293,6 +296,7 @@ namespace armarx::navigation::memory
}
std::map<std::string, navigation::human::Humans> namedProviderHumans;
+ const DateTime timestamp = Clock::Now();
humanSegment.doLocked(
[&]()
@@ -304,17 +308,23 @@ namespace armarx::navigation::memory
{
namedProviderHumans[entity.id().providerSegmentName];
entity.getLatestSnapshot().forEachInstance(
- [&namedProviderHumans](
+ [&namedProviderHumans, ×tamp, &maxAge](
const armarx::armem::wm::EntityInstance& instance)
{
- const auto dto =
- navigation::human::arondto::Human::FromAron(instance.data());
+ const Duration dtToNow =
+ timestamp - instance.metadata().referencedTime;
- navigation::human::Human human;
- fromAron(dto, human);
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto = navigation::human::arondto::Human::FromAron(
+ instance.data());
- namedProviderHumans[instance.id().providerSegmentName].emplace_back(
- std::move(human));
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName]
+ .emplace_back(std::move(human));
+ };
});
});
});
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 86515aa8987281682ec7125ad024d99f0fea4bc7..9ee945cde1d9dde91477322a1429bf154cd56ba6 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -57,7 +57,7 @@ namespace armarx::navigation::memory
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, bool visuTransparent);
+ void drawHumans(std::vector<viz::Layer>& layers, bool enabled, bool visuTransparent, Duration maxAge);
void drawRooms(std::vector<viz::Layer>& layers, bool enabled);
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 3943986f1cccd1496240f844b0d3f08a8f714478..dadc0c3b981cbdfb6bc18f77396dc56aaa5b9443 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -126,6 +126,7 @@ namespace armarx::navigation::components::navigator
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
addPlugin(humanReaderPlugin);
+ addPlugin(laserScannerFeaturesReaderPlugin);
addPlugin(virtualRobotReaderPlugin);
@@ -168,6 +169,7 @@ namespace armarx::navigation::components::navigator
.costmapReader = &costmapReaderPlugin->get(),
.virtualRobotReader = &virtualRobotReaderPlugin->get(),
.humanReader = &humanReaderPlugin->get(),
+ .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
.objectPoseClient = ObjectPoseClientPluginUser::getClient()};
sceneProvider.emplace(srv, params.sceneCfg);
@@ -235,7 +237,6 @@ namespace armarx::navigation::components::navigator
return "navigator";
}
-
void
Component::createConfig(const aron::data::dto::DictPtr& stackConfig,
const std::string& callerId,
@@ -271,6 +272,8 @@ namespace armarx::navigation::components::navigator
executorPtr = &executor.value();
}
+ // if we emplace an existing key it is not replaced, so remove it first.
+ navigators.erase(callerId);
navigators.emplace(
std::piecewise_construct,
std::forward_as_tuple(callerId),
@@ -293,7 +296,8 @@ namespace armarx::navigation::components::navigator
{
ARMARX_TRACE;
- ARMARX_INFO << "moveTo() requested by caller '" << callerId << "' with navigation mode `" << navigationMode << "`.";
+ ARMARX_INFO << "moveTo() requested by caller '" << callerId << "' with navigation mode `"
+ << navigationMode << "`.";
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
@@ -345,7 +349,6 @@ namespace armarx::navigation::components::navigator
core::NavigationFrameNames.from_name(navigationMode));
}
-
void
Component::moveTo2(const client::detail::Waypoints& waypoints,
const std::string& navigationMode,
@@ -363,13 +366,15 @@ namespace armarx::navigation::components::navigator
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
-
- void Component::moveToLocation(const std::string& location,
- const std::string& callerId,
- const Ice::Current& c)
+
+ void
+ Component::moveToLocation(const std::string& location,
+ const std::string& callerId,
+ const Ice::Current& c)
{
ARMARX_TRACE;
- ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId << "'";
+ ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId
+ << "'";
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
@@ -501,9 +506,12 @@ namespace armarx::navigation::components::navigator
"but won't execute it.");
def->required(params.sceneCfg.robotName, "p.scene.robotName");
- def->optional(params.sceneCfg.staticCostmapProviderName, "p.scene.staticCostmapProviderName");
+ def->optional(params.sceneCfg.staticCostmapProviderName,
+ "p.scene.staticCostmapProviderName");
def->optional(params.sceneCfg.staticCostmapName, "p.scene.staticCostmapName");
def->optional(params.sceneCfg.humanProviderName, "p.scene.humanProviderName");
+ def->optional(params.sceneCfg.laserScannerFeaturesProviderName,
+ "p.scene.laserScannerFeaturesProviderName");
return def;
}
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index aac593f1e2ef8bb2bf405bfa2dff65cffeff3e58..b8526d92dc78d976ee3e1a45686685fcc522d90c 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -51,11 +51,11 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <armarx/control/client/ComponentPlugin.h>
-
-#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
@@ -66,7 +66,6 @@
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
-
namespace armarx::navigation::components::navigator
{
@@ -115,13 +114,13 @@ namespace armarx::navigation::components::navigator
const Ice::Current& c = Ice::emptyCurrent) override;
void moveToLocation(const std::string& location,
- const std::string& callerId,
- const Ice::Current& c = Ice::emptyCurrent) override;
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current& c = Ice::emptyCurrent) override;
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void moveTowards(const Eigen::Vector3f& direction,
const std::string& navigationMode,
@@ -208,6 +207,8 @@ namespace armarx::navigation::components::navigator
costmapReaderPlugin = nullptr;
armem::client::plugins::ReaderWriterPlugin<memory::client::human::Reader>*
humanReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Reader>*
+ laserScannerFeaturesReaderPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
diff --git a/source/armarx/navigation/components/navigator/RemoteGui.cpp b/source/armarx/navigation/components/navigator/RemoteGui.cpp
index 42d9ad21404eb682d28809569947ea165337f33b..d801227848b010dd2a8a4b7999225fe48d4ff0b2 100644
--- a/source/armarx/navigation/components/navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/navigator/RemoteGui.cpp
@@ -15,18 +15,19 @@
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include "Component.h"
-#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/global_planning/SPFA.h>
+#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
+#include "Component.h"
+
namespace armarx::navigation::components::navigator
{
namespace gui = RemoteGui;
@@ -267,9 +268,12 @@ namespace armarx::navigation::components::navigator
tab.targetPoseGroup.targetPoseY.getValue(),
tab.targetPoseGroup.targetPoseZ.getValue()};
- const Eigen::Vector3f targetOri{tab.targetPoseGroup.targetPoseRoll.getValue(),
- tab.targetPoseGroup.targetPosePitch.getValue(),
- tab.targetPoseGroup.targetPoseYaw.getValue()};
+ const auto degToRad = [](float deg) { return deg * M_PI / 180.; };
+
+ const Eigen::Vector3f targetOri{
+ degToRad(tab.targetPoseGroup.targetPoseRoll.getValue()),
+ degToRad(tab.targetPoseGroup.targetPosePitch.getValue()),
+ degToRad(tab.targetPoseGroup.targetPoseYaw.getValue())};
const core::Pose targetPose = core::Pose(Eigen::Translation3f(targetPos)) *
core::Pose(simox::math::rpy_to_mat3f(targetOri));
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 172957d00e1adb5b42a1bb47ca9fea7f80083b17..66f3170748191d475ff0bd2088f04c78a56fea15 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -24,6 +24,7 @@
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::core
{
@@ -31,6 +32,7 @@ namespace armarx::navigation::core
struct DynamicScene
{
human::Humans humans;
+ std::vector<memory::LaserScannerFeatures> laserScannerFeatures;
};
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
index baf14d078c65a1db0efb792bca330361d549cb52..cb4172d817c2ca2a5810c0bb967f2ba14528b2da 100644
--- a/source/armarx/navigation/human/HumanFilter.cpp
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -3,7 +3,7 @@
namespace armarx::navigation::human
{
- HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
+ HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter) : useKalmanFilter(useKalmanFilter)
{
//initialize new human for detected human
human.pose = pose0;
@@ -11,29 +11,31 @@ namespace armarx::navigation::human
human.detectionTime = detectionTime;
lastDetectedHuman = human;
- //initialize new kalman filter for new detected human
- UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
- UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
- Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
- Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
+ if (useKalmanFilter) {
+ //initialize new kalman filter for new detected human
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
+ Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
- UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
- R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
- R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
+ R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
- UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
- P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
- P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
+ P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
- UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
- //initial position and orientation according to detected human
- SystemModelT::StateT state0 = toUkfState(pose0);
+ //initial position and orientation according to detected human
+ SystemModelT::StateT state0 = toUkfState(pose0);
- ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ }
}
void
@@ -41,32 +43,45 @@ namespace armarx::navigation::human
{
double dt = (detectionTime - human.detectionTime).toSecondsDouble();
- SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
- ukf->propagation(control, dt);
+ if (useKalmanFilter) {
+ SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
+ ukf->propagation(control, dt);
- human.pose = fromUkfState(ukf->getCurrentState());
- human.detectionTime = detectionTime;
+ human.pose = fromUkfState(ukf->getCurrentState());
+ human.detectionTime = detectionTime;
+ } else {
+ human.pose = human.estimateAt(detectionTime);
+ human.detectionTime = detectionTime;
+ }
}
void
HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
{
double dt = (detectionTime - lastDetectedHuman.detectionTime).toSecondsDouble();
-
- //update ukf with new observation
- SystemModelT::ObsT observation = toUkfObs(pose);
- ukf->update(observation);
-
- core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
- newPose.linear() = pose.linear();
+ core::Pose2D newPose;
+
+ if (useKalmanFilter) {
+ //update ukf with new observation
+ SystemModelT::ObsT observation = toUkfObs(pose);
+ ukf->update(observation);
+
+ newPose = fromUkfState(ukf->getCurrentState());
+ newPose.linear() = pose.linear();
+ } else
+ {
+ newPose = pose;
+ }
// calculate velocity
- Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
- Eigen::Vector2f linVelocity = ds / dt;
- // apply exponential smoothing to velocity
- //TODO try other approaches?
- Eigen::Vector2f newVelocity =
- params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+ Eigen::Vector2f newVelocity = human.linearVelocity;
+ if (dt > 0) {
+ Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
+ Eigen::Vector2f linVelocity = ds / dt;
+ // apply exponential smoothing to velocity
+ //TODO try other approaches?
+ newVelocity = params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+ }
//update stored information about the human
human.detectionTime = detectionTime;
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index aaed6eecb901335156aa6ee281b87183d4acf21f..c252166c8fdab010262883c8cf0f9bcb3d2de682 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -50,7 +50,7 @@ namespace armarx::navigation::human
double initial_state_rot_cov = 0.01;
double alpha = 0.5;
- float velocityAlpha = 0.7;
+ float velocityAlpha = 0.5;
};
/**
@@ -59,7 +59,7 @@ namespace armarx::navigation::human
* @param pose0 the first known pose of the human
* @param detectionTime the point of detection
*/
- HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+ HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter);
/**
* @brief HumanFilter::propagation propagate the system model to the given point in time. This
@@ -105,6 +105,7 @@ namespace armarx::navigation::human
*/
Human human{};
std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf = nullptr;
+ bool useKalmanFilter;
};
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 7273cdd723e0ff9e6bd6ac98125418a3a4b904c7..b12d6f82bfe9af499afd11feb4b3c45caa1f002a 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -6,12 +6,12 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include <armarx/navigation/human/types.h>
#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/types.h>
+
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
-
namespace armarx::navigation::human
{
@@ -39,10 +39,11 @@ namespace armarx::navigation::human
{
//add new tracked human to list of tracked humans
trackedHumans.push_back(TrackedHuman{
- .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+ .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime, parameters.useKalmanFilter},
.trackingId = detectedHuman.trackingId,
.associatedCluster = nullptr,
- .associated = true});
+ .associated = true,
+ .confidence = parameters.confidenceCamera});
}
}
}
@@ -108,6 +109,7 @@ namespace armarx::navigation::human
posDistance.oldHuman->associatedCluster = nullptr;
posDistance.oldHuman->associated = true;
posDistance.newCluster->associated = true;
+ posDistance.oldHuman->confidence += parameters.confidenceLaser;
}
}
@@ -131,6 +133,7 @@ namespace armarx::navigation::human
human.associatedCluster = nullptr;
human.associated = true;
+ human.confidence += parameters.confidenceLaser;
}
}
@@ -160,7 +163,6 @@ namespace armarx::navigation::human
trackedHumans.clear();
}
-
HumanTracker::DetectedHuman
HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose)
@@ -326,6 +328,8 @@ namespace armarx::navigation::human
trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
trackedHuman->trackingId = detectedHuman->trackingId;
+
+ trackedHuman->confidence = parameters.confidenceCamera;
}
float
@@ -365,8 +369,10 @@ namespace armarx::navigation::human
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- // when the tracked human recieved no new measurement for too long, remove it
- if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge)
+ // when the tracked human recieved no new measurement for too long or
+ // wasn't detected by the camera for too long, remove it
+ if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge ||
+ human.confidence <= 0)
{
it = trackedHumans.erase(it);
}
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 1f27aed75375018ce613579ee07f4577564b4249..23583351c59549f041f9bcf76620b3634736d13c 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -24,20 +24,19 @@
#include <ArmarXCore/core/time.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
-#include <VisionX/libraries/armem_human/types.h>
-
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
+
+#include <VisionX/libraries/armem_human/types.h>
namespace armarx::navigation::human
{
- using Cluster = armem::vision::LaserScannerFeature;
-
+ using Cluster = memory::LaserScannerFeature;
/**
* @brief The HumanTracker class can be used to track and filter multiple humans. It hides
@@ -83,6 +82,7 @@ namespace armarx::navigation::human
std::optional<std::string> trackingId = std::nullopt;
AdvancedCluster* associatedCluster;
bool associated;
+ int confidence;
};
struct PosHumanDistance
@@ -114,7 +114,14 @@ namespace armarx::navigation::human
// alpha value from interval [0,1] to determine how much the current (and respectively
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
+
+ int confidenceCamera = 100;
+ int confidenceLaser = -2;
+
+ // whether to use the kalman filter inside the HumanFilter
+ bool useKalmanFilter = true;
};
+
/**
* @brief HumanTracker::update Updates the tracked humans with the human measurements from a camera. When a
* measurement is close enough to an existing tracked human, they are associated, otherwise a
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
index fed4c18cce4105e0ffba8a6023bcb68268ae4b41..084a92158da77cc1a706f0afc4a0d8b6b4e89838 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.h
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -37,7 +37,7 @@ namespace armarx::navigation::human
float intimateRadius = 400;
float personalRadius = 1000;
- float movementInfluence = 1;
+ float movementInfluence = 0.8;
// an offset applied to the personal zone in the coordinate system of the human
// a positive x-value means an offset to the right
// a positive y-value means an offset to the front
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index e16fffd374d72663e51c6c8609856b88565b68ad..f12d5a085152be66f16365e00e4434273f87a5d9 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -1,6 +1,6 @@
#include "TebObstacleManager.h"
-#include <ArmarXCore/core/logging/Logging.h>
+#include <SimoxUtility/algorithm/apply.hpp>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/local_planning/ros_conversions.h>
@@ -22,39 +22,63 @@ namespace armarx::navigation::local_planning
{
return container.size();
}
-
void
- TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
+ TebObstacleManager::addLaserScannerFeaturesObstacle(
+ const memory::LaserScannerFeatures& features,
+ viz::Layer* visLayer)
{
- auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
- const Eigen::Vector2d min = conv::toRos(bbox.getMin());
- const Eigen::Vector2d max = conv::toRos(bbox.getMax());
+ Eigen::Isometry3f global_T_frame = features.frameGlobalPose;
+ for (const auto& feature : features.features)
+ {
+ std::vector<Eigen::Vector2f> convexHull =
+ simox::alg::apply(feature.convexHull,
+ [&](const Eigen::Vector2f& point)
+ {
+ Eigen::Vector3f point3d = conv::to3D(point);
+ return conv::to2D(global_T_frame * point3d);
+ });
+
+ addPolygonObstacle(convexHull, visLayer);
+ }
+ }
- obst->pushBackVertex(min);
- obst->pushBackVertex(min.x(), max.y());
- obst->pushBackVertex(max);
- obst->pushBackVertex(max.x(), min.y());
+ void
+ TebObstacleManager::addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer)
+ {
+ auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
+ for (const auto& vertex : polygon)
+ {
+ obst->pushBackVertex(conv::toRos2D(vertex));
+ }
obst->finalizePolygon();
- obst->setUseForOptimization(true);
container.push_back(obst);
// visualize bounding box if layer is available
if (visLayer != nullptr)
{
- const Eigen::Vector3f min3d = conv::fromRos(min);
- const Eigen::Vector3f max3d = conv::fromRos(max);
-
visLayer->add(viz::Polygon("polygon_" + std::to_string(visualizationIndex++))
- .addPoint(min3d)
- .addPoint(Eigen::Vector3f(min3d.x(), max3d.y(), 0))
- .addPoint(max3d)
- .addPoint(Eigen::Vector3f(max3d.x(), min3d.y(), 0))
+ .points(conv::to3D(polygon))
.color(simox::Color::gray()));
}
}
+ void
+ TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
+ {
+ const Eigen::Vector2f min = conv::to2D(bbox.getMin());
+ const Eigen::Vector2f max = conv::to2D(bbox.getMax());
+
+ Polygon poly{};
+ poly.push_back(min);
+ poly.emplace_back(min.x(), max.y());
+ poly.push_back(max);
+ poly.emplace_back(max.x(), min.y());
+
+ addPolygonObstacle(poly, visLayer);
+ }
+
void
TebObstacleManager::addHumanObstacle(const human::Human& human, viz::Layer* visLayer)
{
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b8859c4bbcb8145c3e1bb159e3ba7490331b14e4..d61906a28ff92288d8596ac410826e0fdcd6d3c1 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -26,6 +26,7 @@
#include <armarx/navigation/human/ProxemicZoneCreator.h>
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
#include <teb_local_planner/obstacles.h>
namespace armarx::navigation::local_planning
@@ -35,6 +36,8 @@ namespace armarx::navigation::local_planning
class TebObstacleManager
{
public:
+ using Polygon = std::vector<Eigen::Vector2f>;
+
TebObstacleManager(teb_local_planner::ObstContainer& container) : container(container)
{
}
@@ -43,6 +46,9 @@ namespace armarx::navigation::local_planning
size_t size();
+ void addLaserScannerFeaturesObstacle(const memory::LaserScannerFeatures& features,
+ viz::Layer* visLayer = nullptr);
+ void addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer = nullptr);
void addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer = nullptr);
void addHumanObstacle(const human::Human& human, viz::Layer* visLayer = nullptr);
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 1430129d43e00729240f602cd8c7e597a369486e..7575087938b3750a8a264431406d01ba23bef3f0 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -23,6 +23,7 @@
#include <armarx/navigation/local_planning/ros_conversions.h>
// #include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
#include <armarx/navigation/local_planning/aron_conversions_teb.h>
+
#include <teb_local_planner/homotopy_class_planner.h>
namespace armarx::navigation::local_planning
@@ -52,7 +53,7 @@ namespace armarx::navigation::local_planning
setTebCostmap();
if (teb_costmap)
{
- ARMARX_VERBOSE << "Costmap available.";
+ ARMARX_INFO << "Costmap available.";
// TODO: where to put all the parameters
const human::ExponentialPenaltyModel penalty{
.minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
@@ -87,7 +88,6 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult>
TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
- TIMING_START(TEB_PLAN);
const core::Pose currentPose{scene.robot->getGlobalPose()};
// prune global trajectory
@@ -107,18 +107,35 @@ namespace armarx::navigation::local_planning
fillObstacles();
+ bool success;
try
{
- hcp_->plan(start, end, &velocity_start, !planToDest);
+ success = hcp_->plan(start, end, &velocity_start, !planToDest);
+ }
+ catch (...)
+ {
+ hcp_->clearPlanner();
+ ARMARX_ERROR << "Caugth exception while planning: " << GetHandledExceptionString();
+ return std::nullopt;
+ }
+
+ if (!success)
+ {
+ hcp_->clearPlanner();
+ ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
+ return std::nullopt;
}
- catch (std::exception& e)
+
+ if (hcp_->hasDiverged())
{
- ARMARX_ERROR << "Caugth exception while planning: " << e.what();
+ hcp_->clearPlanner();
+ ARMARX_WARNING << deactivateSpam(5) << "Planner has diverged!";
return std::nullopt;
}
if (hcp_->getTrajectoryContainer().empty())
{
+ hcp_->clearPlanner();
ARMARX_INFO << deactivateSpam(5) << "Did not find any trajectory!";
return std::nullopt;
}
@@ -155,7 +172,6 @@ namespace armarx::navigation::local_planning
arviz.value().commit(layer);
}
- TIMING_END_COMMENT_STREAM(TEB_PLAN, "Timer: teb planning", ARMARX_VERBOSE);
return {{.trajectory = best}};
}
@@ -188,6 +204,11 @@ namespace armarx::navigation::local_planning
{
obstManager.addHumanObstacle(obst, visPtr);
}
+
+ for (const auto& obst : scene.dynamicScene->laserScannerFeatures)
+ {
+ obstManager.addLaserScannerFeaturesObstacle(obst, visPtr);
+ }
}
if (arviz)
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index c03f9a2b8fb622325659fd65c4ac9016fd22bfaf..c3910e067f645d1a066c478c81f702735c7afff8 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -1,6 +1,12 @@
+armarx_add_aron_library(memory_aron
+ ARON_FILES
+ aron/LaserScannerFeatures.xml
+)
+
armarx_add_library(memory
SOURCES
#./memory.cpp
+ aron_conversions.cpp
client/stack_result/Writer.cpp
client/stack_result/Reader.cpp
client/parameterization/Reader.cpp
@@ -11,10 +17,13 @@ armarx_add_library(memory
client/costmap/Reader.cpp
client/human/Reader.cpp
client/human/Writer.cpp
+ client/laser_scanner_features/Reader.cpp
+ client/laser_scanner_features/Writer.cpp
client/rooms/Reader.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
+ aron_conversions.h
client/stack_result/Writer.h
client/stack_result/Reader.h
client/parameterization/Reader.h
@@ -25,12 +34,16 @@ armarx_add_library(memory
client/costmap/Reader.h
client/human/Reader.h
client/human/Writer.h
+ client/laser_scanner_features/Reader.h
+ client/laser_scanner_features/Writer.h
client/rooms/Reader.h
# ./client/events/Reader.h
+ types.h
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
armem
+ armarx_navigation::memory_aron
armarx_navigation::core
armarx_navigation::algorithms
armarx_navigation::graph
diff --git a/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f78dea48892fab0e2bb8a477eaedc2425a484fc8
--- /dev/null
+++ b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
@@ -0,0 +1,67 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::memory::arondto::Ellipsoid'>
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='radii'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::memory::arondto::Circle'>
+ <ObjectChild key='center'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeature">
+ <ObjectChild key="convexHull">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ <ObjectChild key="circle">
+ <armarx::navigation::memory::arondto::Circle/>
+ </ObjectChild>
+ <ObjectChild key="ellipsoid">
+ <armarx::navigation::memory::arondto::Ellipsoid/>
+ </ObjectChild>
+ <!-- <ObjectChild key="chain">
+ <armarx::navigation::memory::arondto::Chain/>
+ </ObjectChild> -->
+ <ObjectChild key="points">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeatures">
+ <ObjectChild key="frame">
+ <String/>
+ </ObjectChild>
+ <ObjectChild key="frameGlobalPose">
+ <Pose/>
+ </ObjectChild>
+ <ObjectChild key="features">
+ <List>
+ <armarx::navigation::memory::arondto::LaserScannerFeature />
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/memory/aron_conversions.cpp b/source/armarx/navigation/memory/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3b5510c4ad74f9e9f3f798761ef6229ad91144c
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.cpp
@@ -0,0 +1,92 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+
+#include "types.h"
+
+namespace armarx::navigation::memory
+{
+
+
+ /************ toAron ************/
+
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
+
+
+ // LaserScannerFeatures
+
+ void
+ toAron(arondto::Circle& dto, const Circle& bo)
+ {
+ dto.center = bo.center;
+ dto.radius = bo.radius;
+ }
+
+ void
+ toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
+ {
+ dto.globalPose = bo.pose.matrix();
+ dto.radii = bo.radii;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
+ {
+ toAron(dto.circle, bo.circle);
+ dto.convexHull = bo.convexHull;
+ toAron(dto.ellipsoid, bo.ellipsoid);
+ dto.points = bo.points;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::toAron(dto.features, bo.features);
+ }
+
+ void
+ fromAron(const arondto::Circle& dto, Circle& bo)
+ {
+ bo.center = dto.center;
+ bo.radius = dto.radius;
+ }
+
+ void
+ fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
+ {
+ bo.pose = dto.globalPose;
+ bo.radii = dto.radii;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
+ {
+ bo.convexHull = dto.convexHull;
+ fromAron(dto.circle, bo.circle);
+ fromAron(dto.ellipsoid, bo.ellipsoid);
+
+ // bo.chain = dto.chain;
+ bo.points = dto.points;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::fromAron(dto.features, bo.features);
+ }
+
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/memory/aron_conversions.h b/source/armarx/navigation/memory/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d01ac5501a1beeed79ee9072b4e1c4f84a03acf
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.h
@@ -0,0 +1,38 @@
+/*
+ * 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
+
+namespace armarx::navigation::memory
+{
+
+ struct LaserScannerFeatures;
+
+ namespace arondto
+ {
+ struct LaserScannerFeatures;
+ }
+
+ // LaserScannerFeatures
+ void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
+ void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
+
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
index b224149c4aeb3c4858530f19ed051700060fc179..3a55ffc42e32b532d98a759e81cafa7033228beb 100644
--- a/source/armarx/navigation/memory/client/human/Writer.h
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -25,7 +25,6 @@
#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>
@@ -51,12 +50,12 @@ namespace armarx::navigation::memory::client::human
~Writer() override;
bool store(const armarx::navigation::human::Humans& humans,
- // const std::string& name,
+ // 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& name,
const std::string& providerName,
const armem::Time& timestamp);
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bfc47031c994678731a143cdd961f3a813983dd3
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -0,0 +1,180 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+// ArmarXCore
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+
+#include <armarx/navigation/memory/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+ Reader::~Reader() = default;
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.laser_scanner_features.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment};
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ auto& sel = qb.coreSegments()
+ .withName(properties().coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName);
+
+ auto& entitySel = [&]() -> armem::client::query::EntitySelector&
+ {
+ if (query.name.empty())
+ {
+ ARMARX_DEBUG << "querying all entities";
+ return sel.entities().all();
+ }
+ return sel.entities().withName(query.name);
+ }();
+
+ entitySel.snapshots().beforeOrAtTime(query.timestamp);
+
+ return qb;
+ }
+
+ std::vector<LaserScannerFeatures>
+ asFeaturesList(const armem::wm::ProviderSegment& providerSegment)
+ {
+ if (providerSegment.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ return {};
+ }
+
+ // const auto convert = [](const auto& aronLaserScanStamped,
+ // const wm::EntityInstance& ei) -> LaserScanStamped
+ // {
+ // LaserScanStamped laserScanStamped;
+ // fromAron(aronLaserScanStamped, laserScanStamped);
+
+ // const auto ndArrayNavigator =
+ // aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
+
+ // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
+ // ARMARX_IMPORTANT << "4";
+
+ // return laserScanStamped;
+ // };
+
+ std::vector<LaserScannerFeatures> laserScannerFeatures;
+
+ // loop over all entities and their snapshots
+ providerSegment.forEachEntity(
+ [&](const armem::wm::Entity& entity)
+ {
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << entity.id();
+ }
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&](const armem::wm::EntityInstance& entityInstance)
+ {
+ if (const auto o =
+ armem::tryCast<arondto::LaserScannerFeatures>(entityInstance))
+ {
+ LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
+ fromAron(o.value(), f);
+ }
+ return true;
+ });
+ return true;
+ });
+
+ return laserScannerFeatures;
+ }
+
+ Reader::Result
+ Reader::queryData(const Query& query) const
+ {
+ const auto qb = buildQuery(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 {.features = {},
+ .status = Result::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 {.features = {},
+ .status = Result::Status::Error,
+ .errorMessage = "Provider segment " + query.providerName + " does not exist"};
+ }
+
+ // now create result from memory
+ const armem::wm::ProviderSegment& providerSegment =
+ qResult.memory.getCoreSegment(properties().coreSegmentName)
+ .getProviderSegment(query.providerName);
+
+ const auto features = asFeaturesList(providerSegment);
+
+ // const auto laserScans = asLaserScans(providerSegment);
+ // std::vector<std::string> sensors;
+ // providerSegment.forEachEntity(
+ // [&sensors](const wm::Entity& entity)
+ // {
+ // sensors.push_back(entity.name());
+ // return true;
+ // });
+
+ return {.features = features,
+ // .sensors = sensors,
+ .status = Result::Status::Success,
+ .errorMessage = ""};
+ }
+
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..6823d34f3cf2ec8c0a282aa779fd61cd89540463
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -0,0 +1,91 @@
+/*
+ * 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 <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+
+ /**
+ * @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 Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+
+ virtual ~Reader();
+
+ struct Query
+ {
+ std::string providerName;
+
+ std::string name; // sensor name
+
+ armarx::core::time::DateTime timestamp;
+
+ // std::vector<std::string> sensorList;
+ };
+
+ struct Result
+ {
+ std::vector<LaserScannerFeatures> features;
+
+ // std::vector<std::string> sensors;
+
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+
+ std::string errorMessage;
+ };
+
+ Result queryData(const Query& query) const;
+
+ protected:
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a60a4fb33de9709f96adefbb80e211d84d462d4
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -0,0 +1,83 @@
+#include "Writer.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+#include <armarx/navigation/memory/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+ Writer::~Writer() = default;
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.laser_scanner_features.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::LaserScannerFeaturesCoreSegment};
+ }
+
+ bool
+ Writer::store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ // const auto result = memoryWriter.addSegment(constants::memoryName,
+ // constants::LaserScannerFeaturesCoreSegment);
+
+ // if (not result.success)
+ // {
+ // ARMARX_ERROR << result.errorMessage;
+
+ // // TODO(fabian.reister): message
+ // return false;
+ // }
+
+ const auto entityID = armem::MemoryID()
+ .withMemoryName(properties().memoryName)
+ .withCoreSegmentName(properties().coreSegmentName)
+ .withProviderSegmentName(providerName)
+ .withEntityName(features.frame)
+ .withTimestamp(timestamp);
+
+ ARMARX_VERBOSE << "Memory id is " << entityID.str();
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ ARMARX_TRACE;
+
+ arondto::LaserScannerFeatures dto;
+ toAron(dto, features);
+
+ ARMARX_TRACE;
+
+ update.instancesData = {dto.toAron()};
+ update.referencedTime = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+
+ ARMARX_TRACE;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..93bc1d0b255eb9d421e431f210bbe17b494d1558
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.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/>.
+ *
+ * @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 <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+ /**
+ * @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;
+
+ virtual ~Writer();
+
+ bool store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 177f9730e047da14810c1d7ad5dcad2be60ef7ab..0df19cc8d2139d617f2283e806f39e42551b77c4 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,6 +33,7 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
+ inline const std::string LaserScannerFeaturesCoreSegment = "LaserScannerFeatures";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
diff --git a/source/armarx/navigation/memory/types.h b/source/armarx/navigation/memory/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..73935f916f4a5a513f420106c608d43aa675422a
--- /dev/null
+++ b/source/armarx/navigation/memory/types.h
@@ -0,0 +1,75 @@
+/*
+ * 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 <vector>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+namespace armarx::navigation::memory
+{
+
+ struct Ellipsoid
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+
+ Eigen::Vector2f radii = Eigen::Vector2f::Zero();
+ };
+
+ struct Circle
+ {
+ Eigen::Vector2f center = Eigen::Vector2f::Zero();
+ float radius = 0.F;
+ };
+
+ struct LaserScannerFeature
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ Points convexHull;
+
+ Circle circle;
+ Ellipsoid ellipsoid;
+
+ Chain chain;
+
+ Points points;
+ };
+
+ struct LaserScannerFeatures
+ {
+ // TODO(fabian.reister): framed pose
+ std::string frame;
+ Eigen::Isometry3f frameGlobalPose;
+
+ std::vector<LaserScannerFeature> features;
+
+
+ // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
+
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index e198a0c65e540a3cce49155de91cb56417c25988..b33b7ab5b61cc42416c695fd384fb51c02d0f096 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -1075,12 +1075,7 @@ namespace armarx::navigation::server
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
- if (not localPlan.has_value())
- {
- return;
- }
-
- srv.introspector->onLocalPlannerResult(localPlan.value());
+ srv.introspector->onLocalPlannerResult(localPlan);
}
void
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 89a0a92da7100a01e33d7fa58551d286c2622ae9..eaafd1b973846e230bbbb88ec065283ba93a0c0b 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -53,9 +53,14 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
+ ArvizIntrospector::onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result)
{
- drawLocalTrajectory(result.trajectory);
+ if (result)
+ drawLocalTrajectory(result.value().trajectory);
+ else
+ drawLocalTrajectory(std::nullopt);
+
arviz.commit(simox::alg::get_values(layers));
}
@@ -175,8 +180,21 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::drawLocalTrajectory(const core::LocalTrajectory& trajectory)
+ ArvizIntrospector::drawLocalTrajectory(const std::optional<core::LocalTrajectory>& input)
{
+ if (!input)
+ {
+ // no local trajectory found, remove old one
+ auto layer = arviz.layer("local_planner");
+ auto velLayer = arviz.layer("local_planner_velocity");
+
+ layers[layer.data_.name] = std::move(layer);
+ layers[velLayer.data_.name] = std::move(velLayer);
+ return;
+ }
+
+ const auto trajectory = input.value();
+
auto layer = arviz.layer("local_planner");
const std::vector<Eigen::Vector3f> points =
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index 3b2865150d485e2fa656b1a531d3090a6543317b..b97da8bce042efd853229a868b18eaa24834c414 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -54,10 +54,11 @@ namespace armarx::navigation::server
~ArvizIntrospector() override = default;
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result) override;
void onRobotPose(const core::Pose& pose) override;
-
+
void onGoal(const core::Pose& goal) override;
void success() override;
@@ -76,7 +77,7 @@ namespace armarx::navigation::server
private:
void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
- void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
+ void drawLocalTrajectory(const std::optional<core::LocalTrajectory>& trajectory);
void drawRawVelocity(const core::Twist& twist);
void drawSafeVelocity(const core::Twist& twist);
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 1b40320341548f2cdbd00119d1f6af282116d735..5498072447c33bef4feabdf0d3b3ebd4535a5892 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -43,7 +43,8 @@ namespace armarx::navigation::server
virtual void failure() = 0;
virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
- virtual void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) = 0;
+ virtual void
+ onLocalPlannerResult(const std::optional<local_planning::LocalPlannerResult>& result) = 0;
virtual void onRobotPose(const core::Pose& pose) = 0;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index c0b97ecf11fa7043030dee4d50c9df570e1e83c4..b9afc87d26dd49a94415da09334c319dcf955174 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -17,12 +17,12 @@ namespace armarx::navigation::server
}
void
- MemoryIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
+ MemoryIntrospector::onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result)
{
// TODO(fabian.reister): implement
}
-
void
MemoryIntrospector::onGoal(const core::Pose& goal)
{
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 55b5c4f91b35c717d15536e11fbaa3dcc872d5ec..ede5f470eabebbfbf5420b8db73958e7fef5dcc2 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -39,7 +39,8 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result) override;
void onRobotPose(const core::Pose& pose) override {}
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 85e0d6236157edfd07c907367abafa3de69b7863..3fa2eb0179e30da481c3897e2f6dbc62cea8bcf9 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -14,9 +14,9 @@
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/human/Reader.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/util/util.h>
-
namespace armarx::navigation::server::scene_provider
{
@@ -124,11 +124,19 @@ namespace armarx::navigation::server::scene_provider
core::DynamicScene
SceneProvider::getDynamicScene(const DateTime& timestamp) const
{
- const memory::client::human::Reader::Query query{.providerName = config.humanProviderName,
- .timestamp = timestamp,
- .maxAge = Duration::MilliSeconds(500)};
-
- return {.humans = srv.humanReader->queryHumans(query).humans};
+ const memory::client::human::Reader::Query queryHumans{
+ .providerName = config.humanProviderName,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
+
+ const memory::client::laser_scanner_features::Reader::Query queryFeatures{
+ .providerName = config.laserScannerFeaturesProviderName,
+ .name = {},
+ .timestamp = timestamp};
+
+ return {.humans = srv.humanReader->queryHumans(queryHumans).humans,
+ .laserScannerFeatures =
+ srv.laserScannerFeaturesReader->queryData(queryFeatures).features};
}
core::SceneGraph
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
index 1f21e85ead9be31ca561a925e2bcceff0be53567..04780c675a0190a1874a763cef557f6aa7b7ac48 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.h
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -35,23 +35,29 @@
#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
-
namespace armarx::navigation::memory::client
{
namespace graph
{
class Reader;
}
+
namespace costmap
{
class Reader;
}
+
namespace human
{
class Reader;
}
-} // namespace armarx::navigation::memory::client
+ namespace laser_scanner_features
+ {
+ class Reader;
+ }
+
+} // namespace armarx::navigation::memory::client
namespace armarx::navigation::server::scene_provider
{
@@ -72,6 +78,8 @@ namespace armarx::navigation::server::scene_provider
memory::client::human::Reader* humanReader;
+ memory::client::laser_scanner_features::Reader* laserScannerFeaturesReader;
+
objpose::ObjectPoseClient objectPoseClient;
};
@@ -83,6 +91,7 @@ namespace armarx::navigation::server::scene_provider
std::string staticCostmapName = "distance_to_obstacles";
std::string humanProviderName = "dynamic_scene_provider";
+ std::string laserScannerFeaturesProviderName = "dynamic_scene_provider";
};
SceneProvider(const InjectedServices& srv, const Config& config);