diff --git a/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt index 45355685ca5222a58641229a0c9d55f4277ac1d7..a90a8a25c64b237049db9bb16f848efd64b3a45a 100644 --- a/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt +++ b/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt @@ -7,6 +7,7 @@ set(COMPONENT_LIBS RobotAPICore RobotAPIInterfaces armem_skills + aronjsonconverter ) set(SOURCES diff --git a/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.cpp index 1eccd8aa474d8f38882f5600e2ad62844362753a..f5d4b5f47f2174b165604d55ad862826ad2a7497 100644 --- a/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.cpp +++ b/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.cpp @@ -2,9 +2,22 @@ #include "SkillProviderExample.h" +#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h> + namespace armarx::skills::example { - SkillProviderExample::SkillProviderExample() + SkillImplementation::Status HelloWorldSkill::execute(const aron::data::DictPtr& d) + { + ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" << + "I received the following data: \n" << + aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(d).dump(2) << "\n" << + "(executed at: " << IceUtil::Time::now() << ")"; + return SkillImplementation::Status::Succeeded; + } + + + SkillProviderExample::SkillProviderExample() : + SkillProviderComponentPluginUser() {} armarx::PropertyDefinitionsPtr SkillProviderExample::createPropertyDefinitions() @@ -23,7 +36,14 @@ namespace armarx::skills::example void SkillProviderExample::onConnectComponent() { - + // Add example skill + { + skills::SkillDescription helloWorldDesc; + helloWorldDesc.acceptedType = nullptr; + helloWorldDesc.documentation = "This skill logs a message on ARMARX_IMPORTANT"; + helloWorldDesc.name = "common/HelloWorld"; + addSkill(HelloWorldSkill(), helloWorldDesc); + } } void SkillProviderExample::onDisconnectComponent() @@ -33,24 +53,4 @@ namespace armarx::skills::example void SkillProviderExample::onExitComponent() {} - - SkillDescriptionMap SkillProviderExample::getSkills(const Ice::Current ¤t) - { - - } - - SkillStatusMap SkillProviderExample::getSkillExecutionStatus(const Ice::Current ¤t) - { - - } - - void SkillProviderExample::executeSkill(const SkillParametrization &skill, const Ice::Current ¤t) - { - - } - - SkillStatus SkillProviderExample::abortSkill(const std::string &skill, const Ice::Current ¤t) - { - - } } diff --git a/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.h index ba2b60e8246ebef8050ec4edacab5bef37877731..c9eb9026c6f957a204e0c211baa6007b5a0660ee 100644 --- a/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.h +++ b/source/RobotAPI/components/armem/client/SkillProviderExample/SkillProviderExample.h @@ -31,6 +31,13 @@ namespace armarx::skills::example { + // Skills: + class HelloWorldSkill : public SkillImplementation + { + public: + Status execute(const aron::data::DictPtr&) override; + }; + /** * @defgroup Component-ExampleClient ExampleClient * @ingroup RobotAPI-Components @@ -61,14 +68,6 @@ namespace armarx::skills::example void onDisconnectComponent() override; void onExitComponent() override; - // Skills: - void helloWorld(const aron::data::DictPtr&); - void logMessage(const aron::data::DictPtr&); - private: - - struct Properties - { - } p; - } + }; } diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp index 4f0c987addea145e8aada2f5d942bb51256d8062..f5dc4c7a97aaccf9a2d9da50a67b8c4698de2570 100644 --- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp +++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp @@ -22,6 +22,8 @@ #include "SkillsMemory.h" +#include <SimoxUtility/algorithm/string.h> + #include <ArmarXCore/core/ArmarXManager.h> #include <ArmarXCore/core/ArmarXObjectScheduler.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> @@ -75,15 +77,13 @@ namespace armarx { statechartListenerProviderSegment.init(); executableSkillsCoreSegment.init(); - - createRemoteGuiTab(); - RemoteGui_startRunningTask(); } void SkillsMemory::onConnectComponent() { - + createRemoteGuiTab(); + RemoteGui_startRunningTask(); } @@ -114,6 +114,40 @@ namespace armarx return result; } + void SkillsMemory::addProvider(const std::string& name, const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) + { + const auto [skillName, providerName] = skills::segment::ExecutableSkillLibraryCoreSegment::GetSkillProviderNames(name); + + SkillObserverComponentPluginUser::addProvider(name, provider, current); + executableSkillsCoreSegment.addSkillProvider(providerName, skillName, provider); + } + + void SkillsMemory::removeProvider(const std::string& name, const Ice::Current ¤t) + { + std::vector<std::string> names = simox::alg::split(name, "/"); + ARMARX_CHECK_EXPRESSION(name.size() > 1); + + SkillObserverComponentPluginUser::removeProvider(name, current); + + const std::string providerName = names[0]; + names.erase(names.begin()); + const std::string skillName = simox::alg::join(names, ""); + + executableSkillsCoreSegment.removeSkillProvider(providerName, skillName); + } + + void SkillsMemory::executeSkill(const std::string& name, const skills::SkillParametrization& params, const Ice::Current ¤t) + { + std::vector<std::string> names = simox::alg::split(name, "/"); + ARMARX_CHECK_EXPRESSION(name.size() > 1); + + const std::string providerName = names[0]; + names.erase(names.begin()); + const std::string skillName = simox::alg::join(names, ""); + + executableSkillsCoreSegment.directlyExecuteSkill(providerName, skillName); + } + void SkillsMemory::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& x, const Ice::Current&) { statechartListenerProviderSegment.reportStatechartTransitionWithParameters(x); diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h index d2d72ea4cfe2e1a40c18344006496935f9bad398..059619eff043f2a9bfce6f45e6b384295cc814cb 100644 --- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h +++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h @@ -70,9 +70,14 @@ namespace armarx /// @see armarx::ManagedIceObject::getDefaultName() std::string getDefaultName() const override; + // Override StatechartListener void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override; void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override; + // Override SkillObserver to add memory functions + void addProvider(const std::string&, const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) override; + void removeProvider(const std::string&, const Ice::Current ¤t) override; + void executeSkill(const std::string&, const skills::SkillParametrization& params, const Ice::Current ¤t) override; // WritingInterface interface armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override; diff --git a/source/RobotAPI/interface/skills/SkillObserverInterface.ice b/source/RobotAPI/interface/skills/SkillObserverInterface.ice index 2372f738365e6f1acca76b74673bf43baf98fcb2..81d6c4212c0e3dddd9a16aad2d6cc36717cdf1fc 100644 --- a/source/RobotAPI/interface/skills/SkillObserverInterface.ice +++ b/source/RobotAPI/interface/skills/SkillObserverInterface.ice @@ -30,8 +30,8 @@ module armarx { interface SkillObserverInterface { - void addProvider(SkillProviderInterface* provider); - void removeProvider(SkillProviderInterface* provider); + void addProvider(string providerName, SkillProviderInterface* provider); + void removeProvider(string providerName); void executeSkill(string providerName, SkillParametrization skill); }; diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice index 1bfbcc55a6e3e0bf9b8c66ebf44e3a2ed119e24a..a01ea8f9e75a64b86504ecfa0f46f7df4a1d61ff 100644 --- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice +++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice @@ -29,19 +29,22 @@ module armarx { module skills { + sequence<string> StringList; + struct SkillDescription { string name; + string documentation; aron::type::dto::AronObject acceptedType; }; dictionary<string, SkillDescription> SkillDescriptionMap; - module Execution { enum Status { Idle, + Scheduled, RunningButWaitingForDependencies, Running, Failed, diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt index 00e51e9e99a1fb0b518f88e5685bfa7069dc8667..b1f6578a664bfdc937da465ccb514d62adb6a9b7 100644 --- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt @@ -73,7 +73,9 @@ set(LIB_FILES plugins/ReadOnlyPluginUser.cpp plugins/ReadWritePluginUser.cpp - segment/Segment.cpp + segment/detail/SpecializedSegment.cpp + segment/SpecializedCoreSegment.cpp + segment/SpecializedProviderSegment.cpp segment/SpecializedSegment.cpp query_proc/base/BaseQueryProcessorBase.cpp @@ -145,7 +147,9 @@ set(LIB_HEADERS plugins/ReadOnlyPluginUser.h plugins/ReadWritePluginUser.h - segment/Segment.h + segment/detail/SpecializedSegment.h + segment/SpecializedCoreSegment.h + segment/SpecializedProviderSegment.h segment/SpecializedSegment.h query_proc.h diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp index b7d157c746d3b4c5dfc99f7bc91fddefe5405af9..d0f2f4c77863e680bc644a219370a41aac8f4a79 100644 --- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp +++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp @@ -4,7 +4,8 @@ #include <ArmarXCore/core/logging/Logging.h> -#include "RobotAPI/libraries/aron/core/Exception.h" +#include <RobotAPI/libraries/aron/core/data/variant/All.h> +#include <RobotAPI/libraries/aron/core/Exception.h> #include <RobotAPI/libraries/armem/core/Time.h> #include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h> diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt index 7b3c2964c035d9b6fddddb87895cded8759938b5..54482355002f280e19dbe768a6b754798f17830d 100644 --- a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt @@ -27,9 +27,6 @@ armarx_add_library( HEADERS aron_conversions.h #traj_conversions.h - - ARON_FILES - aron/Trajectory.xml ) diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp index 0dbe9da26ba0f180a5fb090540e9abb7eff45127..080766eafbaf6bc20e84a8fec0a0912ceac6cfc2 100644 --- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp +++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp @@ -57,7 +57,7 @@ std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem: Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2)); Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3)); poseMatrix.block<3, 1>(0, 3) = vec; - arondto::TSElement tselement; + armarx::arondto::TSElement tselement; tselement.timestep = element.first; tselement.pose = poseMatrix; trajectory.taskSpace.steps.push_back(tselement); diff --git a/source/RobotAPI/libraries/armem_skills/CMakeLists.txt b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt index 985a36b499d8fef1952b022db238026ec46f7fc1..5f3e01c944d704418d64a4eb1ee607f026ebaeab 100644 --- a/source/RobotAPI/libraries/armem_skills/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt @@ -10,7 +10,7 @@ armarx_add_library( ArmarXCoreObservers RobotAPI::Core - RobotAPI::armem + RobotAPI::armem_server SOURCES ./aron_conversions.cpp diff --git a/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.cpp index dfc4e4c661945e3ab02e387a675764cfcb75a7a0..9e10c893134d2f149fc2b53fd7ffea64df9f923d 100644 --- a/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.cpp +++ b/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.cpp @@ -13,8 +13,13 @@ namespace armarx::plugins { auto& p = parent<SkillProviderComponentPluginUser>(); p.getProxy(skillProvider, -1); + } - skillObserver->addProvider(skillProvider); + void SkillProviderComponentPlugin::postOnConnectComponent() + { + auto& p = parent<SkillProviderComponentPluginUser>(); + std::string providerName = p.getName(); + skillObserver->addProvider(providerName, skillProvider); } void SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) @@ -32,7 +37,7 @@ namespace armarx addPlugin(plugin); } - void SkillProviderComponentPluginUser::addSkill(const std::function<void(const aron::data::DictPtr&)>& fun, const skills::SkillDescription& desc) + void SkillProviderComponentPluginUser::addSkill(const skills::SkillImplementation& fun, const skills::SkillDescription& desc) { std::lock_guard l(skillsMutex); std::string skillName = desc.name; @@ -43,12 +48,19 @@ namespace armarx return; } - SkillImplementation skill(fun, desc); + SkillImplementationWrapper skill(fun, desc); skillImplementations.insert({skillName, skill}); } + void SkillProviderComponentPluginUser::addSkill(const skills::LambdaSkillImplementation::FunT& f, const skills::SkillDescription& desc) + { + skills::LambdaSkillImplementation lmbda(f); + addSkill(lmbda, desc); + } + skills::SkillDescriptionMap SkillProviderComponentPluginUser::getSkills(const Ice::Current ¤t) { + std::lock_guard l(skillsMutex); skills::SkillDescriptionMap skillDesciptions; for (const auto& [key, skillImplementation] : skillImplementations) { @@ -59,9 +71,11 @@ namespace armarx skills::SkillStatusMap SkillProviderComponentPluginUser::getSkillExecutionStatus(const Ice::Current ¤t) { + std::lock_guard l(skillsMutex); skills::SkillStatusMap skillStatus; for (const auto& [key, skillImplementation] : skillImplementations) { + std::lock_guard l(skillImplementation.skillStatusMutex); skillStatus.insert({key, skillImplementation.status}); } return skillStatus; @@ -69,19 +83,71 @@ namespace armarx void SkillProviderComponentPluginUser::executeSkill(const skills::SkillParametrization& params, const Ice::Current ¤t) { + std::lock_guard l(skillsMutex); std::string skillName = params.name; - if (const auto it = skillImplementations.find(skillName); it != skillImplementations.end()) - { + ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0); - } - else + auto& impl = skillImplementations.at(skillName); + + impl.task->join(); + impl.status.params = params.params; + impl.task->start(); + } + + skills::SkillStatus SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current ¤t) + { + std::lock_guard l(skillsMutex); + if (skillImplementations.count(skillName)) { - ARMARX_ERROR << "Could not execute the skill '" << skillName << "' because it is not in the skill list of skill provider '" << getName() << "'."; + auto& impl = skillImplementations.at(skillName); + if (impl.task->isRunning()) + { + impl.task->stop(); + + std::lock_guard l(impl.skillStatusMutex); + impl.status.status = skills::Execution::Aborted; + } + return skillImplementations.at(skillName).status; } + return {}; } - skills::SkillStatus SkillProviderComponentPluginUser::abortSkill(const std::string &skill, const Ice::Current ¤t) + void SkillProviderComponentPluginUser::SkillImplementationWrapper::execute() { - + try + { + // wait for dependencies + { + std::lock_guard l(skillStatusMutex); + status.status = skills::Execution::RunningButWaitingForDependencies; + } + auto initialized = skill.init(); + + if (initialized != skills::SkillImplementation::Status::Succeeded) + { + status.error = "Could not initialize the skill."; + { + std::lock_guard l(skillStatusMutex); + status.status = skills::Execution::Failed; + } + } + + // execute + { + std::lock_guard l(skillStatusMutex); + status.status = skills::Execution::Running; + } + auto params = std::make_shared<aron::data::Dict>(status.params); + auto result = skill.execute(params); + { + std::lock_guard l(skillStatusMutex); + status.status = ((result == skills::SkillImplementation::Status::Succeeded) ? skills::Execution::Succeeded : skills::Execution::Failed); + } + } + catch (...) + { + std::lock_guard l(skillStatusMutex); + status.status = skills::Execution::Failed; + } } } diff --git a/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.h index 21c7681a26a5a67d5161555bdc5d62827f1599df..b3fa265318e41d10681d0287e04314402847058d 100644 --- a/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.h +++ b/source/RobotAPI/libraries/armem_skills/client/SkillProviderComponentPlugin.h @@ -1,6 +1,7 @@ #pragma once #include <mutex> +#include <queue> #include <functional> #include <ArmarXCore/core/ComponentPlugin.h> @@ -21,6 +22,7 @@ namespace armarx::plugins void preOnInitComponent() override; void preOnConnectComponent() override; + void postOnConnectComponent() override; void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; @@ -32,34 +34,101 @@ namespace armarx::plugins namespace armarx { + namespace skills + { + class SkillImplementation + { + public: + enum class Status + { + Succeeded, + TimeoutReached, + Failed + }; + + virtual Status init() + { + return Status::Succeeded; + } + + virtual Status execute(const aron::data::DictPtr&) + { + return Status::Succeeded; + } + + void reset() + { + skillStarted = 0; + } + + protected: + bool isTimeoutReached() const + { + if (timeoutMs < 0) + { + return false; + } + + auto now = IceUtil::Time::now().toMilliSeconds(); + return (now - skillStarted) >= timeoutMs; + } + + public: + long timeoutMs = -1; + + protected: + long skillStarted = 0; + }; + + class LambdaSkillImplementation : public SkillImplementation + { + public: + using FunT = std::function<bool(const aron::data::DictPtr&)>; + + LambdaSkillImplementation() = delete; + LambdaSkillImplementation(const FunT& f) : fun(f) {}; + + SkillImplementation::Status execute(const aron::data::DictPtr& data) override + { + bool res = fun(data); + return res ? Status::Succeeded : Status::Failed; + } + + private: + FunT fun; + }; + } + + class SkillProviderComponentPluginUser : virtual public ManagedIceObject, virtual public skills::SkillProviderInterface { public: - struct SkillImplementation + struct SkillImplementationWrapper { - using SkillFun = std::function<void(const aron::data::DictPtr&)>; - skills::SkillDescription description; - SkillFun skillFun; + skills::SkillImplementation skill; - // Execution status + // Current execution status skills::SkillStatus status; - // Mutex - mutable std::mutex skillMutex; + // Task + mutable std::mutex skillStatusMutex; + RunningTask<SkillImplementationWrapper>::pointer_type task; - SkillImplementation(const SkillFun& fun, const skills::SkillDescription& desc) : - description(desc), skillFun(fun) + SkillImplementationWrapper(const skills::SkillImplementation& fun, const skills::SkillDescription& desc) : + description(desc), skill(fun) { - status.name = description.name; status.status = skills::Execution::Status::Idle; + task = new RunningTask<SkillImplementationWrapper>(this, &SkillImplementationWrapper::execute, description.documentation); } - SkillImplementation(const SkillImplementation& s) : - SkillImplementation(s.skillFun, s.description) + SkillImplementationWrapper(const SkillImplementationWrapper& s) : + SkillImplementationWrapper(s.skill, s.description) {} + + void execute(); }; SkillProviderComponentPluginUser(); @@ -70,13 +139,14 @@ namespace armarx skills::SkillStatus abortSkill(const std::string &skill, const Ice::Current ¤t) override; protected: - void addSkill(const SkillImplementation::SkillFun&, const skills::SkillDescription&); + void addSkill(const skills::LambdaSkillImplementation::FunT&, const skills::SkillDescription&); + void addSkill(const skills::SkillImplementation&, const skills::SkillDescription&); private: armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr; protected: mutable std::mutex skillsMutex; - std::map<std::string, SkillImplementation> skillImplementations; + std::map<std::string, SkillImplementationWrapper> skillImplementations; }; } diff --git a/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.cpp b/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.cpp index b3afd0c65c3b19bc2498f5f0faa166794294affd..479fd58433fa4ec1eaa37b00ef0984c8c6f2f7bd 100644 --- a/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.cpp +++ b/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.cpp @@ -22,22 +22,19 @@ namespace armarx addPlugin(plugin); } - - - void SkillObserverComponentPluginUser::addProvider(const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) + void SkillObserverComponentPluginUser::addProvider(const std::string& providerName, const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) { std::lock_guard l(skillProviderMapMutex); - std::string providerName = provider->ice_toString(); if (skillProviderMap.find(providerName) == skillProviderMap.end()) { + ARMARX_INFO << "Adding a provider with name '" << providerName << "'."; skillProviderMap.insert({providerName, provider}); } } - void SkillObserverComponentPluginUser::removeProvider(const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) + void SkillObserverComponentPluginUser::removeProvider(const std::string& providerName, const Ice::Current ¤t) { std::lock_guard l(skillProviderMapMutex); - std::string providerName = provider->ice_toString(); if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end()) { skillProviderMap.erase(it); diff --git a/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.h b/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.h index 075716e8c668351026f9068112b5762327ec1a35..cbd2e60e6ff9e3a73fe2648d3febb276a37ba6fe 100644 --- a/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.h +++ b/source/RobotAPI/libraries/armem_skills/server/SkillObserverComponentPlugin.h @@ -31,8 +31,8 @@ namespace armarx public: SkillObserverComponentPluginUser(); - void addProvider(const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) override; - void removeProvider(const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) override; + void addProvider(const std::string&, const skills::SkillProviderInterfacePrx& provider, const Ice::Current ¤t) override; + void removeProvider(const std::string&, const Ice::Current ¤t) override; void executeSkill(const std::string&, const skills::SkillParametrization& params, const Ice::Current ¤t) override; private: diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp index 3aed64b30fd5d47e4ae12d222af3f4b64ece3755..a2103b02b6914a834c0aa6d90e564dc86af8aa8f 100644 --- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp +++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp @@ -1,7 +1,44 @@ #include "ExecutableSkillLibrarySegment.h" +#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h> +#include <SimoxUtility/algorithm/string.h> + namespace armarx::skills::segment { + std::pair<std::string, std::string> ExecutableSkillLibraryCoreSegment::GetSkillProviderNames(const std::string& name) + { + std::vector<std::string> names = simox::alg::split(name, "/"); + ARMARX_CHECK_EXPRESSION(name.size() > 1); + + const std::string providerName = names[0]; + names.erase(names.begin()); + const std::string skillName = simox::alg::join(names, ""); + return {providerName, skillName}; + } + + ExecutableSkillLibraryProviderSegment::ExecutableSkillLibraryProviderSegment(const std::string& name, const SkillProviderInterfacePrx& prx, armem::server::MemoryToIceAdapter& iceMemory): + Base(iceMemory, name, "ExecutableSkill"), + skillProvider(prx) + { + ARMARX_IMPORTANT << "CREATED A SKILL"; + // add skills + auto skills = skillProvider->getSkills(); + for (const auto& [key, desc] : skills) + { + (void) key; + (void) desc; + } + } + + void ExecutableSkillLibraryProviderSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix) + { + } + + void ExecutableSkillLibraryProviderSegment::init() + { + Base::init(); + } + ExecutableSkillLibraryCoreSegment::ExecutableSkillLibraryCoreSegment(armem::server::MemoryToIceAdapter& iceMemory): Base(iceMemory, "ExecutableSkill") { @@ -15,4 +52,20 @@ namespace armarx::skills::segment { Base::init(); } + + void ExecutableSkillLibraryCoreSegment::addSkillProvider(const std::string& providerSegmentName, const std::string& skillName, const SkillProviderInterfacePrx& provider) + { + ARMARX_IMPORTANT << "ADDING A SKILL SEGMENT"; + providerSegments.emplace(providerSegmentName, ExecutableSkillLibraryProviderSegment{skillName, provider, iceMemory}); + } + + void ExecutableSkillLibraryCoreSegment::removeSkillProvider(const std::string& providerName, const std::string& skillName) + { + + } + + void ExecutableSkillLibraryCoreSegment::directlyExecuteSkill(const std::string& providerName, const std::string& skillName) + { + + } } diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h index 0f5383638324875245873bc3f691b4eb2a13c8d8..b5285da0667ca1826abb05f0ad458e1e1fdcf610 100644 --- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h +++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h @@ -8,9 +8,25 @@ #include <ArmarXCore/observers/ObserverObjectFactories.h> #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> +#include <RobotAPI/interface/skills/SkillProviderInterface.h> namespace armarx::skills::segment { + class ExecutableSkillLibraryProviderSegment : + public armem::server::segment::SpecializedProviderSegment + { + using Base = armem::server::segment::SpecializedProviderSegment; + + public: + ExecutableSkillLibraryProviderSegment(const std::string& name, const SkillProviderInterfacePrx&, armem::server::MemoryToIceAdapter& iceMemory); + + void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix); + void init(); + + private: + SkillProviderInterfacePrx skillProvider; + }; + class ExecutableSkillLibraryCoreSegment : public armem::server::segment::SpecializedCoreSegment { @@ -22,10 +38,13 @@ namespace armarx::skills::segment void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix); void init(); + void addSkillProvider(const std::string& providerSegmentName, const std::string& skillName, const SkillProviderInterfacePrx& provider); + void removeSkillProvider(const std::string& providerName, const std::string& skillName); + void directlyExecuteSkill(const std::string& providerName, const std::string& skillName); + + static std::pair<std::string, std::string> GetSkillProviderNames(const std::string& n); + private: - struct Properties - { - }; - Properties p; + std::map<std::string, ExecutableSkillLibraryProviderSegment> providerSegments; }; } diff --git a/source/RobotAPI/libraries/armem_system_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_system_state/CMakeLists.txt index 797d1dd826692b7ca313f03a21840384467a01c0..a08cdd53130f952d6ea5063104bd458248e3f539 100644 --- a/source/RobotAPI/libraries/armem_system_state/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_system_state/CMakeLists.txt @@ -10,7 +10,7 @@ armarx_add_library( ArmarXCoreObservers RobotAPI::Core - RobotAPI::armem + RobotAPI::armem_server SOURCES ./server/CPUSegment.cpp