Merge remote-tracking branch 'origin/master' into armem/ltm/dev

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/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;