bugfix (not!)

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

@@ -38,12 +38,10 @@ 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))};
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 4000, 0))};
 
     const auto traj = armarx::nav::core::Trajectory::FromPath(path, 100);
-    BOOST_CHECK_CLOSE(traj.length(), 2000, 0.1);
+    BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
 }
 
 BOOST_AUTO_TEST_CASE(testResampleAlongLine)
@@ -56,24 +54,29 @@ BOOST_AUTO_TEST_CASE(testResampleAlongLine)
 
     const auto resampledTraj = traj.resample(500);
 
+    for (const auto& pt : resampledTraj.positions())
+    {
+        ARMARX_INFO << VAROUT(pt);
+    }
+
     BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
 }
 
-// BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
-// {
-//     armarx::nav::core::Path path{armarx::nav::core::Pose(Eigen::Translation3f(0, 0, 0)),
-//                                  armarx::nav::core::Pose(Eigen::Translation3f(0, 1000, 0)),
-//                                  armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0))};
+BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
+{
+    armarx::nav::core::Path path{armarx::nav::core::Pose(Eigen::Translation3f(0, 0, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 1000, 0)),
+                                 armarx::nav::core::Pose(Eigen::Translation3f(0, 2000, 0))};
 
-//     const auto traj = armarx::nav::core::Trajectory::FromPath(path, 100);
-//     BOOST_CHECK_EQUAL(traj.points().size(), 3);
+    const auto traj = armarx::nav::core::Trajectory::FromPath(path, 100);
+    BOOST_CHECK_EQUAL(traj.points().size(), 3);
 
-//     const auto resampledTraj = traj.resample(500);
+    const auto resampledTraj = traj.resample(500);
 
-//     for (const auto& pt : resampledTraj.positions())
-//     {
-//         ARMARX_INFO << VAROUT(pt);
-//     }
+    for (const auto& pt : resampledTraj.positions())
+    {
+        ARMARX_INFO << VAROUT(pt);
+    }
 
-//     BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
-// }
+    BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
+}

source/Navigation/libraries/server/Navigator.cpp

@@ -191,11 +191,13 @@ namespace armarx::nav::server
         if (isPaused() or isStopped())
         {
             // [[unlikely]]
+            ARMARX_INFO << deactivateSpam(1) << "stopped or paused";
             return;
         }
 
         if (not isStackResultValid())
         {
+            ARMARX_INFO << deactivateSpam(1) << "stack result invalid";
             // [[unlikely]]
             return;
         }
@@ -305,6 +307,8 @@ namespace armarx::nav::server
     void
     Navigator::pause()
     {
+        ARMARX_INFO << "pause";
+
         executorEnabled.store(false);
 
         ARMARX_CHECK_NOT_NULL(config.executor);
@@ -314,12 +318,16 @@ namespace armarx::nav::server
     void
     Navigator::resume()
     {
+        ARMARX_INFO << "resume";
+
         executorEnabled.store(true);
     }
 
     void
     Navigator::stop()
     {
+        ARMARX_INFO << "stop";
+
         pause();
         // TODO: Reset own state.
     }
@@ -327,7 +335,7 @@ namespace armarx::nav::server
     bool
     Navigator::isPaused() const noexcept
     {
-        return executorEnabled.load();
+        return not executorEnabled.load();
     }
 
     bool