update

source/Navigation/components/Navigator/Navigator.cpp

@@ -22,6 +22,8 @@
 
 #include "Navigator.h"
 
+#include <Eigen/src/Geometry/Transform.h>
+
 // Navigation
 #include "Navigation/libraries/core/types.h"
 #include <Navigation/libraries/factories/NavigationStackFactory.h>
@@ -53,24 +55,24 @@ std::string armarx::nav::components::Navigator::getDefaultName() const
 }
 
 void armarx::nav::components::Navigator::moveTo(const Eigen::Matrix4f& pose,
-        const aron::data::AronDictPtr& stackConfig,
-        const std::string& navigationMode,
-        const Ice::Current&)
+                                                const aron::data::AronDictPtr& stackConfig,
+                                                const std::string& navigationMode,
+                                                const Ice::Current&)
 {
 
     server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
-    server::Navigator navigator{stack};
+    server::Navigator navigator{stack, ctx};
 
-    navigator.moveTo(pose, core::NavigationFramesMap.from_name(navigationMode));
+    navigator.moveTo(core::Pose(pose), core::NavigationFramesMap.from_name(navigationMode));
 }
 
 void armarx::nav::components::Navigator::moveTowards(const Eigen::Matrix4f& direction,
-        const aron::data::AronDictPtr& stackConfig,
-        const std::string& navigationMode,
-        const Ice::Current&)
+                                                     const aron::data::AronDictPtr& stackConfig,
+                                                     const std::string& navigationMode,
+                                                     const Ice::Current&)
 {
     server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
-    server::Navigator navigator{stack};
+    server::Navigator navigator{stack, ctx};
 
     // navigator.moveTowards(direction); TODO
 }

source/Navigation/components/Navigator/test/NavigatorTest.cpp

@@ -24,11 +24,12 @@
 
 #define ARMARX_BOOST_TEST
 
-#include <Navigation/Test.h>
 #include "../Navigator.h"
 
 #include <iostream>
 
+#include <Navigation/Test.h>
+
 BOOST_AUTO_TEST_CASE(testExample)
 {
     armarx::nav::components::Navigator instance;

source/Navigation/libraries/algorithms/algorithms.h

@@ -22,7 +22,6 @@
 
 #pragma once
 
-
 namespace armarx
 {
     /**
@@ -39,7 +38,6 @@ namespace armarx
     class algorithms
     {
     public:
-
     };
 
-}
+} // namespace armarx

source/Navigation/libraries/algorithms/test/algorithmsTest.cpp

@@ -24,11 +24,12 @@
 
 #define ARMARX_BOOST_TEST
 
-#include <Navigation/Test.h>
 #include "../algorithms.h"
 
 #include <iostream>
 
+#include <Navigation/Test.h>
+
 BOOST_AUTO_TEST_CASE(testExample)
 {
 

source/Navigation/libraries/client/NavigationStackConfig.cpp

@@ -15,10 +15,9 @@ namespace armarx::nav::client
     NavigationStackConfig::configureGlobalPlanner(const glob_plan::GlobalPlannerParams& params)
     {
         aron::datanavigator::DictNavigatorPtr element(new aron::datanavigator::DictNavigator);
-        element->addElement(
-            core::NAME_KEY,
-            std::make_shared<aron::datanavigator::StringNavigator>(
-                glob_plan::AlgorithmNames.to_name(params.algorithm())));
+        element->addElement(core::NAME_KEY,
+                            std::make_shared<aron::datanavigator::StringNavigator>(
+                                glob_plan::AlgorithmNames.to_name(params.algorithm())));
         element->addElement(core::PARAMS_KEY, params.toAron());
 
         dict.addElement(core::GLOBAL_PLANNER, element);

source/Navigation/libraries/client/NavigationStackConfig.h

@@ -23,14 +23,15 @@
 
 #include <memory>
 
-
 #include "RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h"
 #include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
 #include <RobotAPI/interface/aron/Aron.h>
 #include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
 
 #include "Navigation/libraries/global_planning/GlobalPlanner.h"
-
+#include "Navigation/libraries/local_planning/LocalPlanner.h"
+#include "Navigation/libraries/safety_control/SafetyController.h"
+#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
 #include <type_traits>
 
 namespace armarx::nav::client
@@ -43,18 +44,18 @@ namespace armarx::nav::client
 
         NavigationStackConfig& configureGlobalPlanner(const glob_plan::GlobalPlannerParams& params);
 
-        NavigationStackConfig& configureLocalPlanner();
+        NavigationStackConfig& configureLocalPlanner(const loc_plan::LocalPlannerParams& params);
 
-        template <typename ParamType>
-        NavigationStackConfig& configureTrajectoryController();
+        NavigationStackConfig&
+        configureTrajectoryController(const traj_ctrl::TrajectoryControllerParams& params);
 
-        NavigationStackConfig& configureSafetyController();
+        NavigationStackConfig&
+        configureSafetyController(const safe_ctrl::SafetyControllerParams& params);
 
         aron::data::AronDictPtr toAron() const;
 
     protected:
     private:
         aron::datanavigator::DictNavigator dict;
-
     };
 } // namespace armarx::nav::client
\ No newline at end of file

source/Navigation/libraries/client/Navigator.h

@@ -25,9 +25,7 @@ namespace armarx::nav::client
     class Navigator
     {
     public:
-
     protected:
-
     private:
     };
-}  // namespace armarx::nav::client
\ No newline at end of file
+} // namespace armarx::nav::client
\ No newline at end of file

source/Navigation/libraries/client/test/clientTest.cpp

@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(testExample)
     const auto stackConfig = cfg.toAron();
     // here, we would send data over Ice ...
 
-    Eigen::Matrix4f goal;
+    core::Pose goal = core::Pose::Identity();
     Eigen::Vector3f direction;
 
     // NavigatorPrx navigator;
@@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE(testExample)
     core::Context ctx;
     server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
 
-    server::Navigator navigator(stack);
+    server::Navigator navigator(stack, ctx);
     // DirectionNavigator
     // RelativePointNavigator
 

source/Navigation/libraries/core/DynamicScene.h

@@ -28,9 +28,7 @@ namespace armarx::nav::core
     class DynamicScene
     {
     public:
-
     protected:
-
     private:
     };
 

source/Navigation/libraries/core/StaticScene.h

@@ -28,12 +28,10 @@ namespace armarx::nav::core
     class StaticScene
     {
     public:
-
     protected:
-
     private:
     };
 
     using StaticScenePtr = std::shared_ptr<StaticScene>;
 
-}  // namespace armarx::nav::core
\ No newline at end of file
+} // namespace armarx::nav::core
\ No newline at end of file

source/Navigation/libraries/core/TopologicScene.h

@@ -27,9 +27,7 @@ namespace armarx::nav::core
     class TopologicScene
     {
     public:
-
     protected:
-
     private:
     };
 } // namespace armarx::nav::core
\ No newline at end of file

source/Navigation/libraries/core/constants.h

@@ -24,13 +24,13 @@
 namespace armarx::nav::core
 {
 
-    const std::string NAME_KEY   = "name";
+    const std::string NAME_KEY = "name";
     const std::string PARAMS_KEY = "params";
 
     // TODO enum
-    const std::string GLOBAL_PLANNER        = "global_planner";
-    const std::string LOCAL_PLANNER         = "local_planner";
+    const std::string GLOBAL_PLANNER = "global_planner";
+    const std::string LOCAL_PLANNER = "local_planner";
     const std::string TRAJECTORY_CONTROLLER = "trajectory_controller";
-    const std::string SAFETY_CONTROLLER     = "safety_controller";
+    const std::string SAFETY_CONTROLLER = "safety_controller";
 
 } // namespace armarx::nav::core
\ No newline at end of file

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

@@ -24,11 +24,12 @@
 
 #define ARMARX_BOOST_TEST
 
-#include <Navigation/Test.h>
 #include "../core.h"
 
 #include <iostream>
 
+#include <Navigation/Test.h>
+
 BOOST_AUTO_TEST_CASE(testExample)
 {
 

source/Navigation/libraries/core/types.h

@@ -21,6 +21,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <SimoxUtility/meta/enum/EnumNames.hpp>
 #include <VirtualRobot/VirtualRobot.h>
 
@@ -38,18 +40,17 @@ namespace armarx::nav::core
         Absolute
     };
 
-    const static simox::meta::EnumNames<NavigationFrames> NavigationFramesMap
-    {
+    const static simox::meta::EnumNames<NavigationFrames> NavigationFramesMap{
         {NavigationFrames::Relative, "Relative"},
         {NavigationFrames::Absolute, "Absolute"}};
 
-    using Pose  = Eigen::Affine3f;
+    using Pose = Eigen::Affine3f;
     using Twist = Eigen::Vector6f;
 
     struct Waypoint
     {
         Pose pose;
-        std::optional<Twist> twist;
+        std::optional<Twist> twist = std::nullopt;
     };
 
     using Trajectory = std::vector<Waypoint>;

source/Navigation/libraries/factories/GlobalPlannerFactory.cpp

 #include "GlobalPlannerFactory.h"
-#include "Navigation/libraries/global_planning/AStar.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
 
 #include "Navigation/libraries/core/constants.h"
+#include "Navigation/libraries/global_planning/AStar.h"
+#include "Navigation/libraries/global_planning/Point2Point.h"
 #include "Navigation/libraries/global_planning/core.h"
 
-
 namespace armarx::nav::fac
 {
-    glob_plan::GlobalPlannerPtr GlobalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params, const core::Context& ctx)
+    glob_plan::GlobalPlannerPtr
+    GlobalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
+                                 const core::Context& ctx)
     {
-
-
-        glob_plan::Algorithms algo = glob_plan::Algorithms::AStar; // TODO from params / dict
-        aron::datanavigator::DictNavigatorPtr algoParams =
+        // algo name
+        const auto algoName =
+            aron::datanavigator::StringNavigator::DynamicCast(params->getElement(core::NAME_KEY));
+        ARMARX_CHECK_NOT_NULL(algoName);
+        const glob_plan::Algorithms algo =
+            glob_plan::AlgorithmNames.from_name(algoName->getValue());
+
+        // algo params
+        const auto algoParams =
             aron::datanavigator::DictNavigator::DynamicCast(params->getElement(core::PARAMS_KEY));
+        ARMARX_CHECK_NOT_NULL(algoParams);
 
         glob_plan::GlobalPlannerPtr globalPlanner;
-
         switch (algo)
         {
             case glob_plan::Algorithms::AStar:
-                globalPlanner = std::make_shared<glob_plan::AStar>(glob_plan::AStarParams::FromAron(algoParams), ctx);
+                globalPlanner = std::make_shared<glob_plan::AStar>(
+                    glob_plan::AStarParams::FromAron(algoParams), ctx);
+                break;
+            case glob_plan::Algorithms::Point2Point:
+                globalPlanner = std::make_shared<glob_plan::Point2Point>(
+                    glob_plan::Point2PointParams::FromAron(algoParams), ctx);
+                break;
         }
 
         return globalPlanner;

source/Navigation/libraries/factories/LocalPlannerFactory.cpp

@@ -2,8 +2,10 @@
 
 namespace armarx::nav::fac
 {
-    loc_plan::LocalPlannerPtr LocalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params)
+    loc_plan::LocalPlannerPtr
+    LocalPlannerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
+                                const core::Context& ctx)
     {
         return nullptr; // TODO
     }
-}
\ No newline at end of file
+} // namespace armarx::nav::fac
\ No newline at end of file

source/Navigation/libraries/factories/NavigationStackFactory.cpp

@@ -14,30 +14,29 @@ namespace armarx::nav::fac
                                    const core::Context& ctx)
     {
         const auto globalPlannerCfg = aron::datanavigator::DictNavigator::DynamicCast(
-                                          params->getElement(core::GLOBAL_PLANNER));
+            params->getElement(core::GLOBAL_PLANNER));
 
         const auto localPlannerCfg = aron::datanavigator::DictNavigator::DynamicCast(
-                                         params->getElement(core::LOCAL_PLANNER));
+            params->getElement(core::LOCAL_PLANNER));
 
         const auto trajectoryControllerCfg = aron::datanavigator::DictNavigator::DynamicCast(
-                params->getElement(core::TRAJECTORY_CONTROLLER));
+            params->getElement(core::TRAJECTORY_CONTROLLER));
 
         const auto safetyControllerCfg = aron::datanavigator::DictNavigator::DynamicCast(
-                                             params->getElement(core::SAFETY_CONTROLLER));
+            params->getElement(core::SAFETY_CONTROLLER));
 
-        return server::NavigationStack
-        {
-            .globalPlanner     = GlobalPlannerFactory::create(globalPlannerCfg, ctx),
-            .localPlanner      = LocalPlannerFactory::create(localPlannerCfg, ctx),
+        return server::NavigationStack{
+            .globalPlanner = GlobalPlannerFactory::create(globalPlannerCfg, ctx),
+            .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx),
             .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg, ctx),
-            .safetyControl     = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
+            .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
     }
 
     server::NavigationStack NavigationStackFactory::create(const aron::data::AronDictPtr& params,
-            const core::Context& ctx)
+                                                           const core::Context& ctx)
     {
-        const auto dict = aron::datanavigator::DictNavigator::FromAronDictPtr(params, ctx);
-        return create(dict);
+        const auto dict = aron::datanavigator::DictNavigator::FromAronDictPtr(params);
+        return create(dict, ctx);
     }
 
 } // namespace armarx::nav::fac
\ No newline at end of file

source/Navigation/libraries/factories/SafetyControllerFactory.cpp

@@ -2,8 +2,10 @@
 
 namespace armarx::nav::fac
 {
-    safe_ctrl::SafetyControllerPtr SafetyControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params, const core::Context& ctx)
+    safe_ctrl::SafetyControllerPtr
+    SafetyControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
+                                    const core::Context& ctx)
     {
         return nullptr; // TODO
     }
-}
\ No newline at end of file
+} // namespace armarx::nav::fac
\ No newline at end of file

source/Navigation/libraries/factories/TrajectoryControllerFactory.cpp

@@ -2,8 +2,10 @@
 
 namespace armarx::nav::fac
 {
-    traj_ctrl::TrajectoryControllerPtr TrajectoryControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params)
+    traj_ctrl::TrajectoryControllerPtr
+    TrajectoryControllerFactory::create(const aron::datanavigator::DictNavigatorPtr& params,
+                                        const core::Context& ctx)
     {
         return nullptr;
     }
-}
\ No newline at end of file
+} // namespace armarx::nav::fac
\ No newline at end of file

source/Navigation/libraries/factories/test/factoriesTest.cpp

@@ -24,11 +24,12 @@
 
 #define ARMARX_BOOST_TEST
 
-#include <Navigation/Test.h>
 #include "../factories.h"
 
 #include <iostream>
 
+#include <Navigation/Test.h>
+
 BOOST_AUTO_TEST_CASE(testExample)
 {