feature: Implement new interface after discussion.

source/Navigation/components/Navigator/Navigator.cpp

@@ -21,7 +21,13 @@
  */
 
 #include "Navigator.h"
+
 #include <iterator>
+
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
 #include <Eigen/src/Geometry/Transform.h>
 
 #include "ArmarXCore/core/services/tasks/TaskUtil.h"
@@ -83,55 +89,49 @@ namespace armarx::nav::components
         return p;
     }
 
-    void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
-                           const aron::data::AronDictPtr& stackConfig,
-                           const std::string& navigationMode,
-                           const Ice::Current&)
+    std::string
+    Navigator::createConfig(const aron::data::AronDictPtr& stackConfig,
+                            const Ice::Current&)
     {
+        // TODO: Not thread-safe.
+        const std::string uuid = boost::uuids::to_string(boost::uuids::random_generator()());
+
         server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+        navigators.emplace(uuid, server::Navigator{stack, scene, executor});
 
-        if (navigator.has_value())
-        {
-            navigator->stop();
-            navigator.reset();
-        }
+        return uuid;
+    }
 
-        if (not navigator.has_value())
-        {
-            navigator.emplace(stack, scene, executor);
-        }
+    void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+                           const std::string& navigationMode,
+                           const std::string& configId,
+                           const Ice::Current&)
+    {
+        // TODO: Error handling.
 
-        navigator->moveTo(convert(waypoints), core::NavigationFramesMap.from_name(navigationMode));
+        navigators.at(configId).moveTo(convert(waypoints), core::NavigationFramesMap.from_name(navigationMode));
     }
 
     void Navigator::moveTowards(const Eigen::Vector3f& direction,
-                                const aron::data::AronDictPtr& stackConfig,
                                 const std::string& navigationMode,
+                                const std::string& configId,
                                 const Ice::Current&)
     {
-        server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+        // TODO: Error handling.
 
-        if (navigator.has_value())
-        {
-            navigator->stop();
-            navigator.reset();
-        }
+        navigators.at(configId).moveTowards(direction, core::NavigationFramesMap.from_name(navigationMode));
+    }
 
-        if (not navigator.has_value())
-        {
-            navigator.emplace(stack, scene, executor);
-        }
+    void
+    Navigator::pauseMovement(const Ice::Current&)
+    {
 
-        navigator->moveTowards(direction, core::NavigationFramesMap.from_name(navigationMode));
     }
 
-    void Navigator::stop(const Ice::Current&)
+    void
+    Navigator::resumeMovement(const Ice::Current&)
     {
-        if (navigator.has_value())
-        {
-            navigator->stop();
-            navigator.reset();
-        }
+
     }
 
     armarx::PropertyDefinitionsPtr Navigator::createPropertyDefinitions()

source/Navigation/components/Navigator/Navigator.h

@@ -75,17 +75,22 @@ namespace armarx::nav::components
         /// @see armarx::ManagedIceObject::getDefaultName()
         std::string getDefaultName() const override;
 
+        std::string createConfig(const aron::data::AronDictPtr& stackConfig,
+                                 const Ice::Current&) override;
+
         void moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
-                    const aron::data::AronDictPtr& config,
                     const std::string& navigationMode,
+                    const std::string& configId,
                     const Ice::Current&) override;
 
         void moveTowards(const Eigen::Vector3f& direction,
-                         const aron::data::AronDictPtr& config,
                          const std::string& navigationMode,
+                         const std::string& configId,
                          const Ice::Current&) override;
 
-        void stop(const Ice::Current&) override;
+        void pauseMovement(const Ice::Current&) override;
+
+        void resumeMovement(const Ice::Current&) override;
 
     protected:
         /// @see PropertyUser::createPropertyDefinitions()
@@ -118,7 +123,7 @@ namespace armarx::nav::components
 
         core::Scene scene;
         server::PlatformUnitExecutor executor;
-        std::optional<server::Navigator> navigator;
+        std::map<std::string, server::Navigator> navigators;
 
     };
 } // namespace armarx::nav::components

source/Navigation/components/Navigator/NavigatorInterface.ice

@@ -38,9 +38,11 @@ module armarx
 
             interface NavigatorInterface
             {
-                void moveTo(Eigen::Matrix4fSeq waypoints, aron::data::AronDict config, string navigationFrame);
-                void moveTowards(Eigen::Vector3f direction, aron::data::AronDict config, string navigationFrame);
-                idempotent void stop();
+                string createConfig(aron::data::AronDict config);
+                void moveTo(Eigen::Matrix4fSeq waypoints, string navigationFrame, string configId);
+                void moveTowards(Eigen::Vector3f direction, string navigationFrame, string configId);
+                idempotent void pauseMovement();
+                idempotent void resumeMovement();
             };
 
         };

source/Navigation/libraries/server/Navigator.cpp

@@ -40,7 +40,7 @@ namespace armarx::nav::server
     Navigator::Navigator(const server::NavigationStack& navigationStack,
                          const core::Scene& scene,
                          ExecutorInterface& executor) :
-        stack(navigationStack), scene(scene), executor{executor}
+        stack(navigationStack), scene(&scene), executor{&executor}
     {
         setTag("Navigator");
     }
@@ -62,7 +62,7 @@ namespace armarx::nav::server
                 globalWaypoints.reserve(waypoints.size());
                 std::transform(std::begin(waypoints), std::end(waypoints), std::back_inserter(globalWaypoints), [&](const core::Pose & p)
                 {
-                    return core::Pose(scene.robot->getGlobalPose()) * p;
+                    return core::Pose(scene->robot->getGlobalPose()) * p;
                 });
                 break;
         }
@@ -99,7 +99,7 @@ namespace armarx::nav::server
                 res.safeVelocity = stack.safetyControl->control(res.controlVelocity.value());
             }
 
-            executor.move(res.velocity());
+            executor->move(res.velocity());
         }
     }
 

source/Navigation/libraries/server/Navigator.h

@@ -49,7 +49,9 @@ namespace armarx::nav::server
 
     class Navigator : public armarx::Logging
     {
+
     public:
+
         Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
         virtual ~Navigator();
 
@@ -61,11 +63,10 @@ namespace armarx::nav::server
 
         void stop();
 
-    protected:
     private:
         server::NavigationStack stack;
-        const core::Scene& scene;
+        const core::Scene* scene;
+        ExecutorInterface* executor;
 
-        ExecutorInterface& executor;
     };
 } // namespace armarx::nav::server

source/Navigation/libraries/server/execution/PlatformUnitExecutor.cpp

@@ -16,21 +16,7 @@ namespace armarx::nav::server
 
     void PlatformUnitExecutor::move(const core::Twist& twist)
     {
-        if (enabled)
-        {
-            platformUnit->move(twist.linear.x(), twist.linear.y(), twist.angular.z());
-        }
-    }
-
-    void PlatformUnitExecutor::disableAndStop()
-    {
-        enabled = false;
-        platformUnit->stopPlatform();
-    }
-
-    void PlatformUnitExecutor::enable()
-    {
-        enabled = true;
+        platformUnit->move(twist.linear.x(), twist.linear.y(), twist.angular.z());
     }
 
 } // namespace armarx::nav::server

source/Navigation/libraries/server/execution/PlatformUnitExecutor.h

@@ -22,15 +22,10 @@ namespace armarx::nav::server
 
         void move(const core::Twist& twist) override;
 
-        void disableAndStop();
-        void enable();
-
     private:
 
         PlatformUnitInterfacePrx platformUnit;
 
-        std::atomic_bool enabled{true};
-
     };
 
 }  // namespace armarx::nav::server