From 5a5cf7af9691ffee16d214603c5788ffe02909c7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 3 Aug 2022 15:14:26 +0200
Subject: [PATCH] human tracker skeleton

---
 .../dynamic_scene_provider/CMakeLists.txt     |  2 +
 .../dynamic_scene_provider/Component.cpp      | 11 ++++++
 .../dynamic_scene_provider/Component.h        |  4 ++
 .../dynamic_scene_provider/HumanTracker.cpp   |  6 +++
 .../dynamic_scene_provider/HumanTracker.h     | 37 +++++++++++++++++++
 5 files changed, 60 insertions(+)
 create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
 create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h

diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 146e5b7b..42507ca0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -9,9 +9,11 @@ armarx_add_component(dynamic_scene_provider
     SOURCES
         Component.cpp
         ArVizDrawer.cpp
+        HumanTracker.cpp
     HEADERS
         Component.h
         ArVizDrawer.h
+        HumanTracker.h
     DEPENDENCIES
         # ArmarXCore
         ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9f8cd9f7..a4d10991 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -137,6 +137,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
         robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
         ARMARX_CHECK_NOT_NULL(robot);
 
+        humanTracker.reset();
+
         task = new PeriodicTask<Component>(
             this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
     }
@@ -314,6 +316,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
             // arviz.commit({layer});
         }
+
+        // here ends: data fetching
+        
+        humanTracker.update(HumanTracker::Measurements{
+            .humanPoses = humanPoseResult.humanPoses
+        });
+
+
+
     }
 
 
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 1d7d92fc..171b8638 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -47,6 +47,7 @@
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
 #include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
+#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
 #include "armarx/navigation/memory/client/costmap/Reader.h"
 #include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
 
@@ -197,6 +198,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
             occupancyGridReaderPlugin = nullptr;
+
+
+        HumanTracker humanTracker;
     };
 
 } // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
new file mode 100644
index 00000000..866fa4aa
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -0,0 +1,6 @@
+#include "HumanTracker.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+  
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
new file mode 100644
index 00000000..770901f1
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -0,0 +1,37 @@
+#pragma once
+
+
+#include "VisionX/libraries/armem_human/types.h"
+#include "armarx/navigation/core/basic_types.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+    class HumanTracker
+    {
+    public:
+        HumanTracker() = default;
+
+        struct Measurements
+        {
+            std::vector<armem::human::HumanPose> humanPoses;
+        };
+
+        void update(const Measurements& measurements);
+
+        struct TrackedHuman
+        {
+            // TODO ...
+            core::Pose2D global_T_human;
+
+            Eigen::Vector2f linearVelocity;
+        };
+
+        std::vector<TrackedHuman> getTrackedHumans() const;
+
+        void reset();
+
+    private:
+        std::vector<TrackedHuman> trackedHumans;
+    };
+} // namespace armarx::navigation::components::dynamic_scene_provider
-- 
GitLab