fixing trajectory members (sliding window)

source/Navigation/libraries/core/Trajectory.cpp

@@ -301,8 +301,13 @@ namespace armarx::nav::core
         // the current / last added point to the resampled path
         Position pointOnPath = originalPath.front().translation();
 
-        for (const auto& [wp1, wp2] : originalPath | ranges::views::sliding(2))
+        for (const auto& wp : originalPath | ranges::views::sliding(2))
         {
+            //             const auto& [wp1, wp2] = wp;
+
+            const auto& [wp1, _] = wp;
+            const auto wp2 = wp1 + 1;
+
             ARMARX_TRACE;
 
             ARMARX_INFO << "sliding ...";
@@ -380,7 +385,7 @@ namespace armarx::nav::core
             pts.front().waypoint.pose,
             resampledPath,
             pts.back().waypoint.pose,
-            0.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+            300.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
     }
 
     float
@@ -390,7 +395,8 @@ namespace armarx::nav::core
 
         const auto distanceBetweenWaypoints = [](const auto& p) -> float
         {
-            const auto& [p1, p2] = p;
+            const auto& [p1, _] = p;
+            const auto p2 = p1 + 1;
             return (p1->waypoint.pose.translation() - p2->waypoint.pose.translation()).norm();
         };
 

source/Navigation/libraries/core/test/coreTest.cpp

@@ -34,6 +34,18 @@
 
 #include <Navigation/Test.h>
 
+BOOST_AUTO_TEST_CASE(testPathLength)
+{
+    armarx::nav::core::Path path{armarx::nav::core::Pose(Eigen::Translation3f(0, 0, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0))};
+
+    const auto traj = armarx::nav::core::Trajectory::FromPath(path, 100);
+    BOOST_CHECK_CLOSE(traj.length(), 2000, 0.1);
+}
+
 BOOST_AUTO_TEST_CASE(testResampleAlongLine)
 {
     armarx::nav::core::Path path{armarx::nav::core::Pose(Eigen::Translation3f(0, 0, 0)),

source/Navigation/libraries/global_planning/AStar.cpp

@@ -135,6 +135,6 @@ namespace armarx::nav::glob_plan
         // - check if position could be optimized gently (this might require resampling during optimization)
 
         ARMARX_TRACE;
-        return std::make_shared<core::Trajectory>(trajectory);
+        return std::make_shared<core::Trajectory>(resampledTrajectory);
     }
 } // namespace armarx::nav::glob_plan