Merge branch 'master' of gitlab.com:ArmarX/Navigation

source/Navigation/libraries/server/Navigator.cpp

 #include "Navigator.h"
 
+
 #include <algorithm>
+
 #include <Eigen/src/Geometry/Transform.h>
 
 #include <VirtualRobot/Robot.h>
 
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <Navigation/libraries/core/types.h>
 
-#include "Navigation/libraries/core/types.h"
 
 namespace armarx::nav::server
 {
@@ -47,7 +50,7 @@ namespace armarx::nav::server
 
     Navigator::~Navigator()
     {
-        stop();
+        pauseMovement();
     }
 
     void Navigator::moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame)
@@ -99,23 +102,37 @@ namespace armarx::nav::server
                 res.safeVelocity = stack.safetyControl->control(res.controlVelocity.value());
             }
 
-            executor->move(res.velocity());
+            if (movementEnabled.load())
+            {
+                executor->move(res.velocity());
+            }
         }
     }
 
+
     void Navigator::moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame)
     {
 
     }
 
+
     void Navigator::moveTowardsAbsolute(const core::Direction& direction)
     {
 
     }
 
-    void Navigator::stop()
+
+    void Navigator::pauseMovement()
     {
+        movementEnabled.store(false);
+        const core::Twist zero{Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()};
+        executor->move(zero);
+    }
 
+
+    void Navigator::resumeMovement()
+    {
+        movementEnabled.store(true);
     }
 
 } // namespace armarx::nav::server

source/Navigation/libraries/server/Navigator.h

@@ -19,10 +19,15 @@
  *             GNU General Public License
  */
 
+
 #pragma once
 
+
+// STD/STL
+#include <atomic>
 #include <optional>
 
+// Eigen
 #include <Eigen/Core>
 
 // ArmarX
@@ -34,18 +39,11 @@
 #include "NavigationStack.h"
 #include <Navigation/libraries/server/execution/ExecutorInterface.h>
 
+
 namespace armarx::nav::server
 {
-    struct StackResult
-    {
-        core::TrajectoryPtr globalTrajectory;
-        core::TrajectoryPtr localTrajectory;
-        std::optional<core::Twist> controlVelocity;
-        std::optional<core::Twist> safeVelocity;
 
-        core::TrajectoryPtr trajectory() const;
-        core::Twist velocity() const;
-    };
+    struct StackResult;
 
     class Navigator : public armarx::Logging
     {
@@ -53,20 +51,40 @@ namespace armarx::nav::server
     public:
 
         Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
-        virtual ~Navigator();
 
         void moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame);
-        void moveToAbsolute(const std::vector<core::Pose>& waypoints);
-
         void moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame);
-        void moveTowardsAbsolute(const core::Direction& direction);
 
-        void stop();
+        void pauseMovement();
+        void resumeMovement();
+
+        // Non-API
+        Navigator(Navigator&& other) : stack{std::move(other.stack)}, scene{other.scene}, executor{other.executor}, movementEnabled{other.movementEnabled.load()} {}
+        virtual ~Navigator();
+
+    private:
+
+        void moveToAbsolute(const std::vector<core::Pose>& waypoints);
+        void moveTowardsAbsolute(const core::Direction& direction);
 
     private:
+
         server::NavigationStack stack;
         const core::Scene* scene;
         ExecutorInterface* executor;
+        std::atomic_bool movementEnabled = true;
 
     };
+
+    struct StackResult // TODO: Refactor into own file
+    {
+        core::TrajectoryPtr globalTrajectory;
+        core::TrajectoryPtr localTrajectory;
+        std::optional<core::Twist> controlVelocity;
+        std::optional<core::Twist> safeVelocity;
+
+        core::TrajectoryPtr trajectory() const;
+        core::Twist velocity() const;
+    };
+
 } // namespace armarx::nav::server