From bf28e9e5b2dc9678c7509f13ea513dbe0da3fc2e Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Fri, 6 Oct 2023 14:24:59 +0200
Subject: [PATCH] legacy support old skill api
---
.../libraries/skills/provider/CMakeLists.txt | 6 ++
.../libraries/skills/provider/SimpleSkill.cpp | 55 +++++++++++
.../libraries/skills/provider/SimpleSkill.h | 47 +++++++++
.../{legacy => }/SimpleSpecializedSkill.cpp | 2 +-
.../skills/provider/SimpleSpecializedSkill.h | 97 +++++++++++++++++++
.../provider/SkillProviderComponentPlugin.cpp | 13 ++-
.../provider/SkillProviderComponentPlugin.h | 23 ++++-
.../skills/provider/SpecializedSkill.h | 23 +++--
.../detail/SkillImplementationWrapper.cpp | 67 +++++++------
.../detail/SkillImplementationWrapper.h | 10 +-
.../skills/provider/legacy/SimpleSkill.cpp | 6 --
.../skills/provider/legacy/SimpleSkill.h | 15 ---
.../provider/legacy/SimpleSpecializedSkill.h | 15 ---
13 files changed, 293 insertions(+), 86 deletions(-)
create mode 100644 source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
create mode 100644 source/RobotAPI/libraries/skills/provider/SimpleSkill.h
rename source/RobotAPI/libraries/skills/provider/{legacy => }/SimpleSpecializedSkill.cpp (62%)
create mode 100644 source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
delete mode 100644 source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.cpp
delete mode 100644 source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.h
delete mode 100644 source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.h
diff --git a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
index 4ba71803f..e5df87081 100644
--- a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
@@ -19,17 +19,22 @@ armarx_add_library(
SkillProviderComponentPlugin.cpp
SkillFactory.cpp
LambdaSkill.cpp
+ SimpleSkill.cpp
+ SimpleSpecializedSkill.cpp
SkillProxy.cpp
PeriodicSkill.cpp
SpecializedSkill.cpp
PeriodicSpecializedSkill.cpp
detail/SkillImplementationWrapper.cpp
SkillContext.cpp
+
HEADERS
SkillProviderComponentPlugin.h
SkillFactory.h
LambdaSkill.h
+ SimpleSkill.h
+ SimpleSpecializedSkill.h
SkillProxy.h
PeriodicSkill.h
SpecializedSkill.h
@@ -37,6 +42,7 @@ armarx_add_library(
detail/SkillImplementationWrapper.h
SkillContext.h
+
mixins/All.h
mixins/ArvizSkillMixin.h
mixins/MNSSkillMixin.h
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
new file mode 100644
index 000000000..96ec83917
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
@@ -0,0 +1,55 @@
+#include "SimpleSkill.h"
+
+namespace armarx::skills
+{
+ Skill::InitResult
+ SimpleSKill::init()
+ {
+ InitInput i;
+ i.executorName = this->executorName;
+ i.params = this->parameters;
+ return this->init(i);
+ }
+
+ Skill::MainResult
+ SimpleSKill::main()
+ {
+ MainInput i;
+ i.executorName = this->executorName;
+ i.params = this->parameters;
+ i.callback = this->callback;
+ return this->main(i);
+ }
+
+ Skill::ExitResult
+ SimpleSKill::exit()
+ {
+ ExitInput i;
+ i.executorName = this->executorName;
+ i.params = this->parameters;
+ return this->exit(i);
+ }
+
+ Skill::InitResult
+ SimpleSKill::init(const InitInput& in)
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ SimpleSKill::main(const MainInput& in)
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+
+ Skill::ExitResult
+ SimpleSKill::exit(const ExitInput& in)
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
new file mode 100644
index 000000000..c4b736dfb
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SimpleSKill : public Skill
+ {
+ public:
+ using Skill::Skill;
+
+ struct InitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr params;
+ };
+
+ struct MainInput
+ {
+ std::string executorName;
+ aron::data::DictPtr params;
+ CallbackT callback;
+ };
+
+ struct ExitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr params;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual InitResult init(const InitInput& in);
+ virtual MainResult main(const MainInput& in);
+ virtual ExitResult exit(const ExitInput& in);
+
+ InitResult init() final;
+ MainResult main() final;
+ ExitResult exit() final;
+
+ private:
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
similarity index 62%
rename from source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.cpp
rename to source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
index 4b272f291..49a5c2e49 100644
--- a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
@@ -1,6 +1,6 @@
#include "SimpleSpecializedSkill.h"
-namespace armarx
+namespace armarx::skills
{
}
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
new file mode 100644
index 000000000..a62ca4ae5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
@@ -0,0 +1,97 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SimpleSpecializedSkill : public Skill
+ {
+ public:
+ using ParamType = AronT;
+ using Skill::Skill;
+
+ struct SpecializedInitInput
+ {
+ std::string executorName;
+ AronT params;
+ };
+
+ struct SpecializedMainInput
+ {
+ std::string executorName;
+ AronT params;
+ CallbackT callback;
+ };
+
+ struct SpecializedExitInput
+ {
+ std::string executorName;
+ AronT params;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual InitResult
+ init(const SpecializedInitInput& in)
+ {
+ return InitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ virtual MainResult
+ main(const SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded,
+ .data = nullptr};
+ }
+
+ virtual ExitResult
+ exit(const SpecializedExitInput& in)
+ {
+ return ExitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ InitResult
+ init() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedInitInput i;
+ i.executorName = this->executorName;
+ i.params = p;
+ return this->init(i);
+ }
+
+ MainResult
+ main() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedMainInput i;
+ i.executorName = this->executorName;
+ i.callback = this->callback;
+ i.params = p;
+ return this->main(i);
+ }
+
+ ExitResult
+ exit() final
+ {
+ AronT p;
+ p.fromAron(this->parameters);
+
+ SpecializedExitInput i;
+ i.executorName = this->executorName;
+ i.params = p;
+ return this->exit(i);
+ }
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c187b4ad4..d24535a22 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -103,8 +103,11 @@ namespace armarx::plugins
// NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
ARMARX_CHECK(skillId.fullySpecified());
- ARMARX_CHECK_GREATER(skillFactories.count(skillId), 0)
- << "Skill '" + skillId.toString() + "' not found.";
+ if (skillFactories.count(skillId) == 0)
+ {
+ ARMARX_INFO << "Could not find a skill factory for id: " << skillId;
+ return nullptr;
+ }
auto* facPtr = skillFactories.at(skillId).get();
return static_cast<skills::SkillFactory*>(facPtr);
@@ -191,6 +194,7 @@ namespace armarx::plugins
auto l1 = std::unique_lock{skillFactoriesMutex};
const auto& fac = getSkillFactory(executionRequest.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
// async start execution. But we wait for the execution to finish at the end of this method
execution = std::thread(
@@ -202,7 +206,7 @@ namespace armarx::plugins
auto it = skillExecutions.emplace(
std::piecewise_construct,
std::make_tuple(executionId),
- std::make_tuple(fac, executionId, usedParameterization));
+ std::make_tuple(*fac, executionId, usedParameterization));
ARMARX_CHECK(it.second)
<< "For some reason a skill '" << executionId.skillId.toString()
@@ -257,6 +261,7 @@ namespace armarx::plugins
auto l1 = std::unique_lock{skillFactoriesMutex};
const auto& fac = getSkillFactory(executionRequest.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
// async start execution. But we wait for the execution to finish at the end of this method
execution = std::thread(
@@ -268,7 +273,7 @@ namespace armarx::plugins
auto it = skillExecutions.emplace(
std::piecewise_construct,
std::make_tuple(executionId),
- std::make_tuple(fac, executionId, usedParameterization));
+ std::make_tuple(*fac, executionId, usedParameterization));
ARMARX_CHECK(it.second)
<< "For some reason a skill '" << executionId.skillId.toString()
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 9550dec64..e03d73e26 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -157,7 +157,28 @@ namespace armarx
getSkillProviderPlugin() const;
protected:
- // utility
+ // -----------------------------------------------------------------------------------------
+ // LEGACY, TODO: NEEDS TESTING
+ // -----------------------------------------------------------------------------------------
+ template <class _Skill>
+
+ requires skills::isSkill<_Skill> skills::SkillFactory*
+ addSkill(std::unique_ptr<_Skill>&& s)
+ {
+ return addSkillFactory<_Skill>();
+ }
+
+ template <class _Skill, typename... Args>
+
+ requires skills::isSkill<_Skill> skills::SkillFactory*
+ addSkill(Args&&... args)
+ {
+ return addSkillFactory<_Skill>(std::forward<Args>(args)...);
+ }
+
+ // -----------------------------------------------------------------------------------------
+ // New
+ // -----------------------------------------------------------------------------------------
skills::SkillFactory*
addSkillFactory(const skills::SkillDescription& desc, const skills::LambdaSkill::FunT& f)
{
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 9fc2beba6..d551bd88d 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -17,16 +17,25 @@ namespace armarx
using Base = Skill;
using ParamType = AronT;
- struct SpecializedMainInput
- {
- std::string executorName;
- AronT params;
- CallbackT callback;
- };
-
using Skill::Skill;
virtual ~SpecializedSkill() = default;
+ void
+ setParameters(const AronT& d)
+ {
+ std::scoped_lock l(this->parametersMutex);
+ this->parameters = d;
+
+ Base::setParameters(d.toAron());
+ }
+
+ AronT
+ getParameters() const
+ {
+ std::scoped_lock l(this->parametersMutex);
+ return this->parameters;
+ }
+
/// returns the accepted type of the skill
static armarx::aron::type::ObjectPtr
GetAcceptedType()
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 6846cca66..41679d93a 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -5,22 +5,23 @@ namespace armarx
namespace skills::detail
{
SkillImplementationWrapper::SkillImplementationWrapper(
- const skills::SkillFactory* fac,
+ const skills::SkillFactory& fac,
const skills::SkillExecutionID& execId,
const skills::SkillParameterization& param) :
factory(fac), statusUpdate(execId, param)
{
- ARMARX_CHECK_NOT_NULL(this->factory);
}
// ask a skill to stop
void
SkillImplementationWrapper::stopSkill()
{
- if (this->skill == nullptr)
+ while (this->skill == nullptr)
{
- // skill has not started yet.
- return;
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to stop a skill that has not been constructed yet... "
+ "Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
this->skill->notifySkillToStopASAP();
@@ -29,10 +30,12 @@ namespace armarx
void
SkillImplementationWrapper::addSkillParameters(const aron::data::DictPtr& i)
{
- if (this->skill == nullptr)
+ while (this->skill == nullptr)
{
- // skill has not started yet
- return;
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to add params to a skill that has not been constructed "
+ "yet... Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
this->skill->addParameters(i);
@@ -41,14 +44,10 @@ namespace armarx
TerminatedSkillStatusUpdate
SkillImplementationWrapper::executeSkill()
{
+ std::scoped_lock l(this->executionMutex);
// -------------------------------------------------------------------------------------
// sanity check
// -------------------------------------------------------------------------------------
- ARMARX_CHECK(!has_been_called)
- << "A SKILL HAS ALREADY BEEN CALLED. THIS SHOULD NOT HAPPEN!!";
-
- has_been_called = true;
-
ARMARX_CHECK(this->skill == nullptr)
<< "A SKILL HAS ALREADY BEEN CONSTRUCTED. THIS SHOULD NOT HAPPEN!!";
@@ -59,12 +58,12 @@ namespace armarx
// actually we should lock... however this is only read access and the members are const throughout the execution...
ARMARX_CHECK(statusUpdate.executionId.skillId.fullySpecified());
- const auto initial_aron_params = statusUpdate.usedParameterization.parameterization;
- const auto callback_interface = statusUpdate.usedParameterization.callbackInterface;
- const auto skillId = statusUpdate.executionId.skillId;
- const auto skillName = skillId.skillName;
- const auto providerId = *skillId.providerId;
- const auto executorName = statusUpdate.executionId.executorName;
+ const auto& initial_aron_params = statusUpdate.usedParameterization.parameterization;
+ const auto& callback_interface = statusUpdate.usedParameterization.callbackInterface;
+ const auto& skillId = statusUpdate.executionId.skillId;
+ const auto& skillName = skillId.skillName;
+ const auto& providerId = *skillId.providerId;
+ const auto& executorName = statusUpdate.executionId.executorName;
ARMARX_INFO_S << "Executing skill: " << skillName;
@@ -116,11 +115,17 @@ namespace armarx
// construct skill
// -------------------------------------------------------------------------------------
updateStatus(SkillStatus::Constructing);
- this->skill = this->factory->createSkill(providerId);
+ this->skill = this->factory.createSkill(providerId);
this->skill->executorName = (executorName);
+ this->skill->manager = skills::manager::dti::SkillManagerInterfacePrx::checkedCast(
+ callback_interface); // ugly. Get managerPrx from manager!
+ ;
this->skill->callback = [&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
{ updateStatus(s, d); };
+ // set initial parameters that were attached to the execution request (only add as we are not sure whether some updates already arrived)
+ skill->addParameters(initial_aron_params);
+
auto makeTerminationResult = [&](const std::string& message)
{
@@ -185,9 +190,6 @@ namespace armarx
try
{
updateStatus(SkillStatus::Preparing);
-
- // set initial parameters that were attached to the execution request
- skill->addParameters(initial_aron_params);
auto prepareRet = skill->prepareSkill();
while (prepareRet.status == ActiveOrTerminatedSkillStatus::Running)
{
@@ -270,16 +272,17 @@ namespace armarx
// All succeeded!
- updateStatus(SkillStatus::Succeeded);
-
- // return result of main method
- std::unique_lock l(skillStatusesMutex);
- TerminatedSkillStatusUpdate ret(statusUpdate.executionId,
- statusUpdate.usedParameterization);
- ret.data = mainRet.data;
- ret.status = TerminatedSkillStatus::Succeeded;
+ {
+ updateStatus(SkillStatus::Succeeded);
- return ret;
+ // return result of main method
+ std::unique_lock l(skillStatusesMutex);
+ TerminatedSkillStatusUpdate ret(statusUpdate.executionId,
+ statusUpdate.usedParameterization);
+ ret.data = mainRet.data;
+ ret.status = TerminatedSkillStatus::Succeeded;
+ return ret;
+ }
}
} // namespace skills::detail
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index c57a05485..45108724d 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -18,11 +18,10 @@ namespace armarx
class SkillImplementationWrapper
{
private:
- const skills::SkillFactory* factory;
+ const skills::SkillFactory& factory;
std::unique_ptr<Skill> skill;
-
- std::atomic_bool has_been_called = false; // sanitize
+ mutable std::mutex executionMutex;
public:
// Current execution statuses. Changes during execution
@@ -31,7 +30,7 @@ namespace armarx
SkillStatusUpdate statusUpdate;
// ctor
- SkillImplementationWrapper(const skills::SkillFactory* fac,
+ SkillImplementationWrapper(const skills::SkillFactory& fac,
const skills::SkillExecutionID&,
const skills::SkillParameterization&);
@@ -39,9 +38,10 @@ namespace armarx
// the return type additionally contains the input configuration (similar to the status updates used in callbacks)
TerminatedSkillStatusUpdate executeSkill();
- // ask a skill to stop
+ // ask a skill to stop. Must be concurrent to execute skill
void stopSkill();
+ // add parameters to a skill. Must be concurrent to execute skill
void addSkillParameters(const aron::data::DictPtr& i);
};
} // namespace detail
diff --git a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.cpp b/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.cpp
deleted file mode 100644
index 3d4a52403..000000000
--- a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "SimpleSkill.h"
-
-namespace armarx
-{
-
-}
diff --git a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.h
deleted file mode 100644
index 5872a11ed..000000000
--- a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSkill.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#pragma once
-
-namespace armarx
-{
- namespace skills
- {
- class SimpleSKill
- {
- public:
- SimpleSKill() = default;
-
- private:
- };
- } // namespace skills
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.h
deleted file mode 100644
index 06daeb741..000000000
--- a/source/RobotAPI/libraries/skills/provider/legacy/SimpleSpecializedSkill.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#pragma once
-
-namespace armarx
-{
- namespace skills
- {
- class SimpleSpecializedSkill
- {
- public:
- SimpleSpecializedSkill() = default;
-
- private:
- };
- } // namespace skills
-} // namespace armarx
--
GitLab