human tracker skeleton

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

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
+        });
+
+
+
     }
 
 

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

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp

+#include "HumanTracker.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+  
+}

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h

+#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