Merge branch 'eurobin/skills' into 'master'

Eurobin/skills

See merge request !347

source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp

 #include "SkillManagerComponentPlugin.h"
 
 #include <ArmarXCore/core/Component.h>
+
 #include "../error/Exception.h"
 
 namespace armarx::plugins
 {
-    void SkillManagerComponentPlugin::preOnInitComponent()
-    {}
-
-    void SkillManagerComponentPlugin::preOnConnectComponent()
-    {}
+    void
+    SkillManagerComponentPlugin::preOnInitComponent()
+    {
+    }
 
-    void SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
-    {}
-}
+    void
+    SkillManagerComponentPlugin::preOnConnectComponent()
+    {
+    }
 
+    void
+    SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+    {
+    }
+} // namespace armarx::plugins
 
 namespace armarx
 {
@@ -23,7 +29,9 @@ namespace armarx
         addPlugin(plugin);
     }
 
-    void SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current&)
+    void
+    SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
+                                                 const Ice::Current&)
     {
         std::lock_guard l(skillProviderMapMutex);
         if (skillProviderMap.find(info.providerName) == skillProviderMap.end())
@@ -33,13 +41,16 @@ namespace armarx
         }
         else
         {
-            ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists. "
+            ARMARX_INFO << "Trying to add a provider with name '" << info.providerName
+                        << "' but the provider already exists. "
                         << "Overwriting the old provider info.";
             skillProviderMap[info.providerName] = info.provider;
         }
     }
 
-    void SkillManagerComponentPluginUser::removeProvider(const std::string& providerName, const Ice::Current&)
+    void
+    SkillManagerComponentPluginUser::removeProvider(const std::string& providerName,
+                                                    const Ice::Current&)
     {
         std::lock_guard l(skillProviderMapMutex);
         if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
@@ -49,11 +60,13 @@ namespace armarx
         }
         else
         {
-            ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't be found.";
+            ARMARX_INFO << "Trying to remove a provider with name '" << providerName
+                        << "' but it couldn't be found.";
         }
     }
 
-    std::string SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
+    std::string
+    SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
     {
         for (const auto& [providerName, providerPrx] : skillProviderMap)
         {
@@ -69,18 +82,30 @@ namespace armarx
         return "INVALID PROVIDER NAME";
     }
 
-    skills::provider::dto::SkillStatusUpdate SkillManagerComponentPluginUser::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current&)
+    using SkillProviderInterfacePrxMap =
+        std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx>;
+
+    skills::provider::dto::SkillStatusUpdate
+    SkillManagerComponentPluginUser::executeSkill(
+        const skills::manager::dto::SkillExecutionRequest& info,
+        const Ice::Current&)
     {
         std::string providerName = "INVALID PROVIDER NAME";
         if (info.skillId.providerName == "*")
         {
             providerName = getFirstProviderNameThatHasSkill(info.skillId.skillName);
         }
-        else if(not(info.skillId.providerName.empty()))
+        else if (not(info.skillId.providerName.empty()))
         {
             providerName = info.skillId.providerName;
         }
 
+        SkillProviderInterfacePrxMap skillProviderMap;
+        {
+            std::scoped_lock l(skillProviderMapMutex);
+            skillProviderMap = this->skillProviderMap;
+        }
+
         bool remove = false;
         if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
         {
@@ -108,24 +133,39 @@ namespace armarx
             if (remove)
             {
                 std::scoped_lock l(skillProviderMapMutex);
+                // No copy!
+                SkillProviderInterfacePrxMap& skillProviderMap = this->skillProviderMap;
                 if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
                 {
-                    ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "' during execution. Removing it from skills.";
+                    ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+                                   << n << "' during execution. Removing it from skills.";
                     it = skillProviderMap.erase(it);
                 }
             }
         }
         else
         {
-            ARMARX_ERROR << "Could not execute a skill of provider '" + providerName + "' because the provider does not exist.";
-            throw skills::error::SkillException(__PRETTY_FUNCTION__, "Skill execution failed. Could not execute a skill of provider '" + providerName + "' because the provider does not exist.");
+            ARMARX_ERROR << "Could not execute a skill of provider '" + providerName +
+                                "' because the provider does not exist.";
+            throw skills::error::SkillException(
+                __PRETTY_FUNCTION__,
+                "Skill execution failed. Could not execute a skill of provider '" + providerName +
+                    "' because the provider does not exist.");
         }
         return {}; // Never happens
     }
 
-    void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current &current)
+    void
+    SkillManagerComponentPluginUser::abortSkill(const std::string& providerName,
+                                                const std::string& skillName,
+                                                const Ice::Current& current)
     {
-        std::scoped_lock l(skillProviderMapMutex);
+        SkillProviderInterfacePrxMap skillProviderMap;
+        {
+            std::scoped_lock l(skillProviderMapMutex);
+            skillProviderMap = this->skillProviderMap;
+        }
+
         if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
         {
             const auto& n = it->first;
@@ -136,19 +176,23 @@ namespace armarx
             }
             else
             {
-                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
-                it = skillProviderMap.erase(it);
+                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+                               << n << "'. Removing it from skills on next execute.";
             }
         }
     }
 
-    void SkillManagerComponentPluginUser::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current&)
+    void
+    SkillManagerComponentPluginUser::updateStatusForSkill(
+        const skills::provider::dto::SkillStatusUpdate& statusUpdate,
+        const Ice::Current&)
     {
-        (void) statusUpdate;
+        (void)statusUpdate;
         // If you want to use the status, implement this method!
     }
 
-    skills::manager::dto::SkillDescriptionMapMap SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current &current)
+    skills::manager::dto::SkillDescriptionMapMap
+    SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
     {
         skills::manager::dto::SkillDescriptionMapMap ret;
 
@@ -165,14 +209,16 @@ namespace armarx
             }
             else
             {
-                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+                               << n << "'. Removing it from skills.";
                 it = skillProviderMap.erase(it);
             }
         }
         return ret;
     }
 
-    skills::manager::dto::SkillStatusUpdateMapMap SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current &current)
+    skills::manager::dto::SkillStatusUpdateMapMap
+    SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
     {
         skills::manager::dto::SkillStatusUpdateMapMap ret;
 
@@ -189,10 +235,11 @@ namespace armarx
             }
             else
             {
-                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+                               << n << "'. Removing it from skills.";
                 it = skillProviderMap.erase(it);
             }
         }
         return ret;
     }
-}
+} // namespace armarx

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

@@ -131,6 +131,14 @@ namespace armarx
             onTimeoutReached();
         }
 
+        Skill::MainResult Skill::MakeAbortedResult(aron::data::DictPtr data)
+        {
+            return MainResult{
+                .status = TerminatedSkillStatus::Aborted,
+                .data = data,
+            };
+        }
+
         void Skill::notifySkillToStopASAP()
         {
             stopped = true;

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

@@ -107,6 +107,8 @@ namespace armarx
             MainResult executeFullSkill(const MainInput& in);
 
         protected:
+            static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
+
             // fires if the skill reaches timeout
             void notifyTimeoutReached();
 

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

@@ -43,5 +43,12 @@ namespace armarx
             auto future = manager->begin_executeSkill(req);
             return future;
         }
+
+        void SkillProxy::abortSkill(const std::string& executorName)
+        {
+            // TODO: This will be used in the future, do not remove it!
+            (void) executorName;
+            manager->abortSkill(skillId.providerName, skillId.skillName);
+        }
     }
 }

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

@@ -17,6 +17,8 @@ namespace armarx
             TerminatedSkillStatusUpdate executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
             IceInternal::Handle<Ice::AsyncResult> begin_executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
 
+            void abortSkill(const std::string& executorName);
+
         private:
             const manager::dti::SkillManagerInterfacePrx& manager;