diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h index c4a38f29de96c727e12f5ef2e0fe1f2c92cfd04c..ea77fd7a6d2c41bd5ec7bc375a37cba1c2d1ec57 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 90acc53bc116ecb63ddf20b9a18441b9545a2398..ed6d813ebfefdd21dacf346d9f2ff9a055313c2f 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 1b2ec239553a3b0c397f47bd962355cd46fe84ff..d256904cefa0cee344faeea3bddc4b9197f70c60 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<