RT controller and simplification

The implementation is split such that the Navigator component only generates the trajectory that will subsequently be executed by a RT controller (see: platform_controller/PlatformTrajectory)

Note:

Currently, this cannot be merged as it depends on some unmerged features in armarx::control (branch: new_mp_controller_aron__memory_and_component_plugin__whole_body_control)

source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp

@@ -9,8 +9,8 @@
 #include <armarx/navigation/common/controller_types.h>
 #include <armarx/navigation/core/aron_conversions.h>
 #include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/platform_controller/controller_descriptions.h>
 #include <armarx/navigation/platform_controller/aron_conversions.h>
+#include <armarx/navigation/platform_controller/controller_descriptions.h>
 #include <armarx/navigation/platform_controller/json_conversions.h>
 
 
@@ -21,32 +21,47 @@ namespace armarx::navigation::server
         ControllerComponentPlugin& controllerComponentPlugin) :
         controllerPlugin(controllerComponentPlugin)
     {
+        ARMARX_TRACE;
         controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
             "armarx_navigation", "libarmarx_navigation_platform_controller.so");
 
         // make default configs available to the memory
         ARMARX_INFO << "Loading default configs";
         {
-            const std::string configBasePath =
-                PackagePath("armarx_navigation", "controller_config").toSystemPath();
+            // const std::filesystem::path configBasePath =
+            //     PackagePath("armarx_navigation", "controller_config").toSystemPath();
 
-            armarx::control::memory::config::parseAndStoreDefaultConfigs<
-                armarx::navigation::common::ControllerType::PlatformTrajectory>(
-                configBasePath, controllerComponentPlugin.configMemoryWriter());
+            ARMARX_TRACE;
+            // armarx::control::memory::config::parseAndStoreDefaultConfigs<
+            //     armarx::navigation::common::ControllerType::PlatformTrajectory>(
+            // "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
 
+            ARMARX_TRACE;
             ARMARX_INFO << "asdlfasfdlh";
         }
 
         // initialize controller
         ARMARX_INFO << "Initializing controller";
         {
+            ARMARX_TRACE;
             auto builder = controllerPlugin.createControllerBuilder<
                 armarx::navigation::common::ControllerType::PlatformTrajectory>();
 
-            auto ctrlWrapper = builder.create();
+            ARMARX_TRACE;
+            // FIXME remove withconfig
+            auto ctrlWrapper =
+                builder.withNodeSet("PlatformPlanning")
+                    .withConfig(
+                        "/home/fabi/workspace/armarx/skills/navigation/data/armarx_navigation/"
+                        "controller_config/PlatformTrajectory/default.json")
+                    .create();
+
+            ARMARX_TRACE;
+            ARMARX_CHECK(ctrlWrapper.has_value());
             ctrl.emplace(std::move(ctrlWrapper.value()));
         }
 
+        ARMARX_TRACE;
         ARMARX_CHECK(ctrl.has_value());
         ARMARX_INFO << "PlatformControllerExecutor: init done.";
     }
@@ -57,6 +72,9 @@ namespace armarx::navigation::server
     void
     PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
     {
+        ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
+                    << " points";
+
         toAron(ctrl->config.targets.trajectory, trajectory);
 
         // sends the updated config to the controller and stores it in the memory