Implement HumanTracker::mostRecentTrackedHumans()

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

@@ -49,6 +49,32 @@ namespace armarx::navigation::components::dynamic_scene_provider
     {
     }
 
+    std::vector<human::Human>
+    HumanTracker::mostRecentTrackedHumans() const
+    {
+        if (trackedHumans.empty())
+        {
+            return {};
+        }
+
+        std::sort(trackedHumans.begin(),
+                  trackedHumans.end(),
+                  [](human::Human a, human::Human b) -> bool
+                  { return a.detectionTime.operator<=(b.detectionTime); });
+        DateTime mostRecentTime = trackedHumans.back().detectionTime;
+
+        std::vector<human::Human> mostRecentHumans;
+
+        for (human::Human trackedHuman : trackedHumans)
+        {
+            if (trackedHuman.detectionTime == mostRecentTime)
+            {
+                mostRecentHumans.push_back(trackedHuman);
+            }
+        }
+
+        return mostRecentHumans;
+    }
 
     void
     HumanTracker::reset()