diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 77943d887c6833b87a730b1ce45b926273795ad2..f0e3077422c3d81e044806ddb4e12391424ab67e 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -135,7 +136,7 @@ namespace armarx::articulated_object
}
auto obj =
- VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
VirtualRobot::RobotIO::eStructure);
if (not obj)
@@ -163,6 +164,11 @@ namespace armarx::articulated_object
}
}
+ const auto state = articulatedObjectReaderPlugin->get().queryState(articulatedObject->getType() + "/" + articulatedObject->getName(), Clock::Now());
+
+ ARMARX_CHECK(state.has_value());
+ articulatedObject->setGlobalPose(state->globalPose.matrix());
+
ARMARX_DEBUG << "Reporting articulated objects";
const armem::Time now = armem::Time::Now();
@@ -179,7 +185,7 @@ namespace armarx::articulated_object
jointValue = node->unscaleJointValue(k, 0, 1);
}
- articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+ // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
articulatedObject->setJointValues(jointValues);
auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 80c3168c5fc5e7c893c2813082171e5cd9c6319b..18d7aa50afd11ba59e9b5d3bb95ceda0a942fe58 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -231,6 +231,7 @@ namespace armarx::armem::articulated_object
cs.dataset = objectId.dataset();
objectInstance.pose.objectID = cs;
+ objectInstance.pose.providerName = properties.providerName;
update.instancesData = {objectInstance.toAron()};
update.referencedTime = timestamp;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 514096731b83167b902dfe4198deae82725837ed..b9abf459ec14789b7e4fe3fccf42ba73db0f5eed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -70,6 +70,11 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
+ if(name.empty())
+ {
+ continue;
+ }
+
auto poseHistoryMap = poseHistories.find(name);
if (poseHistoryMap != poseHistories.end())
{
@@ -92,6 +97,12 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, viz::Layer> stage;
for (size_t i = 0; i < objectPoses.size(); ++i)
{
+ if(objectPoses.at(i).providerName.empty())
+ {
+ ARMARX_INFO << "Object pose provider not set!";
+ continue;
+ }
+
visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
objectPoses.at(i),
poseHistories.at(i),
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;