PeriodicSkill and PeriodicSpecializedSkill

source/RobotAPI/libraries/skills/CMakeLists.txt

@@ -15,6 +15,7 @@ armarx_add_library(
         ./manager/SkillManagerComponentPlugin.cpp
         ./provider/SkillProviderComponentPlugin.cpp
         ./provider/Skill.cpp
+        ./provider/PeriodicSkill.cpp
         ./provider/SpecializedSkill.cpp
         ./provider/SkillDescription.cpp
         ./provider/SkillStatusUpdate.cpp
@@ -24,6 +25,7 @@ armarx_add_library(
         ./manager/SkillManagerComponentPlugin.h
         ./provider/SkillProviderComponentPlugin.h
         ./provider/Skill.h
+        ./provider/PeriodicSkill.h
         ./provider/SpecializedSkill.h
         ./provider/SkillDescription.h
         ./provider/SkillStatusUpdate.h

source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "PeriodicSkill.h"
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Frequency.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "RobotAPI/libraries/skills/provider/Skill.h"
+
+namespace armarx::skills
+{
+    PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
+                                 const armarx::Frequency& frequency) :
+        Skill(skillDescription), frequency(frequency)
+    {
+    }
+
+    Skill::Status
+    PeriodicSkill::_execute(const aron::data::DictPtr& params, const CallbackT& callback)
+    {
+        if(not initialize(params))
+        {
+            onFailed();
+            return Skill::Status::Failed;
+        }
+
+        core::time::Metronome metronome(frequency);
+
+        while (not stopped and not timeoutReached)
+        {
+            const auto status = executeOnce(params);
+            switch (status)
+            {
+                case Status::Running:
+                    // nothing to do here
+                    break;
+                case Status::Succeeded:
+                    onSucceeded();
+                    return Skill::Status::Succeeded;
+                case Status::Failed:
+                    onFailed();
+                    return Skill::Status::Failed;
+            }
+
+            const auto sleepDuration = metronome.waitForNextTick();
+            if (not sleepDuration.isPositive())
+            {
+                ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+                            << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+            }
+        }
+
+        if (stopped)
+        {
+            onStopped();
+            return Skill::Status::Stopped;
+        }
+
+        if (timeoutReached)
+        {
+            onTimeoutReached();
+            return Skill::Status::TimeoutReached;
+        }
+
+        throw armarx::LocalException("should not happen.");
+    }
+
+
+} // namespace armarx::skills

source/RobotAPI/libraries/skills/provider/PeriodicSkill.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+    class PeriodicSkill : virtual public Skill
+    {
+    public:
+        PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
+
+    protected:
+        typename Skill::Status _execute(const aron::data::DictPtr& params,
+                                        const Skill::CallbackT& callback) final;
+
+        enum class Status
+        {
+            Running,
+            Failed,
+            Succeeded
+        };
+
+        virtual bool initialize(const aron::data::DictPtr& params) = 0;
+        virtual Status executeOnce(const aron::data::DictPtr& params) = 0;
+
+        virtual void onSucceeded() = 0;
+        virtual void onStopped() = 0;
+        virtual void onFailed() = 0;
+        virtual void onTimeoutReached() = 0;
+
+    private:
+        const armarx::Frequency frequency;
+    };
+
+
+  
+} // namespace armarx::skills

source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "ArmarXCore/core/time/Metronome.h"
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+    template <class AronT>
+    class PeriodicSpecializedSkill : virtual public SpecializedSkill<AronT>
+    {
+    public:
+        PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+                                 const armarx::Frequency& frequency);
+
+    protected:
+        typename Skill::Status _execute(const AronT& params,
+                                        const Skill::CallbackT& callback) final;
+
+        enum class Status
+        {
+            Running,
+            Failed,
+            Succeeded
+        };
+
+        virtual bool initialize(const AronT& params) = 0;
+        virtual Status executeOnce(const AronT& params) = 0;
+
+        virtual void onSucceeded() = 0;
+        virtual void onStopped() = 0;
+        virtual void onFailed() = 0;
+        virtual void onTimeoutReached() = 0;
+
+    private:
+        const armarx::Frequency frequency;
+    };
+
+    template <class AronT>
+    PeriodicSpecializedSkill<AronT>::PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+                                 const armarx::Frequency& frequency) :
+        Skill(skillDescription), frequency(frequency)
+    {
+    }
+
+    template <class AronT>
+    Skill::Status
+    PeriodicSpecializedSkill<AronT>::_execute(const AronT& params, const Skill::CallbackT& callback)
+    {
+       if(not initialize(params))
+        {
+            onFailed();
+            return Skill::Status::Failed;
+        }
+
+        core::time::Metronome metronome(frequency);
+
+        while (not Skill::stopped and not Skill::timeoutReached)
+        {
+            const auto status = executeOnce(params);
+            switch (status)
+            {
+                case Status::Running:
+                    // nothing to do here
+                    break;
+                case Status::Succeeded:
+                    onSucceeded();
+                    return Skill::Status::Succeeded;
+                case Status::Failed:
+                    onFailed();
+                    return Skill::Status::Failed;
+            }
+
+            const auto sleepDuration = metronome.waitForNextTick();
+            if (not sleepDuration.isPositive())
+            {
+                ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+                            << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+            }
+        }
+
+        if (Skill::stopped)
+        {
+            onStopped();
+            return Skill::Status::Stopped;
+        }
+
+        if (Skill::timeoutReached)
+        {
+            onTimeoutReached();
+            return Skill::Status::TimeoutReached;
+        }
+
+        throw armarx::LocalException("should not happen.");
+    }
+} // namespace armarx::skills