Add test case for deletion of old humans

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

@@ -354,6 +354,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         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?
@@ -371,8 +372,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
         //TODO use clusters for obstacle creation
 
+        std::vector<human::Human> humans = humanTracker.getTrackedHumans();
+
+        ARMARX_INFO << "Detected " << humans.size() << " humans";
 
-        humanWriterPlugin->get().store(humanTracker.getTrackedHumans(), getName(), timestamp);
+        humanWriterPlugin->get().store(humans, getName(), timestamp);
     }
 
 

source/armarx/navigation/human/test/human_tracker_test.cpp

@@ -227,4 +227,21 @@ namespace armarx::navigation::human
         BOOST_CHECK(h.linearVelocity.x() - movementSpeedMmPerSec.x() < 0.05 * 1000);
         BOOST_CHECK(h.linearVelocity.y() - movementSpeedMmPerSec.y() < 0.05 * 1000);
     }
+
+
+    BOOST_AUTO_TEST_CASE(testRemoveOldHumans)
+    {
+        HumanTracker tracker = HumanTracker();
+        auto zero = Eigen::Vector3f(0.f, 0.f, 0.f);
+        auto measurements =
+            generateCamMeasurements(zero, zero, Eigen::Quaternionf::Identity(), 0, 1);
+        tracker.update(measurements.at(0));
+
+        BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
+
+        CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+        tracker.update(late);
+
+        BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);
+    }
 } // namespace armarx::navigation::human