17 #include <RobotAPI/interface/aron/Aron.h>
18 #include <RobotAPI/interface/skills/SkillManagerInterface.h>
19 #include <RobotAPI/libraries/skills/core/aron/FluxioEdge.aron.generated.h>
20 #include <RobotAPI/libraries/skills/core/aron/FluxioNode.aron.generated.h>
21 #include <RobotAPI/libraries/skills/core/aron/FluxioParameter.aron.generated.h>
22 #include <RobotAPI/libraries/skills/core/aron/FluxioSkill.aron.generated.h>
34 std::optional<manager::dto::FluxioSkill>
39 ARMARX_WARNING <<
"SkillProvider for Skill with id " <<
id <<
" not set";
43 manager::dto::FluxioSkill
ret;
53 skills::manager::dto::FluxioParameterList ret_parameters;
56 ret_parameters.push_back(parameter.toManagerIce());
58 ret.parameters = ret_parameters;
60 skills::manager::dto::FluxioNodeList ret_nodes;
61 skills::manager::dto::FluxioEdgeList ret_edges;
64 ret.nodesHasValue =
false;
65 ret.edgesHasValue =
false;
69 ret.nodesHasValue =
true;
70 ret.edgesHasValue =
true;
72 for (
const auto& [
id, nodePtr] :
nodes)
74 const auto& node = nodePtr->toManagerIce();
76 if (!node.has_value())
78 ARMARX_WARNING <<
"Node with id " <<
id <<
" could not be converted";
82 ret_nodes.push_back(node.value());
85 for (
const auto& edge :
edges)
87 const auto& edgeDTO = edge.toManagerIce();
89 if (!edgeDTO.has_value())
95 ret_edges.push_back(edgeDTO.value());
98 ret.nodes = ret_nodes;
99 ret.edges = ret_edges;
106 manager::dto::FluxioIdentificator
109 manager::dto::FluxioIdentificator
ret;
117 manager::arondto::FluxioIdentificator
120 manager::arondto::FluxioIdentificator
ret;
130 std::map<std::string, FluxioProvider>& providersMap,
131 std::map<std::string, FluxioProfile>& profilesMap,
132 std::map<std::string, FluxioSkill>& skillsMap,
133 std::map<std::string, aron::type::ObjectPtr>& typesMap)
137 ARMARX_WARNING <<
"Trying to update Skill with id " <<
id <<
" with Skill with id "
161 std::map<std::string, manager::dto::FluxioParameter> parameterDTOMap;
162 for (
const auto& parameter : i.parameters)
164 parameterDTOMap[parameter.id] = parameter;
168 std::list<std::string> parameterIdsToDelete;
171 const auto& paramIt = parameterDTOMap.find(
id);
172 if (paramIt == parameterDTOMap.end())
174 parameterIdsToDelete.push_back(
id);
179 for (
const std::string& paramId : parameterIdsToDelete)
181 for (
auto& [k,
s] : skillsMap)
183 s.deleteParameter(paramId);
187 for (
const auto& [
id, paramDTO] : parameterDTOMap)
196 parameters[
id].updateFromIce(paramDTO, profilesMap, typesMap);
204 for (
const manager::dto::FluxioNode& node : i.nodes)
210 nodes.emplace(n->nodeId, std::move(n));
219 for (
const manager::dto::FluxioEdge& edge : i.edges)
225 edges.push_back(e.value());
239 std::map<std::string, FluxioProvider>& providersMap,
240 std::map<std::string, FluxioProfile>& profilesMap,
241 std::map<std::string, FluxioSkill>& skillsMap,
242 std::map<std::string, aron::type::ObjectPtr>& typesMap)
246 if (!dict->fullfillsType(manager::arondto::FluxioSkill::ToAronType()))
252 const manager::arondto::FluxioSkill i = manager::arondto::FluxioSkill::FromAron(d);
256 ARMARX_WARNING <<
"Trying to update Skill with id " <<
id <<
" with Skill with id "
275 i.skillProviderId, providersMap);
280 std::map<std::string, manager::arondto::FluxioParameter> parameterDTOMap;
281 for (
const auto& parameter : i.parameters)
283 parameterDTOMap[parameter.id] = parameter;
287 std::list<std::string> parameterIdsToDelete;
290 const auto& paramIt = parameterDTOMap.find(
id);
291 if (paramIt == parameterDTOMap.end())
293 parameterIdsToDelete.push_back(
id);
298 for (
const std::string& paramId : parameterIdsToDelete)
300 for (
auto& [k,
s] : skillsMap)
302 s.deleteParameter(paramId);
306 for (
const auto& [
id, paramDTO] : parameterDTOMap)
315 parameters[
id].updateFromAron(paramDTO, profilesMap, typesMap);
323 for (
const manager::arondto::FluxioNode& node : i.nodes)
329 nodes.emplace(n->nodeId, std::move(n));
338 for (
const manager::arondto::FluxioEdge& edge : i.edges)
344 edges.push_back(e.value());
358 bool keepParameterNodes)
360 std::vector<std::string> nodeIdsToDelete = {};
361 for (
const auto& [nodeId, nodePtr] :
nodes)
363 if (nodePtr ==
nullptr)
376 if (parameterNodePtr ==
nullptr)
382 if (parameterNodePtr->parameterPtr ==
nullptr)
387 if (parameterNodePtr->parameterPtr->id == parameterId)
396 nodeIdsToDelete.push_back(nodeId);
400 if (!keepParameterNodes)
402 for (
const auto&
id : nodeIdsToDelete)
419 std::vector<std::string> nodeIdsToDelete = {};
420 for (
const auto& [nodeId, nodePtr] :
nodes)
422 if (nodePtr ==
nullptr)
435 if (subSkillNodePtr ==
nullptr)
441 if (subSkillNodePtr->skillPtr ==
nullptr)
447 if (subSkillNodePtr->skillPtr->id == skillId)
454 nodeIdsToDelete.push_back(nodeId);
458 for (
const auto&
id : nodeIdsToDelete)
486 std::map<std::string, FluxioSkill>& skillsMap)
488 const auto& skillIt = skillsMap.find(i.id);
490 if (skillIt == skillsMap.end())
501 std::map<std::string, FluxioSkill>& skillsMap)
503 const auto& skillIt = skillsMap.find(i.id);
505 if (skillIt == skillsMap.end())
514 std::unique_ptr<FluxioSkill>
516 std::map<std::string, FluxioProvider>& providersMap,
517 std::map<std::string, FluxioProfile>& profilesMap,
518 std::map<std::string, FluxioSkill>& skillsMap,
519 std::map<std::string, aron::type::ObjectPtr>& typesMap)
521 const auto& providerPtr =
523 if (providerPtr ==
nullptr)
525 ARMARX_WARNING <<
"Provider for Skill with id " << i.id <<
" not found";
533 ret.description = i.description;
535 ret.lastChanged = i.lastChanged;
536 ret.executable = i.executable;
537 ret.native = i.native;
538 ret.skillProviderPtr = providerPtr;
539 ret.parameters = std::map<std::string, FluxioParameter>();
540 ret.nodes = std::map<const std::string, const std::unique_ptr<FluxioNode>>();
541 ret.edges = std::list<FluxioEdge>();
543 for (
const manager::dto::FluxioParameter& parameter : i.parameters)
547 ret.parameters[param.
id] = param;
552 return std::make_unique<FluxioSkill>(std::move(
ret));
557 for (
const manager::dto::FluxioNode& node : i.nodes)
559 auto n =
CreateNode(node,
ret.parameters, skillsMap, profilesMap, typesMap);
563 ret.nodes.emplace(n->nodeId, std::move(n));
570 for (
const manager::dto::FluxioEdge& edge : i.edges)
576 ret.edges.push_back(e.value());
581 return std::make_unique<FluxioSkill>(std::move(
ret));
584 std::unique_ptr<FluxioSkill>
586 std::map<std::string, FluxioProvider>& providersMap,
587 std::map<std::string, FluxioProfile>& profilesMap,
588 std::map<std::string, FluxioSkill>& skillsMap,
589 std::map<std::string, aron::type::ObjectPtr>& typesMap)
592 if (!dict->fullfillsType(manager::arondto::FluxioSkill::ToAronType()))
598 const manager::arondto::FluxioSkill i = manager::arondto::FluxioSkill::FromAron(d);
600 const auto& providerPtr =
603 if (providerPtr ==
nullptr)
605 ARMARX_WARNING <<
"Provider for Skill with id " << i.id <<
" not found";
613 ret.description = i.description;
614 ret.timeout = i.timeout;
615 ret.lastChanged = i.lastChanged;
616 ret.executable = i.executable;
617 ret.native = i.native;
618 ret.skillProviderPtr = providerPtr;
619 ret.parameters = std::map<std::string, FluxioParameter>();
620 ret.nodes = std::map<const std::string, const std::unique_ptr<FluxioNode>>();
621 ret.edges = std::list<FluxioEdge>();
623 for (
const manager::arondto::FluxioParameter& parameter : i.parameters)
627 ret.parameters[param.
id] = param;
632 return std::make_unique<FluxioSkill>(std::move(
ret));
637 for (
const manager::arondto::FluxioNode& node : i.nodes)
639 auto n =
CreateNode(node,
ret.parameters, skillsMap, profilesMap, typesMap);
643 ret.nodes.emplace(n->nodeId, std::move(n));
650 for (
const manager::arondto::FluxioEdge& edge : i.edges)
656 ret.edges.push_back(e.value());
661 return std::make_unique<FluxioSkill>(std::move(
ret));
664 std::unique_ptr<FluxioNode>
666 std::map<std::string, FluxioParameter>& parametersMap,
667 std::map<std::string, FluxioSkill>& skillsMap,
668 std::map<std::string, FluxioProfile>& profilesMap,
669 std::map<std::string, aron::type::ObjectPtr>& typesMap)
679 return std::make_unique<FluxioParameterNode>(n.value());
682 ARMARX_WARNING <<
"ParameterNode with id " << i.nodeId <<
" could not be converted";
690 return std::make_unique<FluxioSubSkillNode>(n.value());
692 ARMARX_WARNING <<
"SubSkillNode with id " << i.nodeId <<
" could not be converted";
700 return std::make_unique<FluxioControlNode>(n.value());
702 ARMARX_WARNING <<
"controlNode with id " << i.nodeId <<
" could not be converted";
706 ARMARX_WARNING <<
"Node with id " << i.nodeId <<
" has unknown type";
710 ARMARX_INFO <<
"Node type " << i.nodeType <<
" not supported yet. Ignoring.";
716 std::unique_ptr<FluxioNode>
718 std::map<std::string, FluxioParameter>& parametersMap,
719 std::map<std::string, FluxioSkill>& skillsMap,
720 std::map<std::string, FluxioProfile>& profilesMap,
721 std::map<std::string, aron::type::ObjectPtr>& typesMap)
731 return std::make_unique<FluxioParameterNode>(n.value());
734 ARMARX_WARNING <<
"ParameterNode with id " << i.nodeId <<
" could not be converted";
742 return std::make_unique<FluxioSubSkillNode>(n.value());
744 ARMARX_WARNING <<
"SubSkillNode with id " << i.nodeId <<
" could not be converted";
752 return std::make_unique<FluxioControlNode>(n.value());
754 ARMARX_WARNING <<
"controlNode with id " << i.nodeId <<
" could not be converted";
758 ARMARX_WARNING <<
"Node with id " << i.nodeId <<
" has unknown type";
762 ARMARX_INFO <<
"Node type " << i.nodeType <<
" not supported yet. Ignoring.";
768 std::optional<manager::arondto::FluxioSkill>
773 ARMARX_WARNING <<
"SkillProvider for Skill with id " <<
id <<
" not set";
777 manager::arondto::FluxioSkill
ret;
787 std::vector<manager::arondto::FluxioParameter> ret_parameters;
789 for (
const auto& [
id, parameter] :
parameters)
791 ret_parameters.push_back(parameter.toAron());
793 ret.parameters = ret_parameters;
795 std::vector<manager::arondto::FluxioNode> ret_nodes;
796 std::vector<manager::arondto::FluxioEdge> ret_edges;
799 ret.nodesHasValue =
false;
800 ret.edgesHasValue =
false;
804 ret.nodesHasValue =
true;
805 ret.edgesHasValue =
true;
807 for (
const auto& [
id, nodePtr] :
nodes)
809 const auto& node = nodePtr->toAron();
811 if (!node.has_value())
813 ARMARX_WARNING <<
"Node with id " <<
id <<
" could not be converted";
817 ret_nodes.push_back(node.value());
820 for (
const auto& edge :
edges)
822 const auto& edgeDTO = edge.toAron();
824 if (!edgeDTO.has_value())
830 ret_edges.push_back(edgeDTO.value());
833 ret.nodes = ret_nodes;
834 ret.edges = ret_edges;
841 std::optional<aron::data::dto::DictPtr>
844 const std::optional<manager::arondto::FluxioSkill>
ret =
toAronXml();
845 if (!
ret.has_value())