source/RobotAPI/interface/skills/SkillManagerInterface.ice

@@ -147,6 +147,16 @@ module armarx
                     FluxioValueList values;
                 };
 
+                struct FluxioEdge
+                {
+                    FluxioIdentificator fromNodeId;
+                    FluxioIdentificator fromParameterId;
+                    FluxioIdentificator toNodeId;
+                    FluxioIdentificator toParameterId;
+                };
+
+                sequence<FluxioParameter> FluxioParameterList;
+                
                 struct FluxioNode
                 {
                     string nodeType; // decides which extended attributes are used
@@ -155,18 +165,11 @@ module armarx
                     float xPos;
                     float yPos;
                     FluxioIdentificator parameterId; // empty id field if node is not a parameter node
-                    FluxioIdentificator skillId; // empty id field if node is not a subskill node                        
+                    FluxioIdentificator skillId; // empty id field if node is not a subskill node   
+                    string controlType; // empty string if node is not a control node
+                    FluxioParameterList parameters; // empty list if node is not a control node                     
                 };
 
-                struct FluxioEdge
-                {
-                    FluxioIdentificator fromNodeId;
-                    FluxioIdentificator fromParameterId;
-                    FluxioIdentificator toNodeId;
-                    FluxioIdentificator toParameterId;
-                };
-
-                sequence<FluxioParameter> FluxioParameterList;
                 sequence<FluxioNode> FluxioNodeList;
                 sequence<FluxioEdge> FluxioEdgeList;
 

source/RobotAPI/libraries/skills/core/CMakeLists.txt

@@ -33,6 +33,7 @@ armarx_add_library(
         FluxioNode.cpp
         FluxioEdge.cpp
         FluxioSkill.cpp
+        FluxioControlNode.cpp
         FluxioParameterNode.cpp
         FluxioSubSkillNode.cpp
         FluxioSkillStatusUpdate.cpp
@@ -56,6 +57,7 @@ armarx_add_library(
         FluxioNode.h
         FluxioEdge.h
         FluxioSkill.h
+        FluxioControlNode.h
         FluxioParameterNode.h
         FluxioSubSkillNode.h
         FluxioSkillStatusUpdate.h

source/RobotAPI/libraries/skills/core/FluxioControlNode.cpp

+#include "FluxioControlNode.h"
+
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "RobotAPI/libraries/skills/core/FluxioNode.h"
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "FluxioParameter.h"
+
+namespace armarx
+{
+    namespace skills
+    {
+        std::optional<manager::dto::FluxioNode>
+        FluxioControlNode::toManagerIce() const
+        {
+            const auto& nt = FluxioNodeTypeToString(nodeType);
+            if (!nt.has_value())
+            {
+                return std::nullopt;
+            }
+
+            manager::dto::FluxioNode ret;
+            ret.nodeId = nodeId;
+            ret.nodeType = nt.value();
+            ret.name = name;
+            ret.xPos = xPos;
+            ret.yPos = yPos;
+            ret.controlType = controlType;
+
+            manager::dto::FluxioParameterList params;
+            for (const auto& [id, parameter] : parametersMap)
+            {
+                params.push_back(parameter.toManagerIce());
+            }
+            ret.parameters = params;
+
+            manager::dto::FluxioIdentificator emptyId;
+            emptyId.id = "";
+            emptyId.hint = "";
+
+            ret.parameterId = emptyId;
+            ret.skillId = emptyId;
+
+            return ret;
+        }
+
+        std::optional<FluxioControlNode>
+        FluxioControlNode::FromIce(const manager::dto::FluxioNode& i,
+                                   std::map<std::string, FluxioProfile>& profilesMap)
+        {
+            FluxioControlNode ret;
+
+            ret.nodeId = i.nodeId;
+            ret.nodeType = FluxioNodeTypeFromString(i.nodeType);
+            ret.name = i.name;
+            ret.xPos = i.xPos;
+            ret.yPos = i.yPos;
+            ret.controlType = i.controlType;
+
+            std::map<std::string, FluxioParameter> paramsMap;
+            for (const auto& parameter : i.parameters)
+            {
+                const auto& param = FluxioParameter::FromIce(parameter, profilesMap);
+                paramsMap.insert({parameter.id, param});
+            }
+
+            ret.parametersMap = paramsMap;
+
+            return ret;
+        }
+    } // namespace skills
+} // namespace armarx

source/RobotAPI/libraries/skills/core/FluxioControlNode.h

+#pragma once
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "FluxioNode.h"
+#include "FluxioParameter.h"
+
+namespace armarx
+{
+    namespace skills
+    {
+        struct FluxioControlNode : public FluxioNode
+        {
+            std::string controlType;
+            std::map<std::string, FluxioParameter> parametersMap;
+
+            std::optional<manager::dto::FluxioNode> toManagerIce() const override;
+
+            static std::optional<FluxioControlNode>
+            FromIce(const manager::dto::FluxioNode& i, std::map<std::string, FluxioProfile>& profilesMap);
+        };
+    } // namespace skills
+} // namespace armarx

source/RobotAPI/libraries/skills/core/FluxioEdge.cpp

@@ -4,6 +4,8 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include "RobotAPI/libraries/skills/core/FluxioControlNode.h"
+
 #include "FluxioNode.h"
 #include "FluxioParameter.h"
 #include "FluxioSubSkillNode.h"
@@ -52,7 +54,7 @@ namespace armarx
             skills::FluxioParameter* toParameterPtr = nullptr;
 
             // Check if fromNode is a SubSkillNode
-            if (fromNodePtr->nodeType == "SUBSKILL")
+            if (fromNodePtr->nodeType == FluxioNodeType::SUBSKILL)
             {
                 const auto* subSkillNodePtr = dynamic_cast<FluxioSubSkillNode*>(fromNodePtr);
 
@@ -68,15 +70,31 @@ namespace armarx
                     return std::nullopt;
                 }
             }
-            else
+            else if (fromNodePtr->nodeType == FluxioNodeType::PARAMETER)
             {
                 fromParameterPtr =
                     FluxioParameter::FromFluxioIdentificatorIce(i.fromParameterId, parametersMap);
             }
+            else if (fromNodePtr->nodeType == FluxioNodeType::CONTROL)
+            {
+                auto* controlNodePtr = dynamic_cast<FluxioControlNode*>(fromNodePtr);
+                if (controlNodePtr == nullptr)
+                {
+                    ARMARX_WARNING << "Failed to cast node to control node";
+                    return std::nullopt;
+                }
 
+                fromParameterPtr = FluxioParameter::FromFluxioIdentificatorIce(
+                    i.fromParameterId, controlNodePtr->parametersMap);
+            }
+            else
+            {
+                ARMARX_WARNING << "Unknown from node type";
+                return std::nullopt;
+            }
 
             // Check if toNode is a SubSkillNode
-            if (toNodePtr->nodeType == "SUBSKILL")
+            if (toNodePtr->nodeType == FluxioNodeType::SUBSKILL)
             {
                 const auto* subSkillNodePtr = dynamic_cast<FluxioSubSkillNode*>(toNodePtr);
 
@@ -92,11 +110,28 @@ namespace armarx
                     return std::nullopt;
                 }
             }
-            else
+            else if (toNodePtr->nodeType == FluxioNodeType::CONTROL)
+            {
+                auto* controlNodePtr = dynamic_cast<FluxioControlNode*>(toNodePtr);
+                if (controlNodePtr == nullptr)
+                {
+                    ARMARX_WARNING << "Failed to cast node to control node";
+                    return std::nullopt;
+                }
+
+                toParameterPtr = FluxioParameter::FromFluxioIdentificatorIce(
+                    i.toParameterId, controlNodePtr->parametersMap);
+            }
+            else if (toNodePtr->nodeType == FluxioNodeType::PARAMETER)
             {
                 toParameterPtr =
                     FluxioParameter::FromFluxioIdentificatorIce(i.toParameterId, parametersMap);
             }
+            else
+            {
+                ARMARX_WARNING << "Unknown to node type";
+                return std::nullopt;
+            }
 
             if (fromParameterPtr == nullptr || toParameterPtr == nullptr)
             {

source/RobotAPI/libraries/skills/core/FluxioNode.cpp

@@ -8,26 +8,71 @@ namespace armarx
 {
     namespace skills
     {
+        FluxioNodeType
+        FluxioNodeTypeFromString(const std::string& type)
+        {
+            if (type == "PARAMETER")
+            {
+                return FluxioNodeType::PARAMETER;
+            }
+            else if (type == "CONTROL")
+            {
+                return FluxioNodeType::CONTROL;
+            }
+            else if (type == "SUBSKILL")
+            {
+                return FluxioNodeType::SUBSKILL;
+            }
+            else
+            {
+                ARMARX_WARNING << "Unknown node type: " << type;
+                return FluxioNodeType::UNKNOWN;
+            }
+        }
+
+        std::optional<std::string>
+        FluxioNodeTypeToString(const FluxioNodeType& type)
+        {
+            switch (type)
+            {
+                case FluxioNodeType::PARAMETER:
+                    return "PARAMETER";
+                case FluxioNodeType::CONTROL:
+                    return "CONTROL";
+                case FluxioNodeType::SUBSKILL:
+                    return "SUBSKILL";
+                case FluxioNodeType::UNKNOWN:
+                default:
+                    ARMARX_WARNING << "Unknown node type: " << static_cast<int>(type);
+                    return std::nullopt;
+            }
+        }
+
         std::optional<manager::dto::FluxioNode>
         FluxioNode::toManagerIce() const
         {
             manager::dto::FluxioNode ret;
 
-            ret.nodeType = nodeType;
+            const auto& nt = FluxioNodeTypeToString(nodeType);
+            if (!nt.has_value())
+            {
+                return std::nullopt;
+            }
+
+            ret.nodeType = nt.value();
             ret.nodeId = nodeId;
             ret.name = name;
             ret.xPos = xPos;
             ret.yPos = yPos;
+            ret.controlType = "";
+            ret.parameters = {};
 
-            manager::dto::FluxioIdentificator paramId;
-            paramId.id = "";
-            paramId.hint = "";
-            ret.parameterId = paramId;
+            manager::dto::FluxioIdentificator emptyId;
+            emptyId.id = "";
+            emptyId.hint = "";
 
-            manager::dto::FluxioIdentificator subSId;
-            subSId.id = "";
-            subSId.hint = "";
-            ret.skillId = subSId;
+            ret.parameterId = emptyId;
+            ret.skillId = emptyId;
 
             return ret;
         }
@@ -61,7 +106,7 @@ namespace armarx
         {
             FluxioNode ret;
 
-            ret.nodeType = i.nodeType;
+            ret.nodeType = FluxioNodeTypeFromString(i.nodeType);
             ret.nodeId = i.nodeId;
             ret.xPos = i.xPos;
             ret.yPos = i.yPos;

source/RobotAPI/libraries/skills/core/FluxioNode.h

 #pragma once
 
-#include <string>
 #include <optional>
+#include <string>
 
 #include <RobotAPI/interface/skills/SkillManagerInterface.h>
 
@@ -9,10 +9,18 @@ namespace armarx
 {
     namespace skills
     {
+        enum class FluxioNodeType
+        {
+            UNKNOWN = 0,
+            PARAMETER = 1,
+            CONTROL = 2,
+            SUBSKILL = 3,
+        };
+
         struct FluxioNode
         {
             virtual ~FluxioNode() = default;
-            std::string nodeType;
+            FluxioNodeType nodeType;
             std::string nodeId;
             std::string name;
             float xPos = 0;
@@ -26,5 +34,8 @@ namespace armarx
                                        std::map<std::string, FluxioNode*>& nodesMap);
             static FluxioNode FromIce(const manager::dto::FluxioNode& i);
         };
+
+        FluxioNodeType FluxioNodeTypeFromString(const std::string& type);
+        std::optional<std::string> FluxioNodeTypeToString(const FluxioNodeType& type);
     } // namespace skills
 } // namespace armarx

source/RobotAPI/libraries/skills/core/FluxioParameterNode.cpp

@@ -7,6 +7,7 @@
 #include <RobotAPI/interface/skills/SkillManagerInterface.h>
 
 #include "FluxioParameter.h"
+#include "RobotAPI/libraries/skills/core/FluxioNode.h"
 
 namespace armarx
 {
@@ -21,9 +22,15 @@ namespace armarx
                 return std::nullopt;
             }
 
+            const auto& nt = FluxioNodeTypeToString(nodeType);
+            if(!nt.has_value())
+            {
+                return std::nullopt;
+            }
+
             manager::dto::FluxioNode ret;
             ret.nodeId = nodeId;
-            ret.nodeType = nodeType;
+            ret.nodeType = nt.value();
             ret.name = name;
             ret.xPos = xPos;
             ret.yPos = yPos;
@@ -57,7 +64,7 @@ namespace armarx
             FluxioParameterNode ret;
 
             ret.nodeId = i.nodeId;
-            ret.nodeType = i.nodeType;
+            ret.nodeType = FluxioNodeTypeFromString(i.nodeType);
             ret.name = i.name;
             ret.xPos = i.xPos;
             ret.yPos = i.yPos;

source/RobotAPI/libraries/skills/core/FluxioSkill.cpp

@@ -10,6 +10,7 @@
 #include "FluxioEdge.h"
 #include "FluxioNode.h"
 #include "FluxioParameterNode.h"
+#include "FluxioControlNode.h"
 #include "FluxioSubSkillNode.h"
 
 namespace armarx
@@ -172,7 +173,7 @@ namespace armarx
                 nodes.clear(); // create all nodes anew
                 for (const manager::dto::FluxioNode& node : i.nodes)
                 {
-                    FluxioNode* n = CreateNode(node, parameters, skillsMap);
+                    FluxioNode* n = CreateNode(node, parameters, skillsMap, profilesMap);
 
                     if (n != nullptr)
                     {
@@ -223,7 +224,7 @@ namespace armarx
                 for (const auto& [nodeId, nodePtr] : nodes)
                 {
                     // TODO: check nodePtr for nullptr
-                    if (nodePtr->nodeType != "SUBSKILL")
+                    if (nodePtr->nodeType != FluxioNodeType::SUBSKILL)
                     {
                         continue;
                     }
@@ -318,7 +319,7 @@ namespace armarx
             {
                 for (const manager::dto::FluxioNode& node : i.nodes)
                 {
-                    FluxioNode* n = CreateNode(node, ret.parameters, skillsMap);
+                    FluxioNode* n = CreateNode(node, ret.parameters, skillsMap, profilesMap);
 
                     if (n != nullptr)
                     {
@@ -346,9 +347,12 @@ namespace armarx
         skills::FluxioNode*
         FluxioSkill::CreateNode(const manager::dto::FluxioNode& i,
                                 std::map<std::string, FluxioParameter>& parametersMap,
-                                std::map<std::string, FluxioSkill>& skillsMap)
+                                std::map<std::string, FluxioSkill>& skillsMap,
+                                std::map<std::string, FluxioProfile>& profilesMap)
         {
-            if (i.nodeType == "PARAMETER")
+            FluxioNodeType nodeType =  FluxioNodeTypeFromString(i.nodeType);
+
+            if (nodeType == FluxioNodeType::PARAMETER)
             {
                 const auto& n = FluxioParameterNode::FromIce(i, parametersMap);
 
@@ -359,7 +363,7 @@ namespace armarx
 
                 ARMARX_WARNING << "ParameterNode with id " << i.nodeId << " could not be converted";
             }
-            else if (i.nodeType == "SUBSKILL")
+            else if (nodeType == FluxioNodeType::SUBSKILL)
             {
                 const auto& n = FluxioSubSkillNode::FromIce(i, skillsMap);
 
@@ -369,6 +373,20 @@ namespace armarx
                 }
                 ARMARX_WARNING << "SubSkillNode with id " << i.nodeId << " could not be converted";
             }
+            else if (nodeType == FluxioNodeType::CONTROL)
+            {
+                const auto& n = FluxioControlNode::FromIce(i, profilesMap);
+
+                if (n.has_value())
+                {
+                    return new FluxioControlNode(*n);
+                }
+                ARMARX_WARNING << "controlNode with id " << i.nodeId << " could not be converted";
+            }
+            else if (nodeType == FluxioNodeType::UNKNOWN)
+            {
+                ARMARX_WARNING << "Node with id " << i.nodeId << " has unknown type";
+            }
             else
             {
                 ARMARX_INFO << "Node type " << i.nodeType << " not supported yet. Ignoring.";

source/RobotAPI/libraries/skills/core/FluxioSkill.h

 #pragma once
 
 #include <list>
-#include <string>
 #include <optional>
+#include <string>
 
 #include <RobotAPI/interface/skills/SkillManagerInterface.h>
 
@@ -48,7 +48,8 @@ namespace armarx
             static skills::FluxioNode*
             CreateNode(const manager::dto::FluxioNode& i,
                        std::map<std::string, FluxioParameter>& parametersMap,
-                       std::map<std::string, FluxioSkill>& skillsMap);
+                       std::map<std::string, FluxioSkill>& skillsMap,
+                       std::map<std::string, FluxioProfile>& profilesMap);
         };
     } // namespace skills
 } // namespace armarx

source/RobotAPI/libraries/skills/core/FluxioSubSkillNode.cpp

@@ -5,6 +5,7 @@
 #include <RobotAPI/interface/skills/SkillManagerInterface.h>
 
 #include "FluxioSkill.h"
+#include "RobotAPI/libraries/skills/core/FluxioNode.h"
 
 namespace armarx
 {
@@ -19,9 +20,15 @@ namespace armarx
                 return std::nullopt;
             }
 
+            const auto& nt = FluxioNodeTypeToString(nodeType);
+            if (!nt.has_value())
+            {
+                return std::nullopt;
+            }
+
             manager::dto::FluxioNode ret;
             ret.nodeId = nodeId;
-            ret.nodeType = nodeType;
+            ret.nodeType = nt.value();
             ret.name = name;
             ret.xPos = xPos;
             ret.yPos = yPos;
@@ -56,7 +63,7 @@ namespace armarx
 
             FluxioSubSkillNode ret;
             ret.nodeId = i.nodeId;
-            ret.nodeType = i.nodeType;
+            ret.nodeType = FluxioNodeTypeFromString(i.nodeType);
             ret.name = i.name;
             ret.xPos = i.xPos;
             ret.yPos = i.yPos;

source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.h

 #pragma once
 
+#include <atomic>
 #include <optional>
 #include <string>
 
@@ -11,26 +12,30 @@
 #include "FluxioExecutor.h"
 #include "SkillManagerComponentPlugin.h"
 
-namespace armarx::plugins
-{
-    class SkillManagerComponentPlugin; // forward declaration
+namespace armarx::plugins {
+class SkillManagerComponentPlugin; // forward declaration
 
-    class FluxioCompositeExecutor : public FluxioExecutor
-    {
-    public:
-        FluxioCompositeExecutor(std::string& id,
-                                SkillManagerComponentPlugin* plugin,
-                                skills::FluxioSkill* skill);
-        void run(std::string executorName, armarx::aron::data::DictPtr parameters) override;
-        void abort() override;
-        std::optional<std::vector<skills::FluxioSkillStatusUpdate>> getStatusUpdate() override;
-        bool validateSkill();
+class FluxioCompositeExecutor : public FluxioExecutor {
+public:
+  FluxioCompositeExecutor(std::string &id, SkillManagerComponentPlugin *plugin,
+                          skills::FluxioSkill *skill);
+  void run(const std::string& executorName,
+           armarx::aron::data::DictPtr parameters) override;
+  void abort() override;
+  std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
+  getStatusUpdate() override;
+  bool validateSkill(skills::FluxioNode *&startNodeId);
 
-    private:
-        std::optional<skills::SkillExecutionID> executionId = std::nullopt;
-        skills::FluxioSkill* skill;
-        void setStatus(skills::SkillStatus status) override;
-        void pollSubStatuses();
-        std::map<std::string, FluxioExecutor*> subExecutionsMap = {}; // key is subSkillNodeId
-    };
+private:
+  void startSubRoutine(const skills::FluxioNode *startNode,
+                       std::atomic_bool &running,
+                       const std::string &executorName);
+  void abortSubExecutions();
+  std::optional<skills::SkillExecutionID> executionId = std::nullopt;
+  skills::FluxioSkill *skill;
+  void setStatus(skills::SkillStatus status) override;
+  void pollSubStatuses();
+  std::map<std::string, FluxioExecutor *> subExecutionsMap =
+      {}; // key is subSkillNodeId
+};
 } // namespace armarx::plugins

source/RobotAPI/libraries/skills/manager/FluxioExecutor.h

@@ -18,7 +18,7 @@ namespace armarx::plugins
 
         FluxioExecutor(std::string& id, SkillManagerComponentPlugin* plugin, bool native) :
             id(id), native(native), plugin(plugin){};
-        virtual void run(std::string executorName, armarx::aron::data::DictPtr parameters){};
+        virtual void run(const std::string& executorName, armarx::aron::data::DictPtr parameters){};
         virtual void abort(){};
 
         virtual std::optional<std::vector<skills::FluxioSkillStatusUpdate>>

source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.cpp

@@ -20,7 +20,7 @@ namespace armarx::plugins
     }
 
     void
-    FluxioNativeExecutor::run(std::string executorName, armarx::aron::data::DictPtr parameters)
+    FluxioNativeExecutor::run(const std::string& executorName, armarx::aron::data::DictPtr parameters)
     {
         skills::SkillExecutionRequest req;
         req.skillId = skillId;

source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.h

@@ -21,7 +21,7 @@ namespace armarx::plugins
                              SkillManagerComponentPlugin* plugin,
                              skills::SkillID& skillId);
                              
-        void run(std::string executorName, armarx::aron::data::DictPtr parameters) override;
+        void run(const std::string& executorName, armarx::aron::data::DictPtr parameters) override;
         void abort() override;
         std::optional<std::vector<skills::FluxioSkillStatusUpdate>> getStatusUpdate() override;
 

source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp

 #include "SkillManagerComponentPlugin.h"
 
-#include <cstddef>
 #include <list>
 #include <optional>
 #include <string>
@@ -612,7 +611,8 @@ namespace armarx::plugins
     //****************************//
 
     FluxioExecutor*
-    SkillManagerComponentPlugin::executeFluxioSkill(std::string skillId, std::string executorName)
+    SkillManagerComponentPlugin::executeFluxioSkill(std::string skillId,
+                                                    const std::string& executorName)
     {
         const auto& skill = getSkill(skillId);
         if (!skill.has_value())

source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h

@@ -75,7 +75,7 @@ namespace armarx::plugins
         //****************************//
 
         FluxioExecutor* executeFluxioSkill(std::string skillId,
-                                                      std::string executorName = "Fluxio");
+                                                      const std::string& executorName);
 
         void abortFluxioSkill(const std::string& executionId);
 

source/RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.cpp

@@ -160,7 +160,7 @@ namespace armarx
     SkillManagerComponentPluginUser::executeFluxioSkill(const std::string& skillId,
                                                         const Ice::Current& current)
     {
-        const auto& res = this->plugin->executeFluxioSkill(skillId);
+        const auto& res = this->plugin->executeFluxioSkill(skillId, "Fluxio");
         if (res != nullptr)
         {
             return res->id;