Save WIP state of human tracker test

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

@@ -58,13 +58,15 @@ namespace armarx::navigation::human
         HumanTracker tracker = HumanTracker();
 
         Eigen::Vector3f initialPosition(0, 0, 2);
-        Eigen::Quaternionf orientation = quatFromEuler(0, 0, M_PI_2);
+        float orientation = M_PI_2;
+        Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
 
         std::int64_t timestepMs = 100;
         Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
         int cameraSteps = 10;
 
-        Eigen::Vector3f posDelta = movementSpeedMetPerSec * (timestepMs / 1000);
+        Eigen::Vector3f posDelta =
+            movementSpeedMetPerSec * (static_cast<float>(timestepMs) / 1000.0);
 
         std::vector<CamMm> camMeasurements = std::vector<CamMm>();
         for (int i = 0; i < cameraSteps; i++)
@@ -73,7 +75,7 @@ namespace armarx::navigation::human
 
             Eigen::Vector3f newPos = initialPosition + i * posDelta;
             FramedPosition headPosition = FramedPosition(newPos, "", "");
-            FramedOrientation headOrientation = FramedOrientation(orientation, "", "");
+            FramedOrientation headOrientation = FramedOrientation(orientationQuat, "", "");
             PoseKeypoint head = {.label = "HEAD",
                                  .confidence = 0.95,
                                  .positionGlobal = headPosition,
@@ -85,10 +87,26 @@ namespace armarx::navigation::human
             tracker.update(camMm);
         }
 
-        // TODO: now add a lasersensor measurement roughly at the next human pose
 
         DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
 
+        using Point = Eigen::Vector2f;
+
+        std::vector<Point> relOffsets = {Point(0.01, 0.003),
+                                         Point(-0.02, 0.007),
+                                         Point(-0.01, -0.01),
+                                         Point(0.015, 0.009),
+                                         Point(0.002, 0.001)};
+
+        Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
+        //Eigen::Vector3f leftFootPos = laserPos
+
+        std::vector<Point> leftFootPoints = std::vector<Point>();
+        for (Point& offset : relOffsets)
+        {
+            //leftFootPoints.emplace_back();
+        }
+
         std::vector<Cluster> clusters = std::vector<Cluster>();
         LaserMm laserMm;
         tracker.update(laserMm);