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/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index d53a8dd6a89f0cd37f609c9a4742d97d86f4deba..0f1f8047f3f47976a7ce52a928bac858d665b053 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -62,6 +62,7 @@ namespace armarx::obstacle_avoidance
if (auto obstacle = finder.loadManipulationObject(objectPose))
{
obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ obstacle->setName(objectPose.objectID.instanceName());
sceneObjects->addSceneObject(obstacle);
}
}
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index f0171437b9b4898a87a48fc2cc2e49d4a467e986..455215311950946070f5ffcf225dc75755cc6447 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -201,25 +201,16 @@ namespace armarx
{
const auto& n = it->first;
const auto& s = it->second;
- try
+ if (s)
{
- if (s)
- {
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
- ++it;
- }
- else
- {
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
- }
+ skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
+ ret.insert({n, m});
+ ++it;
}
- catch (...)
+ else
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << n << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
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;