From 56fdef2eeb23930f8493b4e80fa5d321241a1e70 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 21:29:51 +0200
Subject: [PATCH] server/execution
---
.../server/execution/ExecutorInterface.h | 2 +
.../execution/PlatformControllerExecutor.cpp | 50 +++++++++++++++++--
.../execution/PlatformControllerExecutor.h | 6 +--
3 files changed, 51 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index c4a38f29..ea77fd7a 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -18,6 +18,8 @@ namespace armarx::navigation::server
virtual ~ExecutorInterface() = default;
virtual void execute(const core::Trajectory& trajectory) = 0;
+
+ virtual void start() = 0;
virtual void stop() = 0;
};
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
index 90acc53b..ed6d813e 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -1,10 +1,17 @@
#include "PlatformControllerExecutor.h"
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/common/type.h>
+#include <armarx/control/memory/config/util.h>
#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/json_conversions.h>
namespace armarx::navigation::server
@@ -17,13 +24,31 @@ namespace armarx::navigation::server
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();
+
+ armarx::control::memory::config::parseAndStoreDefaultConfigs<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>(
+ configBasePath, controllerComponentPlugin.configMemoryWriter());
+
+ ARMARX_INFO << "asdlfasfdlh";
+ }
- auto builder = controllerPlugin.createControllerBuilder<
- armarx::navigation::common::ControllerType::PlatformTrajectory>();
+ // initialize controller
+ ARMARX_INFO << "Initializing controller";
+ {
+ auto builder = controllerPlugin.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>();
- ctrl.emplace(builder.create().value());
+ auto ctrlWrapper = builder.create();
+ ctrl.emplace(std::move(ctrlWrapper.value()));
+ }
ARMARX_CHECK(ctrl.has_value());
+ ARMARX_INFO << "PlatformControllerExecutor: init done.";
}
PlatformControllerExecutor::~PlatformControllerExecutor() = default;
@@ -32,5 +57,22 @@ namespace armarx::navigation::server
void
PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
{
+ toAron(ctrl->config.targets.trajectory, trajectory);
+
+ // sends the updated config to the controller and stores it in the memory
+ ctrl->updateConfig();
+ }
+
+ void
+ PlatformControllerExecutor::start()
+ {
+ // TODO check whether the controller must be resetted (trajectory)
+ ctrl->activate();
+ }
+
+ void
+ PlatformControllerExecutor::stop()
+ {
+ ctrl->deactivate();
}
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
index 1b2ec239..d256904c 100644
--- a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -33,9 +33,9 @@ namespace armarx::navigation::server
~PlatformControllerExecutor() override;
void execute(const core::Trajectory& trajectory) override;
- void stop() override{
- // FIXME implement
- };
+
+ void start() override;
+ void stop() override;
private:
std::optional<armarx::control::client::ControllerWrapper<
--
GitLab