From 2fec47b5f71448fa70385fa74e9086113ded7c87 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 18 Oct 2023 09:58:22 +0200
Subject: [PATCH] unify stepResult struct of periodic skills. Added simple
periodic skill classes
---
.../skills/provider/PeriodicSkill.cpp | 16 ++--
.../libraries/skills/provider/PeriodicSkill.h | 2 -
.../provider/PeriodicSpecializedSkill.h | 43 +++------
.../skills/provider/SimplePeriodicSkill.cpp | 49 ++++++++--
.../skills/provider/SimplePeriodicSkill.h | 27 +++---
.../provider/SimplePeriodicSpecializedSkill.h | 90 ++++++++++++-------
.../libraries/skills/provider/SimpleSkill.h | 4 +-
.../skills/provider/SimpleSpecializedSkill.h | 4 +-
8 files changed, 138 insertions(+), 97 deletions(-)
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index 197159cb7..fa354d03d 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -36,12 +36,6 @@ namespace armarx::skills
{
}
- PeriodicSkill::StepResult
- PeriodicSkill::stepOfSkill()
- {
- return this->step();
- }
-
Skill::MainResult
PeriodicSkill::main()
{
@@ -51,7 +45,7 @@ namespace armarx::skills
{
this->throwIfSkillShouldTerminate();
- const auto res = stepOfSkill();
+ const auto res = step();
switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
@@ -68,14 +62,14 @@ namespace armarx::skills
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " too long. Expected "
- << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
// never happens
- return MakeSucceededResult();
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
PeriodicSkill::StepResult
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
index b11239dcf..aff91f196 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -41,8 +41,6 @@ namespace armarx::skills
PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
- StepResult stepOfSkill();
-
protected:
/// Do not use anymore
Skill::MainResult main() final;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 2c0669e54..914c6da02 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -50,12 +50,6 @@ namespace armarx::skills
{
}
- StepResult
- stepOfSkill()
- {
- return this->step();
- }
-
protected:
/// Do not use anymore
Skill::MainResult
@@ -63,42 +57,33 @@ namespace armarx::skills
{
armarx::core::time::Metronome metronome(frequency);
- while (not Skill::shouldSkillTerminate())
+ while (true)
{
- const auto statusUpdate = stepOfSkill();
- switch (statusUpdate.status)
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step();
+ switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
- // nothing to do here
+ // nothing to do here. break switch
break;
case ActiveOrTerminatedSkillStatus::Aborted:
- return {TerminatedSkillStatus::Aborted, statusUpdate.data};
+ return Skill::MakeAbortedResult();
case ActiveOrTerminatedSkillStatus::Succeeded:
- return {TerminatedSkillStatus::Succeeded, statusUpdate.data};
+ return Skill::MakeSucceededResult(res.data);
case ActiveOrTerminatedSkillStatus::Failed:
- return {TerminatedSkillStatus::Failed, statusUpdate.data};
+ return Skill::MakeFailedResult();
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " too long. Expected "
- << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (stopped)
- {
- return {TerminatedSkillStatus::Aborted, nullptr};
- }
-
- if (timeoutReached)
- {
- ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
- return {TerminatedSkillStatus::Failed, nullptr};
- }
-
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
@@ -111,10 +96,6 @@ namespace armarx::skills
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
- private:
- using Skill::stopped;
- using Skill::timeoutReached;
-
protected:
const armarx::Frequency frequency;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
index d5f8e6cbd..33a6475b3 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -2,20 +2,53 @@
namespace armarx::skills
{
+ SimplePeriodicSkill::SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ SimpleSkill(skillDescription), frequency(frequency)
+ {
+ }
- PeriodicSkill::StepResult SimplePeriodicSkill::step()
+ Skill::MainResult
+ SimplePeriodicSkill::main(const MainInput& in)
{
- StepInput i;
- i.executorName = this->executorName;
- i.parameters = this->parameters;
- i.callback = this->callback;
- return this->step(i);
+ armarx::core::time::Metronome metronome(frequency);
+
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
- PeriodicSkill::StepResult SimplePeriodicSkill::step(const StepInput& in)
+ SimplePeriodicSkill::StepResult
+ SimplePeriodicSkill::step(const MainInput& in)
{
ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
<< "'. Please overwrite this method!";
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
-}
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
index 69a7ccc18..3ca70d213 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
@@ -1,30 +1,33 @@
#pragma once
-#include <RobotAPI/libraries/skills/provider/PeriodicSkill.h>
+#include "PeriodicSkill.h"
+#include "SimpleSkill.h"
namespace armarx
{
namespace skills
{
- class SimplePeriodicSkill : public PeriodicSkill
+ class SimplePeriodicSkill : public SimpleSkill
{
public:
- using Base = PeriodicSkill;
+ using Base = SimpleSkill;
- struct StepInput
- {
- std::string executorName;
- aron::data::DictPtr parameters;
- CallbackT callback;
- };
+ using Base::Base;
- using Base::Base;
+ using StepResult = PeriodicSkill::StepResult;
+
+ SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency);
protected:
- StepResult step() final;
+ /// Do not use anymore
+ Skill::MainResult main(const MainInput& in) final;
- virtual StepResult step(const StepInput& in);
+ /// Override this method with your own step function
+ virtual StepResult step(const MainInput& in);
+ protected:
+ const armarx::Frequency frequency;
};
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
index 221fdc2c7..ce3c968f5 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -1,51 +1,79 @@
#pragma once
-#include <RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h>
+#include "PeriodicSkill.h"
+#include "SimpleSpecializedSkill.h"
namespace armarx
{
namespace skills
{
template <class AronT>
- class SimplePeriodicSpecializedSkill : public PeriodicSpecializedSkill<AronT>
+ class SimplePeriodicSpecializedSkill : public SimpleSpecializedSkill<AronT>
{
public:
- using Base = PeriodicSpecializedSkill<AronT>;
- using Skill::description;
- using Skill::getSkillId;
+ using Base = SimpleSpecializedSkill<AronT>;
+ using ParamType = AronT;
- using StepResult = PeriodicSkill::StepResult;
+ using Base::Base;
- using Base::Base;
+ using StepResult = PeriodicSkill::StepResult;
- struct SpecializedStepInput
- {
- std::string executorName;
- AronT parameters;
- Skill::CallbackT callback;
- };
+ SimplePeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
+ {
+ }
protected:
- StepResult step() final
- {
- AronT p;
- p.fromAron(this->parameters);
-
- SpecializedStepInput i;
- i.executorName = this->executorName;
- i.parameters = p;
- i.callback = this->callback;
- return this->step(i);
- }
-
- virtual StepResult step(const SpecializedStepInput& in)
- {
- ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
- << "'. Please overwrite this method!";
- return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
- }
+ /// Do not use anymore
+ Skill::MainResult
+ main(const typename Base::SpecializedMainInput& in) final
+ {
+ armarx::core::time::Metronome metronome(frequency);
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return Skill::MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return Skill::MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return Skill::MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ /// Override this method with your own step function
+ virtual StepResult
+ step(const typename Base::SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillId
+ << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+
+ protected:
+ const armarx::Frequency frequency;
};
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
index a6cc87324..45e4715d8 100644
--- a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
@@ -9,7 +9,9 @@ namespace armarx
class SimpleSkill : public Skill
{
public:
- using Skill::Skill;
+ using Base = Skill;
+
+ using Base::Base;
struct InitInput
{
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
index 1572c16b7..b14531195 100644
--- a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
@@ -10,8 +10,10 @@ namespace armarx
class SimpleSpecializedSkill : public Skill
{
public:
+ using Base = Skill;
using ParamType = AronT;
- using Skill::Skill;
+
+ using Base::Base;
struct SpecializedInitInput
{
--
GitLab