diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 60a38b465ddb8bd9d6cbdac5ec4952606624880c..455215311950946070f5ffcf225dc75755cc6447 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,20 +1,26 @@
#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 ¤t)
+ 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 ¤t)
+ 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 ¤t)
+ 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
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 21b32da1a75488fde1efea01b3b48e42b7fa77b9..ba48923e7beab1876c6477d04845d05ce64883b9 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 0522191bfb57d272785380502217f23006a9e254..bfba4d4cce7c37a880ea93b919373bdd3e126402 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/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();
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
index e0ebdb3b653a1be1d59d9b945ac6b35c58962595..93e18a137f3f927017426aab55ae6b2c05507830 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ b/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);
+ }
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
index 6e8ddc5c74ec5e3d36423984d67807ae12da0680..6fb15914686e6d9f01d0387719b6d5729b98c495 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ b/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;