From 609ed1d468420f26a84b5cc804047f153d6b8e06 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Thu, 27 Oct 2022 15:13:11 +0200
Subject: [PATCH] Feed lasersensor results from reader to humanTracker
---
.../dynamic_scene_provider/Component.cpp | 19 ++++++++++++++++++-
1 file changed, 18 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 185d5568..4a177243 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -349,11 +349,28 @@ namespace armarx::navigation::components::dynamic_scene_provider
// here ends: data fetching
- ARMARX_INFO << "Running human tracker";
+ ARMARX_INFO << "Running human tracker with camera measurements";
humanTracker.update(human::HumanTracker::CameraMeasurement{
.detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+ ARMARX_INFO << "Running human tracker with lasersensor measurements";
+
+ //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});
+
+ ARMARX_INFO << "Human tracking done";
+
+
+ //TODO use clusters for obstacle creation
+
humanWriterPlugin->get().store(humanTracker.getTrackedHumans(), getName(), timestamp);
}
--
GitLab