From e10793f03da3cd9a7c6c157bc8d210dc56014922 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:39:34 +0200
Subject: [PATCH] server/execution: using RT-Controller instead

---
 .../server/execution/ExecutorInterface.h      |  5 +-
 .../execution/PlatformControllerExecutor.cpp  | 36 ++++++++++++++
 .../execution/PlatformControllerExecutor.h    | 48 +++++++++++++++++++
 3 files changed, 87 insertions(+), 2 deletions(-)
 create mode 100644 source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
 create mode 100644 source/armarx/navigation/server/execution/PlatformControllerExecutor.h

diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index e0ba451e..c4a38f29 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -2,7 +2,7 @@
 
 namespace armarx::navigation::core
 {
-    struct Twist;
+    class Trajectory;
 } // namespace armarx::navigation::core
 
 namespace armarx::navigation::server
@@ -17,7 +17,8 @@ namespace armarx::navigation::server
     public:
         virtual ~ExecutorInterface() = default;
 
-        virtual void move(const core::Twist& twist) = 0;
+        virtual void execute(const core::Trajectory& trajectory) = 0;
+        virtual void stop() = 0;
     };
 
 } // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
new file mode 100644
index 00000000..90acc53b
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -0,0 +1,36 @@
+#include "PlatformControllerExecutor.h"
+
+#include <armarx/control/client/ComponentPlugin.h>
+#include <armarx/control/common/type.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+
+
+
+namespace armarx::navigation::server
+{
+
+    PlatformControllerExecutor::PlatformControllerExecutor(
+        ControllerComponentPlugin& controllerComponentPlugin) :
+        controllerPlugin(controllerComponentPlugin)
+    {
+        controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
+            "armarx_navigation", "libarmarx_navigation_platform_controller.so");
+
+
+        auto builder = controllerPlugin.createControllerBuilder<
+            armarx::navigation::common::ControllerType::PlatformTrajectory>();
+
+        ctrl.emplace(builder.create().value());
+
+        ARMARX_CHECK(ctrl.has_value());
+    }
+
+    PlatformControllerExecutor::~PlatformControllerExecutor() = default;
+
+
+    void
+    PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
+    {
+    }
+} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
new file mode 100644
index 00000000..1b2ec239
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <armarx/control/client/ComponentPlugin.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/controller_descriptions.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
+
+namespace armarx::control::client
+{
+    class ComponentPlugin;
+}
+
+namespace armarx::navigation::server
+{
+
+    /**
+     * @brief The PlatformUnitExecutor class
+     *
+     * TODO: Should be renamed to whatever the new unit will be called which takes Mat4f
+     * as input instead of X/Y/Yaw, so that we have a generic interface.
+     */
+    class PlatformControllerExecutor : virtual public ExecutorInterface
+    {
+
+    public:
+        using ControllerComponentPlugin = armarx::control::client::ComponentPlugin;
+
+        PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
+        ~PlatformControllerExecutor() override;
+
+        void execute(const core::Trajectory& trajectory) override;
+        void stop() override{
+            // FIXME implement
+        };
+
+    private:
+        std::optional<armarx::control::client::ControllerWrapper<
+            armarx::navigation::common::ControllerType::PlatformTrajectory>>
+            ctrl;
+
+        ControllerComponentPlugin& controllerPlugin;
+    };
+
+} // namespace armarx::navigation::server
-- 
GitLab