From a316ba1f2562be003bca7f58983e217ed5613e8d Mon Sep 17 00:00:00 2001
From: Fabian Peller-Konrad <fabian.peller-konrad@kit.edu>
Date: Mon, 6 Dec 2021 09:20:21 +0100
Subject: [PATCH] merge and skill observer
---
.../SkillProviderExample/CMakeLists.txt | 1 +
.../SkillProviderExample.cpp | 44 ++++-----
.../SkillProviderExample.h | 17 ++--
.../server/SkillsMemory/SkillsMemory.cpp | 42 +++++++-
.../armem/server/SkillsMemory/SkillsMemory.h | 5 +
.../skills/SkillObserverInterface.ice | 4 +-
.../skills/SkillProviderInterface.ice | 5 +-
.../libraries/armem/server/CMakeLists.txt | 8 +-
.../tree_visitors/TreeTypedJSONConverter.cpp | 3 +-
.../libraries/armem_mps/CMakeLists.txt | 3 -
.../MotionPrimitives/motionprimitives.cpp | 2 +-
.../libraries/armem_skills/CMakeLists.txt | 2 +-
.../client/SkillProviderComponentPlugin.cpp | 86 ++++++++++++++--
.../client/SkillProviderComponentPlugin.h | 98 ++++++++++++++++---
.../server/SkillObserverComponentPlugin.cpp | 9 +-
.../server/SkillObserverComponentPlugin.h | 4 +-
.../segment/ExecutableSkillLibrarySegment.cpp | 53 ++++++++++
.../segment/ExecutableSkillLibrarySegment.h | 27 ++++-
.../armem_system_state/CMakeLists.txt | 2 +-
19 files changed, 332 insertions(+), 83 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/SkillProviderExample/CMakeLists.txt
index 45355685c..a90a8a25c 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 1eccd8aa4..f5d4b5f47 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 ba2b60e82..c9eb9026c 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 4f0c987ad..f5dc4c7a9 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 d2d72ea4c..059619eff 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 2372f7383..81d6c4212 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 1bfbcc55a..a01ea8f9e 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 00e51e9e9..b1f6578a6 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 b7d157c74..d0f2f4c77 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 7b3c2964c..544823550 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 0dbe9da26..080766eaf 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 985a36b49..5f3e01c94 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 dfc4e4c66..9e10c8931 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 21c7681a2..b3fa26531 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 b3afd0c65..479fd5843 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 075716e8c..cbd2e60e6 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 3aed64b30..a2103b02b 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 0f5383638..b5285da06 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 797d1dd82..a08cdd531 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
--
GitLab