diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp index fd15dc3bc0ec0343af1191c8db4bb7fac5ef5b69..dcd76e7d8f67e2f0a6d5006893a3182bdcd82508 100644 --- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp +++ b/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()