Merge branch 'master' of git.h2t.iar.kit.edu:sw/armarx/robot-api

# Conflicts:
#	source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp

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();

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;

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),

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);
             }
         }

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);
             }
         }

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;