diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index dbaaf4873bd64b8aede4752f0b8cdc8de3029579..16f091a3ac5ca8ae10f5b812761b179b86efc6ea 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -43,7 +43,9 @@ set(LIB_FILES
NJointControllers/NJointController.cpp
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
NJointControllers/NJointTCPController.cpp
@@ -121,8 +123,11 @@ set(LIB_HEADERS
NJointControllers/NJointControllerRegistry.h
NJointControllers/NJointControllerWithTripleBuffer.h
NJointControllers/NJointTrajectoryController.h
+ NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
+ NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+ NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
NJointControllers/NJointHolonomicPlatformRelativePositionController.h
NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
NJointControllers/NJointTCPController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index bd440c1b0b124795cbe519299bfca7931a64d3bf..892a1ae7e554387630efa35d048ee6cd44f02584 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -32,8 +32,7 @@ namespace armarx
NJointHolonomicPlatformUnitVelocityPassThroughController(
RobotUnit* prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
- const VirtualRobot::RobotPtr&) :
- maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ const VirtualRobot::RobotPtr&)
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -72,32 +71,6 @@ namespace armarx
}
}
- void
- NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX,
- float velocityY,
- float velocityRotation)
- {
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
- getWriterControlStruct().velocityRotation = velocityRotation;
- getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
- writeControlStruct();
- }
-
- IceUtil::Time
- NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
- {
- return maxCommandDelay;
- }
-
- void
- NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(
- const IceUtil::Time& value)
- {
- maxCommandDelay = value;
- }
-
NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
"NJointHolonomicPlatformUnitVelocityPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 37df1f055d7daeb61630e62698585e484ed6cc75..ef08635a5f8329e80020f47b1eb8b49b3a894b7a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,6 +24,8 @@
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
+
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "../util.h"
#include "NJointControllerWithTripleBuffer.h"
@@ -45,24 +47,12 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
- class NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData
- {
- public:
- float velocityX = 0;
- float velocityY = 0;
- float velocityRotation = 0;
- IceUtil::Time commandTimestamp;
- };
-
- TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
-
/**
* @brief The NJointHolonomicPlatformUnitVelocityPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
class NJointHolonomicPlatformUnitVelocityPassThroughController :
- virtual public NJointControllerWithTripleBuffer<
- NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
+ virtual public NJointHolonomicPlatformVelocityControllerInterface
{
public:
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
@@ -74,9 +64,6 @@ namespace armarx
void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
- //for the platform unit
- void setVelocites(float velocityX, float velocityY, float velocityRotation);
-
//ice interface
std::string
getClassName(const Ice::Current& = Ice::emptyCurrent) const override
@@ -84,13 +71,8 @@ namespace armarx
return "NJointHolonomicPlatformUnitVelocityPassThroughController";
}
- IceUtil::Time getMaxCommandDelay() const;
- void setMaxCommandDelay(const IceUtil::Time& value);
-
protected:
- IceUtil::Time maxCommandDelay;
-
ControlTargetHolonomicPlatformVelocity* target;
- NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
+ NJointHolonomicPlatformVelocityControllerControlData initialSettings;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0db22289e2d7be967f79ac044a88270c06f4a47f
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "NJointHolonomicPlatformVelocityControllerInterface.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+namespace armarx
+{
+ NJointHolonomicPlatformVelocityControllerInterface::
+ NJointHolonomicPlatformVelocityControllerInterface() :
+ maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ {
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerInterface::setVelocites(float velocityX,
+ float velocityY,
+ float velocityRotation)
+ {
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
+ getWriterControlStruct().velocityRotation = velocityRotation;
+ getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
+ writeControlStruct();
+ }
+
+ IceUtil::Time
+ NJointHolonomicPlatformVelocityControllerInterface::getMaxCommandDelay() const
+ {
+ return maxCommandDelay;
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerInterface::setMaxCommandDelay(
+ const IceUtil::Time& value)
+ {
+ maxCommandDelay = value;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a9acbe71cf958b2a1027c639466972221948965
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerInterface
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "NJointControllerWithTripleBuffer.h"
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerControlData);
+
+ class NJointHolonomicPlatformVelocityControllerControlData
+ {
+ public:
+ float velocityX = 0;
+ float velocityY = 0;
+ float velocityRotation = 0;
+ IceUtil::Time commandTimestamp;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerInterface);
+
+ class NJointHolonomicPlatformVelocityControllerInterface :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformVelocityControllerControlData>
+ {
+ public:
+ NJointHolonomicPlatformVelocityControllerInterface();
+
+
+ void setVelocites(float velocityX, float velocityY, float velocityRotation);
+
+ IceUtil::Time getMaxCommandDelay() const;
+ void setMaxCommandDelay(const IceUtil::Time& value);
+
+ protected:
+ IceUtil::Time maxCommandDelay;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
new file mode 100644
index 0000000000000000000000000000000000000000..c864e700da2ac4c4159893c8099de48e43ee87ae
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerTypes
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx
+{
+ enum class NJointHolonomicPlatformVelocityControllerTypes
+ {
+ PassThroughController,
+ ControllerWithRamp
+ };
+
+
+ inline const simox::meta::EnumNames<NJointHolonomicPlatformVelocityControllerTypes>
+ NJointHolonomicPlatformVelocityControllerTypesNames{
+ {NJointHolonomicPlatformVelocityControllerTypes::PassThroughController,
+ "PassThroughController"},
+ {NJointHolonomicPlatformVelocityControllerTypes::ControllerWithRamp,
+ "ControllerWithRamp"}};
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e5642d3fb78c95afd81644dec24d4c13f313ff4
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -0,0 +1,159 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "NJointHolonomicPlatformVelocityControllerWithRamp.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+namespace armarx
+{
+ NJointHolonomicPlatformVelocityControllerWithRamp::
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT cfg,
+ const VirtualRobot::RobotPtr&) :
+ ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
+ {
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator " << cfg->platformName << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
+
+ const auto sensor = useSensorValue(cfg->platformName);
+ ARMARX_CHECK_EXPRESSION(sensor) << "No sensor value for " << cfg->platformName;
+ velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(velocitySensor)
+ << "Sensor value for " << cfg->platformName << " has invalid type";
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
+ {
+ activationVelocity(0) = velocitySensor->velocityX;
+ activationVelocity(1) = velocitySensor->velocityY;
+ activationVelocity(2) = velocitySensor->velocityRotation;
+ activationTime = IceUtil::Time::now();
+
+ // init velocity ramp
+ ramp.init(activationVelocity);
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+ rtGetControlStruct().velocityRotation !=
+ 0.0f)) // only throw error if any command is not zero
+ {
+ throw LocalException(
+ "Platform target velocity was not set for a too long time: delay: ")
+ << commandAge.toSecondsDouble()
+ << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ }
+ else
+ {
+ Eigen::Vector3f result;
+ if (activationTime > rtGetControlStruct().commandTimestamp)
+ {
+ result = ramp.update(activationVelocity, timeSinceLastIteration.toSecondsDouble());
+ }
+ else
+ {
+ Eigen::Vector3f x(rtGetControlStruct().velocityX,
+ rtGetControlStruct().velocityY,
+ rtGetControlStruct().velocityRotation);
+ result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+ }
+
+ target->velocityX = result(0);
+ target->velocityY = result(1);
+ target->velocityRotation = result(2);
+ }
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration)
+ {
+ ramp.setMaxPositionAcceleration(maxPositionAcceleration);
+ ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+ }
+
+ NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
+ registrationNJointHolonomicPlatformVelocityControllerWithRamp(
+ "NJointHolonomicPlatformVelocityControllerWithRamp");
+
+ Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
+ float maxOrientationAcceleration) :
+ maxPositionAcceleration(maxPositionAcceleration),
+ maxOrientationAcceleration(maxOrientationAcceleration)
+ {
+ }
+
+ void
+ Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
+ {
+ this->state = state;
+ }
+
+ Eigen::Vector3f
+ Cartesian2DimVelocityRamp::update(const Eigen::Vector3f& target, float dt)
+ {
+ if (dt <= 0)
+ {
+ return state;
+ }
+ Eigen::Vector3f delta = target - state;
+ float factor = 1;
+
+ Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
+ float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
+ factor = std::max(factor, posFactor);
+
+ float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
+ factor = std::max(factor, oriFactor);
+
+ state += delta / factor;
+ return state;
+ }
+
+ void
+ Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+ {
+ this->maxPositionAcceleration = maxPositionAcceleration;
+ }
+
+ void
+ Cartesian2DimVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+ {
+ this->maxOrientationAcceleration = maxOrientationAcceleration;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..8cd957b50d7911524c6e5d68a91ad4359213f145
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -0,0 +1,102 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerInterface
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
+
+#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+#include "NJointControllerWithTripleBuffer.h"
+
+namespace armarx
+{
+
+ class Cartesian2DimVelocityRamp
+ {
+ public:
+ Cartesian2DimVelocityRamp(float maxPositionAcceleration, float maxOrientationAcceleration);
+ void init(const Eigen::Vector3f& state);
+
+ Eigen::Vector3f update(const Eigen::Vector3f& target, float dt);
+
+ void setMaxPositionAcceleration(float maxPositionAcceleration);
+ void setMaxOrientationAcceleration(float maxOrientationAcceleration);
+
+ private:
+ float maxPositionAcceleration = 0;
+ float maxOrientationAcceleration = 0;
+
+ Eigen::Vector3f state; // [velX,velY,velRotation]
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampConfig);
+
+ class NJointHolonomicPlatformVelocityControllerWithRampConfig :
+ virtual public NJointControllerConfig
+ {
+ public:
+ std::string platformName;
+
+ float maxPositionAcceleration;
+ float maxOrientationAcceleration;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRamp);
+
+ class NJointHolonomicPlatformVelocityControllerWithRamp :
+ virtual public NJointHolonomicPlatformVelocityControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointHolonomicPlatformVelocityControllerWithRampConfigPtr;
+
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT config,
+ const VirtualRobot::RobotPtr&);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration);
+
+ //ice interface
+ std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return "NJointHolonomicPlatformVelocityControllerWithRamp";
+ }
+
+ protected:
+ void rtPreActivateController() override;
+
+ protected:
+ Cartesian2DimVelocityRamp ramp;
+
+ Eigen::Vector3f activationVelocity;
+ IceUtil::Time activationTime;
+
+ ControlTargetHolonomicPlatformVelocity* target;
+ const SensorValueHolonomicPlatformVelocity* velocitySensor;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 16af4f8c064391718d3aef43efac649030d41ed8..34168e9a242f8c43da13eb43e2a809594ebbc226 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -28,6 +28,8 @@
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
#include "../RobotUnit.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
@@ -449,23 +451,62 @@ namespace armarx::RobotUnitModule
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
ARMARX_TRACE;
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
- new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
- config->initialVelocityX = 0;
- config->initialVelocityY = 0;
- config->initialVelocityRotation = 0;
- config->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController",
- getName() + "_" + configName + "_VelPTContoller",
- config,
- false,
- true));
- ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(ctrl);
- unit->pt = ctrl;
- ARMARX_TRACE;
+ const NJointHolonomicPlatformVelocityControllerTypes platformControllerType =
+ getProperty<NJointHolonomicPlatformVelocityControllerTypes>(
+ "PlatformUnitVelocityControllerType");
+ if (platformControllerType ==
+ NJointHolonomicPlatformVelocityControllerTypes::PassThroughController)
+ {
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
+ new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ config->initialVelocityX = 0;
+ config->initialVelocityY = 0;
+ config->initialVelocityRotation = 0;
+ config->platformName = _module<RobotData>().getRobotPlatformName();
+
+ auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
+ ARMARX_TRACE;
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ ARMARX_TRACE;
+ }
+ else if (platformControllerType ==
+ NJointHolonomicPlatformVelocityControllerTypes::ControllerWithRamp)
+ {
+ NJointHolonomicPlatformVelocityControllerWithRampConfigPtr config =
+ new NJointHolonomicPlatformVelocityControllerWithRampConfig;
+ config->platformName = _module<RobotData>().getRobotPlatformName();
+
+ config->maxPositionAcceleration =
+ getProperty<float>("PlatformUnitMaximumPositionAcceleration");
+ config->maxOrientationAcceleration =
+ getProperty<float>("PlatformUnitMaximumOrientationAcceleration");
+
+ auto ctrl = NJointHolonomicPlatformVelocityControllerWithRampPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformVelocityControllerWithRamp",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
+ ARMARX_TRACE;
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ ARMARX_TRACE;
+ }
+ else
+ {
+ ARMARX_ERROR << "Invalid Platform velocity controller specified '"
+ << NJointHolonomicPlatformVelocityControllerTypesNames.to_name(
+ platformControllerType)
+ << "'";
+ }
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -479,7 +520,6 @@ namespace armarx::RobotUnitModule
false,
true));
ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
- unit->pt = ctrl;
unit->relativePosCtrl = ctrlRelativePosition;
// unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
@@ -496,7 +536,6 @@ namespace armarx::RobotUnitModule
true));
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
- unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
ARMARX_TRACE;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 6a423722690a411ab270477bcacd387118ba8d61..f33c7ee47065d3bb7c5f1302d806d3af8229920a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -22,6 +22,7 @@
#pragma once
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../Units/RobotUnitSubUnit.h"
@@ -43,6 +44,25 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::string>(
"PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
+ auto& types = defineOptionalProperty<NJointHolonomicPlatformVelocityControllerTypes>(
+ "PlatformUnitVelocityControllerType",
+ NJointHolonomicPlatformVelocityControllerTypes::PassThroughController,
+ "Which controller to use for velocity control of the platform");
+ for (const auto& [type, name] :
+ NJointHolonomicPlatformVelocityControllerTypesNames.map())
+ {
+ types.map(name, type);
+ }
+
+ defineOptionalProperty<float>(
+ "PlatformUnitMaximumPositionAcceleration",
+ 800,
+ "The maximum allowed acceleration for the position of the platform");
+ defineOptionalProperty<float>(
+ "PlatformUnitMaximumOrientationAcceleration",
+ 80,
+ "The maximum allowed acceleration for the orientation of the platform");
+
defineOptionalProperty<std::string>(
"ForceTorqueUnitName",
"ForceTorqueUnit",
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 90d5443bfd71a795ba244f650ae97b24988359e2..dd41099f1f79411eb23fcd7e484b9c57b7cd42a8 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -31,10 +31,10 @@
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include "../NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotUnitSubUnit.h"
@@ -93,7 +93,7 @@ namespace armarx
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
- NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
+ NJointHolonomicPlatformVelocityControllerInterfacePtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index 06dda7ed51718dff0061ea71682188aaa22113cb..b6acfc402a85a5fdc016aa84a0ce37fdb37ba003 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -154,6 +154,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
@@ -162,18 +172,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;
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index 9d067df7ed847624e6a6d07f9ececfe3669ee88d..30f2e949b575e22bf631abc06ca1c4ba8172efaf 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -124,6 +124,7 @@ namespace detail
// std::optional
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
{
if (bo.has_value())
@@ -138,6 +139,7 @@ namespace detail
}
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
{
if (dto.has_value())
@@ -230,6 +232,7 @@ namespace detail
// std::vector
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
dtos.clear();
@@ -240,6 +243,7 @@ namespace detail
}
}
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
bos.clear();
@@ -339,11 +343,13 @@ namespace armarx
// std::optional
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
{
armarx::aron::toAron(dto, bo);
}
template <class DtoT, class BoT>
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
{
armarx::aron::fromAron(dto, bo);
diff --git a/source/RobotAPI/libraries/skills/core/CMakeLists.txt b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
index b065e1833e569b8a5b39fe08afc116f377a6d6e3..a31f2913e57562b83d25615622af9cd753b3b18a 100644
--- a/source/RobotAPI/libraries/skills/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
@@ -15,6 +15,10 @@ armarx_add_library(
SOURCES
error/Exception.cpp
+ executor/FluxioExecutor.cpp
+ executor/FluxioNativeExecutor.cpp
+ executor/FluxioCompositeExecutor.cpp
+ executor/FluxioMergerExecutor.cpp
SkillID.cpp
ProviderID.cpp
ProviderInfo.cpp
@@ -33,6 +37,7 @@ armarx_add_library(
FluxioNode.cpp
FluxioEdge.cpp
FluxioSkill.cpp
+ FluxioControlNode.cpp
FluxioParameterNode.cpp
FluxioSubSkillNode.cpp
FluxioSkillStatusUpdate.cpp
@@ -40,6 +45,10 @@ armarx_add_library(
HEADERS
error/Exception.h
FluxioResult.h
+ executor/FluxioExecutor.h
+ executor/FluxioNativeExecutor.h
+ executor/FluxioCompositeExecutor.h
+ executor/FluxioMergerExecutor.h
SkillID.h
ProviderID.h
ProviderInfo.h
@@ -58,6 +67,7 @@ armarx_add_library(
FluxioNode.h
FluxioEdge.h
FluxioSkill.h
+ FluxioControlNode.h
FluxioParameterNode.h
FluxioSubSkillNode.h
FluxioSkillStatusUpdate.h
diff --git a/source/RobotAPI/libraries/skills/core/FluxioControlNode.cpp b/source/RobotAPI/libraries/skills/core/FluxioControlNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4cd5ee9285fb8dc2bf21d6f6dc29d8fd5c5bcb1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/FluxioControlNode.cpp
@@ -0,0 +1,115 @@
+#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
+ {
+ FluxioControlNodeType
+ FluxioControlNodeTypeFromString(const std::string& type)
+ {
+ if (type == "SPLITTER")
+ {
+ return FluxioControlNodeType::SPLITTER;
+ }
+ else if (type == "AND_MERGER")
+ {
+ return FluxioControlNodeType::AND_MERGER;
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown control node type: " << type;
+ return FluxioControlNodeType::UNKNOWN;
+ }
+ }
+
+ std::optional<std::string>
+ FluxioControlNodeTypeToString(const FluxioControlNodeType& type)
+ {
+ switch (type)
+ {
+ case FluxioControlNodeType::SPLITTER:
+ return "SPLITTER";
+ case FluxioControlNodeType::AND_MERGER:
+ return "AND_MERGER";
+ case FluxioControlNodeType::UNKNOWN:
+ default:
+ ARMARX_WARNING << "Unknown control node type: " << static_cast<int>(type);
+ return std::nullopt;
+ }
+ }
+
+ std::optional<manager::dto::FluxioNode>
+ FluxioControlNode::toManagerIce() const
+ {
+ const auto& nt = FluxioNodeTypeToString(nodeType);
+ if (!nt.has_value())
+ {
+ return std::nullopt;
+ }
+ const auto& ct = skills::FluxioControlNodeTypeToString(controlType);
+ 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 = ct.value();
+
+
+ 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 = skills::FluxioControlNodeTypeFromString(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
diff --git a/source/RobotAPI/libraries/skills/core/FluxioControlNode.h b/source/RobotAPI/libraries/skills/core/FluxioControlNode.h
new file mode 100644
index 0000000000000000000000000000000000000000..102ed2d5d2bd6ced5894a0aaf8c3b583030fafb7
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/FluxioControlNode.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "FluxioNode.h"
+#include "FluxioParameter.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+
+ enum class FluxioControlNodeType
+ {
+ UNKNOWN = 0,
+ SPLITTER = 1,
+ AND_MERGER = 2,
+ };
+
+ struct FluxioControlNode : public FluxioNode
+ {
+ FluxioControlNodeType 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);
+ FluxioControlNodeType FluxioControlNodeTypeFromString(const std::string& type);
+ std::optional<std::string>
+ FluxioControlNodeTypeToString(const FluxioControlNodeType& type);
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/FluxioEdge.cpp b/source/RobotAPI/libraries/skills/core/FluxioEdge.cpp
index 1dba082002933b9b732bcb9bc2f9250266359cbc..d81b3f59eb689998a6a9253a4d5908766837c1bb 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioEdge.cpp
+++ b/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)
{
diff --git a/source/RobotAPI/libraries/skills/core/FluxioNode.cpp b/source/RobotAPI/libraries/skills/core/FluxioNode.cpp
index 814c6b0c1731e7a77dcf434b0259b2e1b452c125..68601ff86ab6bdbc14ed1346acfde108e153153f 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioNode.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/skills/core/FluxioNode.h b/source/RobotAPI/libraries/skills/core/FluxioNode.h
index 8d2264ef312ed8b2c29d24027215cb26428a1fa6..d1594664ee768f3fe06ff4df918c9ef782641fef 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioNode.h
+++ b/source/RobotAPI/libraries/skills/core/FluxioNode.h
@@ -1,7 +1,7 @@
#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
diff --git a/source/RobotAPI/libraries/skills/core/FluxioParameter.h b/source/RobotAPI/libraries/skills/core/FluxioParameter.h
index 124d8690ea5fe63982c8342c943083d65eb45982..d6e1c2d41669905f3906bd39bc5a0cdd6df6c164 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioParameter.h
+++ b/source/RobotAPI/libraries/skills/core/FluxioParameter.h
@@ -19,7 +19,7 @@ namespace armarx
std::string type;
bool required = true;
bool isInput;
- std::list<FluxioValue> values = {};
+ std::list<FluxioValue> values;
manager::dto::FluxioParameter toManagerIce() const;
manager::dto::FluxioIdentificator toFluxioIdentificatorIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/FluxioParameterNode.cpp b/source/RobotAPI/libraries/skills/core/FluxioParameterNode.cpp
index b88dfcf2188f68ca657de8c18f56970c1bf34878..a814d9380dbbc55ad7c593c64bdb0c406c2d756f 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioParameterNode.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/skills/core/FluxioSkill.cpp b/source/RobotAPI/libraries/skills/core/FluxioSkill.cpp
index 7678016c6b0382585c007eb39069a5fb7792b2f5..b67f7e34b054a698c46717a22c869e5d3f14c7a7 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioSkill.cpp
+++ b/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.";
diff --git a/source/RobotAPI/libraries/skills/core/FluxioSkill.h b/source/RobotAPI/libraries/skills/core/FluxioSkill.h
index d9a9f7e9b9926ae068edc680b266c7d16ed393c1..791357933d8d1fcadc053cf7ad0f6d5ad46af56b 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioSkill.h
+++ b/source/RobotAPI/libraries/skills/core/FluxioSkill.h
@@ -1,8 +1,8 @@
#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
diff --git a/source/RobotAPI/libraries/skills/core/FluxioSubSkillNode.cpp b/source/RobotAPI/libraries/skills/core/FluxioSubSkillNode.cpp
index 529406a5c7c471d145150b16183c92538900b168..33e81216ff527af83dd16d45f4357aa763bb6f00 100644
--- a/source/RobotAPI/libraries/skills/core/FluxioSubSkillNode.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.cpp b/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d60649b2fe5e301264c5f5cc7fcba1ab58be9b6d
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.cpp
@@ -0,0 +1,650 @@
+#include "FluxioCompositeExecutor.h"
+
+#include <algorithm>
+#include <optional>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <IceUtil/UUID.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "../FluxioControlNode.h"
+#include "../FluxioEdge.h"
+#include "../FluxioParameter.h"
+#include "FluxioMergerExecutor.h"
+
+namespace armarx::skills
+{
+ FluxioCompositeExecutor::FluxioCompositeExecutor(
+ std::string& id,
+ skills::FluxioSkill* skill,
+ const std::function<void(const std::string& executionId)>&& abortFluxioSkillFunc,
+ const std::function<FluxioExecutor*(const std::string& skillId,
+ const std::string& executorName)>&&
+ executeFluxioSkillFunc) :
+ FluxioExecutor(id, false),
+ skill(skill),
+ abortFluxioSkill(abortFluxioSkillFunc),
+ executeFluxioSkill(executeFluxioSkillFunc)
+ {
+ }
+
+ void
+ FluxioCompositeExecutor::run(const std::string executorName,
+ armarx::aron::data::DictPtr parameters)
+ {
+
+ ARMARX_INFO << "Running skill " << skill->name;
+ setStatus(skills::SkillStatus::Constructing);
+
+ skills::FluxioEdge startEdge;
+ if (!validateSkill(startEdge))
+ {
+
+ ARMARX_WARNING << "Skill execution cancelled.";
+ return;
+ }
+
+ setStatus(skills::SkillStatus::Initializing);
+ subExecutionsMap.clear();
+ setStatus(skills::SkillStatus::Preparing);
+
+ std::atomic_bool skillRunning = true;
+
+ // thread for polling status updates
+ std::thread(
+ [this, &skillRunning]
+ {
+ while (skillRunning)
+ {
+ this->pollSubStatuses();
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ }
+ })
+ .detach();
+
+ // thread for the sub routines
+ const std::string newExecutorName = executorName + "/" + skill->name;
+ std::thread(
+ [this, &startEdge, &skillRunning, &newExecutorName]
+ {
+ this->startSubRoutine(
+ startEdge.toNodePtr, startEdge.toParameterPtr, skillRunning, newExecutorName);
+ })
+ .detach();
+
+ setStatus(skills::SkillStatus::Running);
+
+ // main loop
+ while (skillRunning)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ if (status->status == skills::SkillStatus::Aborted ||
+ status->status == skills::SkillStatus::Failed ||
+ status->status == skills::SkillStatus::Succeeded)
+ {
+
+ skillRunning = false;
+
+ // the skill was aborted with the abort method,no need to abort the
+ // SubExecutions twice
+ if (status->status != skills::SkillStatus::Aborted)
+ {
+ abortSubExecutions();
+ }
+
+ return;
+ }
+ }
+ }
+
+ void
+ FluxioCompositeExecutor::startSubRoutine(const skills::FluxioNode* startNode,
+ const skills::FluxioParameter* startParameter,
+ std::atomic_bool& running,
+ const std::string& executorName)
+ {
+ if (!running)
+ {
+ return;
+ }
+
+ if (startNode == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ if (startNode->nodeType == skills::FluxioNodeType::PARAMETER)
+ {
+ // cast to parameter node
+ const auto* paramNodePtr = dynamic_cast<const skills::FluxioParameterNode*>(startNode);
+ if (paramNodePtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr.Dynamic cast from FluxioNodePtr to "
+ "FluxioParameterNodePtr failed.";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ this->handleParameterRoutine(paramNodePtr, running, executorName);
+ }
+ else if (startNode->nodeType == skills::FluxioNodeType::CONTROL)
+ {
+ // cast to control node
+ const auto* controlNodePtr = dynamic_cast<const skills::FluxioControlNode*>(startNode);
+
+ if (controlNodePtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
+ "FluxioControlNodePtr failed.";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ this->handleControlRoutine(controlNodePtr, startParameter, running, executorName);
+ }
+ else if (startNode->nodeType == skills::FluxioNodeType::SUBSKILL)
+ {
+ // cast to subskill node
+ const auto* subSkillNodePtr =
+ dynamic_cast<const skills::FluxioSubSkillNode*>(startNode);
+ if (subSkillNodePtr == nullptr || subSkillNodePtr->skillPtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
+ "FluxioSubSkillNodePtr failed.";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ this->handleSubSkillRoutine(subSkillNodePtr, running, executorName);
+ }
+ else
+ {
+ ARMARX_WARNING
+ << "Unexpected node type '"
+ << skills::FluxioNodeTypeToString(startNode->nodeType).value_or("UNKNOWN")
+ << "' for Node with id " << startNode->nodeId;
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+ }
+
+ void
+ FluxioCompositeExecutor::handleParameterRoutine(
+ const skills::FluxioParameterNode* parameterNode,
+ std::atomic_bool& running,
+ const std::string& /*unused*/)
+ {
+ if (parameterNode == nullptr || parameterNode->parameterPtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // make sure it is an event parameter
+ if (parameterNode->parameterPtr->type != "Event")
+ {
+ ARMARX_WARNING << "Unexpected parameter type " << parameterNode->parameterPtr->type;
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // get the event type and set the status accordingly
+ const std::string& eventType = parameterNode->parameterPtr->name;
+ if (eventType == "Succeeded")
+ {
+ setStatus(skills::SkillStatus::Succeeded);
+ }
+ else if (eventType == "Failed")
+ {
+ setStatus(skills::SkillStatus::Failed);
+ }
+ else if (eventType == "Aborted")
+ {
+ setStatus(skills::SkillStatus::Aborted);
+ }
+ else
+ {
+ ARMARX_WARNING << "Unexpected event type " << eventType << " for parameter "
+ << parameterNode->parameterPtr->name;
+ setStatus(skills::SkillStatus::Failed);
+ }
+ }
+
+ void
+ FluxioCompositeExecutor::handleSubSkillRoutine(const skills::FluxioSubSkillNode* subSkillNode,
+ std::atomic_bool& running,
+ const std::string& executorName)
+ {
+ if (subSkillNode == nullptr || subSkillNode->skillPtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // start skill execution
+ auto* executorPtr = executeFluxioSkill(subSkillNode->skillPtr->id, executorName);
+ if (executorPtr == nullptr)
+ {
+ ARMARX_WARNING << "Failed to execute subskill " << subSkillNode->skillPtr->id;
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ subExecutionsMap.insert({subSkillNode->nodeId, executorPtr});
+
+ // wait until the skill has finished (or the super skill is finished)
+ skills::FluxioSkillStatusUpdate statusUpdate;
+ while (running)
+ {
+ // sleep for a while
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ executorPtr->getStatusUpdate(); // FIXME: bad design
+ const auto& statusUpdateIt = executorPtr->getStatus();
+
+ if (!statusUpdateIt.has_value())
+ {
+ ARMARX_INFO << "No status update from skill " << subSkillNode->skillPtr->name
+ << " yet. Waiting...";
+ continue;
+ }
+
+ statusUpdate = statusUpdateIt.value();
+ // did the status change? update statusUpdates list
+ const auto& lastUpdate =
+ std::find_if(statusUpdates.begin(),
+ statusUpdates.end(),
+ [&subSkillNode](const skills::FluxioSkillStatusUpdate& statusUpdate)
+ { return statusUpdate.subSkillNodeId == subSkillNode->nodeId; });
+
+ if (lastUpdate == statusUpdates.end() || lastUpdate->status != statusUpdate.status)
+ {
+ statusUpdates.push_front({armarx::DateTime::Now(),
+ executorPtr->id,
+ subSkillNode->nodeId,
+ statusUpdate.status});
+ }
+
+ // check subskill is finished
+ if (statusUpdate.status == skills::SkillStatus::Succeeded ||
+ statusUpdate.status == skills::SkillStatus::Failed ||
+ statusUpdate.status == skills::SkillStatus::Aborted)
+ {
+ break;
+ }
+ }
+
+ if (!running)
+ {
+ return;
+ }
+
+ // check the final skill status get the output event parameter
+ const std::string& outputEventName =
+ statusUpdate.status == skills::SkillStatus::Succeeded ? "Succeeded"
+ : statusUpdate.status == skills::SkillStatus::Failed ? "Failed"
+ : statusUpdate.status == skills::SkillStatus::Aborted ? "Aborted"
+ : "Undefined";
+ const auto& outputParam = std::find_if(
+ subSkillNode->skillPtr->parameters.begin(),
+ subSkillNode->skillPtr->parameters.end(),
+ [&outputEventName](const std::pair<std::string, skills::FluxioParameter>& param)
+ {
+ return (param.second.type == "Event" && !param.second.isInput &&
+ param.second.name == outputEventName);
+ });
+
+ if (outputParam == subSkillNode->skillPtr->parameters.end())
+ {
+ ARMARX_WARNING << "Skill " << subSkillNode->skillPtr->name
+ << " is missing the output event parameter " << outputEventName;
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // find the connected edge
+ const auto& edge =
+ std::find_if(skill->edges.begin(),
+ skill->edges.end(),
+ [&subSkillNode, &outputParam](const skills::FluxioEdge& edge)
+ {
+ return (edge.fromNodePtr->nodeId == subSkillNode->nodeId &&
+ edge.fromParameterPtr->id == outputParam->second.id);
+ });
+
+ if (edge == skill->edges.end())
+ {
+ ARMARX_WARNING << "Skill " << skill->name
+ << " has no edge connected to the output event parameter "
+ << outputEventName;
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // start new subroutine
+ const std::string& nextExecutorName = executorName + "/" + subSkillNode->skillPtr->name;
+ startSubRoutine(edge->toNodePtr, edge->toParameterPtr, running, nextExecutorName);
+ }
+
+ void
+ FluxioCompositeExecutor::handleControlRoutine(const skills::FluxioControlNode* controlNode,
+ const skills::FluxioParameter* startParameter,
+ std::atomic_bool& running,
+ const std::string& executorName)
+ {
+ if (controlNode == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // check the controlType
+ if (controlNode->controlType == skills::FluxioControlNodeType::SPLITTER)
+ {
+ // find connected nodes and store the relevant edges
+ std::vector<const skills::FluxioEdge*> edgePtrs = {};
+ for (const auto& [id, param] : controlNode->parametersMap)
+ {
+ // ignore the input parameters
+ if (param.isInput)
+ {
+ continue;
+ }
+
+ for (const auto& edge : skill->edges)
+ {
+ if (edge.fromParameterPtr != nullptr && edge.fromParameterPtr->id == id)
+ {
+ edgePtrs.push_back(&edge);
+ }
+ }
+ }
+
+ const std::string newExecutorName = executorName + "/Splitter";
+
+ // start subroutines in separate threads
+ for (const skills::FluxioEdge* edgePtr : edgePtrs)
+ {
+ std::thread(
+ [this, edgePtr, &running, &newExecutorName] {
+ startSubRoutine(
+ edgePtr->toNodePtr, edgePtr->toParameterPtr, running, newExecutorName);
+ })
+ .detach();
+ }
+ }
+ else if (controlNode->controlType == skills::FluxioControlNodeType::AND_MERGER)
+ {
+ // check the list of subexecutions for the node id
+ FluxioMergerExecutor* mergerExecutorPtr = nullptr;
+ const auto& executorPtr = subExecutionsMap.find(controlNode->nodeId);
+ if (executorPtr == subExecutionsMap.end())
+ {
+ // assemble paramId vector
+ std::vector<std::string> paramIds = {};
+ for (const auto& [id, param] : controlNode->parametersMap)
+ {
+ if (param.isInput)
+ {
+ paramIds.push_back(id);
+ }
+ }
+
+ // there is no execution for the merger yet, let´s start one
+ subExecutionsMap[controlNode->nodeId] =
+ new FluxioMergerExecutor(IceUtil::generateUUID(), paramIds);
+
+ mergerExecutorPtr =
+ dynamic_cast<FluxioMergerExecutor*>(subExecutionsMap[controlNode->nodeId]);
+ }
+ else
+ {
+ mergerExecutorPtr = dynamic_cast<FluxioMergerExecutor*>(executorPtr->second);
+ }
+
+ if (mergerExecutorPtr == nullptr)
+ {
+ ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioExecutorPtr to "
+ "FluxioMergerExecutorPtr failed.";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // check in the token
+ mergerExecutorPtr->checkInToken(startParameter->id);
+
+ if (mergerExecutorPtr->getStatus() != std::nullopt)
+ {
+ return;
+ }
+
+ // reuse this thread
+ mergerExecutorPtr->run(executorName, nullptr);
+
+ const auto& outputParam =
+ std::find_if(controlNode->parametersMap.begin(),
+ controlNode->parametersMap.end(),
+ [](const std::pair<std::string, skills::FluxioParameter>& param)
+ { return !param.second.isInput; });
+
+ if (outputParam == controlNode->parametersMap.end())
+ {
+ ARMARX_WARNING << "Control node " << controlNode->nodeId
+ << " has no output parameter";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // find the connected edge
+ const auto& edge =
+ std::find_if(skill->edges.begin(),
+ skill->edges.end(),
+ [&controlNode, &outputParam](const skills::FluxioEdge& edge)
+ {
+ return (edge.fromNodePtr->nodeId == controlNode->nodeId &&
+ edge.fromParameterPtr->id == outputParam->second.id);
+ });
+
+ if (edge == skill->edges.end())
+ {
+ ARMARX_WARNING
+ << "Skill " << skill->name
+ << " has no edge connected to the output event parameter of the AND merger";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+
+ // start new subroutine
+ startSubRoutine(edge->toNodePtr, edge->toParameterPtr, running, executorName);
+ }
+ else
+ {
+ ARMARX_WARNING << "Unexpected control type ";
+ setStatus(skills::SkillStatus::Failed);
+ return;
+ }
+ }
+
+ void
+ FluxioCompositeExecutor::abort()
+ {
+ ARMARX_INFO << "Aborting skill " << skill->name;
+ setStatus(skills::SkillStatus::Aborted);
+ abortSubExecutions();
+ }
+
+ void
+ FluxioCompositeExecutor::abortSubExecutions()
+ {
+ for (const auto& [nodeId, executorPtr] : subExecutionsMap)
+ {
+ auto s = executorPtr->getStatus();
+ if (!s.has_value() || s->status == skills::SkillStatus::Succeeded ||
+ s->status == skills::SkillStatus::Failed ||
+ s->status == skills::SkillStatus::Aborted)
+ {
+ continue;
+ }
+
+ executorPtr->abort();
+ statusUpdates.push_front(
+ {armarx::DateTime::Now(), executorPtr->id, nodeId, skills::SkillStatus::Aborted});
+ }
+ }
+
+ std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
+ FluxioCompositeExecutor::getStatusUpdate()
+ {
+ ARMARX_INFO << "Getting status updates for skill " << skill->name;
+ // convert statusupdates list to vector
+ auto ret = std::vector<skills::FluxioSkillStatusUpdate>(statusUpdates.begin(),
+ statusUpdates.end());
+
+ return ret;
+ }
+
+ void
+ FluxioCompositeExecutor::pollSubStatuses()
+ {
+ for (const auto& [nodeId, executorPtr] : subExecutionsMap)
+ {
+ executorPtr->getStatusUpdate();
+ auto s = executorPtr->getStatus();
+ if (!s.has_value())
+ {
+ continue;
+ }
+
+ const auto& lastStatus =
+ find_if(statusUpdates.begin(),
+ statusUpdates.end(),
+ [&](const skills::FluxioSkillStatusUpdate& statusUpdate)
+ { return statusUpdate.subSkillNodeId == nodeId; })
+ ->status;
+ if (lastStatus != s->status)
+ {
+ statusUpdates.push_front(
+ {armarx::DateTime::Now(), executorPtr->id, nodeId, s->status});
+ }
+ }
+ }
+
+ bool
+ FluxioCompositeExecutor::validateSkill(skills::FluxioEdge& ret)
+ {
+ // check if skill is not null
+ if (skill == nullptr)
+ {
+ ARMARX_WARNING << "Skill is not set";
+ return false;
+ }
+
+ // get start parameter
+ const auto& startParam =
+ std::find_if(skill->parameters.begin(),
+ skill->parameters.end(),
+ [](const std::pair<std::string, skills::FluxioParameter>& param) {
+ return (param.second.type == "Event" && param.second.isInput &&
+ param.second.name == "Start");
+ });
+
+ if (startParam == skill->parameters.end())
+ {
+ ARMARX_WARNING << "Skill has no start parameter";
+ return false;
+ }
+
+ // get all parameter nodes for the start parameter
+ const auto& startNode =
+ std::find_if(skill->nodes.begin(),
+ skill->nodes.end(),
+ [startParam](const std::pair<std::string, skills::FluxioNode*>& nodeEntry)
+ {
+ if (nodeEntry.second->nodeType != skills::FluxioNodeType::PARAMETER)
+ {
+ return false;
+ }
+
+ const auto* paramNode =
+ dynamic_cast<const skills::FluxioParameterNode*>(nodeEntry.second);
+ return (paramNode->parameterPtr->id == startParam->second.id);
+ });
+
+ // there can only be one
+ if (startNode == skill->nodes.end())
+ {
+ ARMARX_WARNING << "Skill has no start node";
+ return false;
+ }
+
+ // check if the start node is connected
+ const auto& startEdge =
+ std::find_if(skill->edges.begin(),
+ skill->edges.end(),
+ [startNode](const skills::FluxioEdge& edge)
+ { return (edge.fromNodePtr->nodeId == startNode->second->nodeId); });
+
+ // there can only be one
+ if (startEdge == skill->edges.end())
+ {
+ ARMARX_WARNING << "Skill has noedge connected to the start node";
+ return false;
+ }
+
+ // get the output event parameters
+ const auto& outputParamsSuccess =
+ std::find_if(skill->parameters.begin(),
+ skill->parameters.end(),
+ [](const std::pair<std::string, skills::FluxioParameter>& param)
+ {
+ return (param.second.type == "Event" && !param.second.isInput &&
+ param.second.name == "Succeeded");
+ });
+ const auto& outputParamsFailed =
+ std::find_if(skill->parameters.begin(),
+ skill->parameters.end(),
+ [](const std::pair<std::string, skills::FluxioParameter>& param)
+ {
+ return (param.second.type == "Event" && !param.second.isInput &&
+ param.second.name == "Failed");
+ });
+ const auto& outputParamsAborted =
+ std::find_if(skill->parameters.begin(),
+ skill->parameters.end(),
+ [](const std::pair<std::string, skills::FluxioParameter>& param)
+ {
+ return (param.second.type == "Event" && !param.second.isInput &&
+ param.second.name == "Aborted");
+ });
+
+ if (outputParamsSuccess == skill->parameters.end() ||
+ outputParamsFailed == skill->parameters.end() ||
+ outputParamsAborted == skill->parameters.end())
+ {
+ ARMARX_WARNING << "Skill is missing one or more output event parameters";
+ return false;
+ }
+
+ // TODO: the rest
+
+ ARMARX_INFO << "Skill validation is not fully implemented yet.";
+ ret = *startEdge;
+ return true;
+ }
+
+ void
+ FluxioCompositeExecutor::setStatus(skills::SkillStatus status)
+ {
+ const skills::FluxioSkillStatusUpdate update = {
+ armarx::DateTime::Now(), id, skill->id, status};
+ this->status = update;
+ this->statusUpdates.push_front(update);
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.h b/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.h
new file mode 100644
index 0000000000000000000000000000000000000000..5fc27bb61cfd921c895d5c99fb4a11bee044c753
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <atomic>
+#include <optional>
+#include <string>
+
+#include "../FluxioControlNode.h"
+#include "../FluxioParameter.h"
+#include "../FluxioParameterNode.h"
+#include "../FluxioSkill.h"
+#include "../FluxioSkillStatusUpdate.h"
+#include "../FluxioSubSkillNode.h"
+#include "../SkillExecutionID.h"
+#include "../SkillStatusUpdate.h"
+#include "FluxioExecutor.h"
+
+namespace armarx::skills
+{
+ class FluxioCompositeExecutor : public FluxioExecutor
+ {
+ public:
+ FluxioCompositeExecutor(
+ std::string& id,
+ skills::FluxioSkill* skill,
+ const std::function<void(const std::string& executionId)>&& abortFluxioSkillFunc,
+ const std::function<FluxioExecutor*(const std::string& skillId,
+ const std::string& executorName)>&&
+ executeFluxioSkillFunc);
+ 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::FluxioEdge& ret);
+
+ private:
+ void startSubRoutine(const skills::FluxioNode* startNode,
+ const skills::FluxioParameter* startParameter,
+ std::atomic_bool& running,
+ const std::string& executorName);
+
+ void handleParameterRoutine(const skills::FluxioParameterNode* parameterNode,
+ std::atomic_bool& running,
+ const std::string& executorName);
+ void handleSubSkillRoutine(const skills::FluxioSubSkillNode* subSkillNode,
+ std::atomic_bool& running,
+ const std::string& executorName);
+ void handleControlRoutine(const skills::FluxioControlNode* controlNode,
+ const skills::FluxioParameter* startParameter,
+ 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 node id
+ const std::function<void(const std::string& executionId)> abortFluxioSkill;
+ const std::function<FluxioExecutor*(const std::string& skillId,
+ const std::string& executorName)>
+ executeFluxioSkill;
+ };
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioExecutor.cpp b/source/RobotAPI/libraries/skills/core/executor/FluxioExecutor.cpp
similarity index 100%
rename from source/RobotAPI/libraries/skills/manager/FluxioExecutor.cpp
rename to source/RobotAPI/libraries/skills/core/executor/FluxioExecutor.cpp
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioExecutor.h b/source/RobotAPI/libraries/skills/core/executor/FluxioExecutor.h
similarity index 52%
rename from source/RobotAPI/libraries/skills/manager/FluxioExecutor.h
rename to source/RobotAPI/libraries/skills/core/executor/FluxioExecutor.h
index 9701c5c48d2dab0a5807bba985d7f10b77ff3b75..9ca0992451f5725ccd6353b967c5ad4fb8e7b09b 100644
--- a/source/RobotAPI/libraries/skills/manager/FluxioExecutor.h
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioExecutor.h
@@ -1,25 +1,21 @@
#pragma once
+#include <list>
#include <string>
#include <vector>
-#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
+#include "../FluxioSkillStatusUpdate.h"
-#include "SkillManagerComponentPlugin.h"
-
-namespace armarx::plugins
+namespace armarx::skills
{
- class SkillManagerComponentPlugin; // forward declaration
-
class FluxioExecutor
{
public:
virtual ~FluxioExecutor(){};
- 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 abort(){};
+ FluxioExecutor(const std::string& id, bool native) : id(id), native(native){};
+ virtual void run(const std::string executorName, armarx::aron::data::DictPtr parameters) {};
+ virtual void abort() {};
virtual std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
getStatusUpdate()
@@ -37,10 +33,9 @@ namespace armarx::plugins
const bool native;
protected:
- std::list<skills::FluxioSkillStatusUpdate> statusUpdates = {};
+ std::list<skills::FluxioSkillStatusUpdate> statusUpdates;
std::string* executorName = nullptr;
- SkillManagerComponentPlugin* plugin = nullptr;
std::optional<skills::FluxioSkillStatusUpdate> status = std::nullopt;
- virtual void setStatus(skills::SkillStatus status){};
+ virtual void setStatus(skills::SkillStatus status) {};
};
-} // namespace armarx::plugins
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.cpp b/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8f6728479e34e56d24943de75ac48e785fd3596
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.cpp
@@ -0,0 +1,97 @@
+#include "FluxioMergerExecutor.h"
+
+#include <optional>
+#include <thread>
+#include <vector>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "../SkillStatusUpdate.h"
+
+namespace armarx::skills
+{
+ FluxioMergerExecutor::FluxioMergerExecutor(const std::string& id,
+ const std::vector<std::string>& parameterIds) :
+ FluxioExecutor(id, false)
+ {
+ for (const auto& id : parameterIds)
+ {
+ tokenHasArrivedMap[id] = false;
+ }
+ ARMARX_WARNING << "Fluxio Merger is waiting for the following tokens to arrive: "
+ << parameterIds;
+ }
+
+ void
+ FluxioMergerExecutor::run(const std::string /*unused*/, armarx::aron::data::DictPtr /*unused*/)
+ {
+ // set status running
+ setStatus(skills::SkillStatus::Running);
+
+ // wait for all tokens to arrive
+ while (true)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+
+ // check if all tokens have arrived
+ bool allTokensArrived = true;
+ for (const auto& [id, hasArrived] : tokenHasArrivedMap)
+ {
+ if (!hasArrived)
+ {
+ allTokensArrived = false;
+ break;
+ }
+ }
+
+ if (status->status == skills::SkillStatus::Aborted ||
+ status->status == skills::SkillStatus::Failed ||
+ status->status == skills::SkillStatus::Succeeded)
+ {
+ return;
+ }
+
+ if (allTokensArrived)
+ {
+ setStatus(skills::SkillStatus::Succeeded);
+ return;
+ }
+ }
+ }
+
+ void
+ FluxioMergerExecutor::abort()
+ {
+ setStatus(skills::SkillStatus::Aborted);
+ }
+
+ void
+ FluxioMergerExecutor::checkInToken(const std::string& parameterId)
+ {
+ const auto it = tokenHasArrivedMap.find(parameterId);
+ if (it == tokenHasArrivedMap.end())
+ {
+ ARMARX_WARNING << "Unexpected parameterId: " << parameterId;
+ }
+ else
+ {
+ ARMARX_WARNING << "Fluxio Merger received token for parameterId: " << parameterId;
+ tokenHasArrivedMap[parameterId] = true;
+ }
+ }
+
+ std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
+ FluxioMergerExecutor::getStatusUpdate()
+ {
+ // unused method
+ return std::nullopt;
+ }
+
+ void
+ FluxioMergerExecutor::setStatus(skills::SkillStatus status)
+ {
+ const skills::FluxioSkillStatusUpdate update = {
+ armarx::DateTime::Now(), this->id, "noIdNeeded", status};
+ this->status = update;
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.h b/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.h
new file mode 100644
index 0000000000000000000000000000000000000000..5f8bcfff89649aec9f9747d4a588705a498af163
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioMergerExecutor.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <optional>
+#include <string>
+
+#include "../FluxioSkillStatusUpdate.h"
+#include "FluxioExecutor.h"
+
+namespace armarx::skills
+{
+ class FluxioMergerExecutor : public FluxioExecutor
+ {
+ public:
+ FluxioMergerExecutor(const std::string& id, const std::vector<std::string>& parameterIds);
+
+ void run(const std::string executorName, armarx::aron::data::DictPtr parameters) override;
+ void abort() override;
+ void checkInToken(const std::string& parameterId);
+ std::optional<std::vector<skills::FluxioSkillStatusUpdate>> getStatusUpdate() override;
+
+ private:
+ void setStatus(skills::SkillStatus status) override;
+ std::map<std::string, bool> tokenHasArrivedMap; // keys are parameterIds
+ };
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.cpp b/source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.cpp
similarity index 55%
rename from source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.cpp
rename to source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.cpp
index 89c09c6977942ba317e85204e03b907369e03d58..e3b2f03fbd531c571b4b5d36349b49b250b1ec15 100644
--- a/source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.cpp
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.cpp
@@ -4,30 +4,40 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
-#include "RobotAPI/libraries/skills/core/ProviderID.h"
-#include "RobotAPI/libraries/skills/core/SkillExecutionID.h"
-#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"
-#include "RobotAPI/libraries/skills/core/SkillID.h"
+#include "../ProviderID.h"
+#include "../SkillExecutionRequest.h"
+#include "../SkillID.h"
-namespace armarx::plugins
+namespace armarx::skills
{
- FluxioNativeExecutor::FluxioNativeExecutor(std::string& id,
- SkillManagerComponentPlugin* plugin,
- skills::SkillID& skillId) :
- FluxioExecutor(id, plugin, true), skillId(skillId)
+ FluxioNativeExecutor::FluxioNativeExecutor(
+ std::string& id,
+ skills::SkillID& skillId,
+ std::string& fluxioUUID,
+ const std::function<bool(const skills::SkillExecutionID&)>&& abortSkillFunc,
+ const std::function<skills::SkillExecutionID(const skills::SkillExecutionRequest&)>&&
+ executeSkillAsyncFunc,
+ const std::function<std::optional<skills::SkillStatusUpdate>(
+ const skills::SkillExecutionID&)>&& getSkillExecutionStatusFunc) :
+ FluxioExecutor(id, true),
+ skillId(skillId),
+ fluxioUUID(fluxioUUID),
+ abortSkill(abortSkillFunc),
+ executeSkillAsync(executeSkillAsyncFunc),
+ getSkillExecutionStatus(getSkillExecutionStatusFunc)
{
}
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;
req.parameters = parameters;
req.executorName = executorName;
- auto eid = this->plugin->executeSkillAsync(req);
+ auto eid = executeSkillAsync(req);
this->executionId = eid;
}
@@ -35,7 +45,7 @@ namespace armarx::plugins
void
FluxioNativeExecutor::abort()
{
- if (!this->executionId.has_value() || this->plugin == nullptr)
+ if (!this->executionId.has_value())
{
// error
ARMARX_WARNING << "Execution ID or plugin is not set";
@@ -43,13 +53,13 @@ namespace armarx::plugins
}
// TODO: check last execution status
- this->plugin->abortSkill(this->executionId.value());
+ abortSkill(this->executionId.value());
}
std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
FluxioNativeExecutor::getStatusUpdate()
{
- if (!this->executionId.has_value() || this->plugin == nullptr)
+ if (!this->executionId.has_value())
{
// error
ARMARX_WARNING << "Execution ID or plugin is not set";
@@ -58,7 +68,7 @@ namespace armarx::plugins
auto executionId = this->executionId.value();
- auto status = this->plugin->getSkillExecutionStatus(executionId);
+ auto status = getSkillExecutionStatus(executionId);
if (!status.has_value())
{
@@ -69,7 +79,7 @@ namespace armarx::plugins
}
skills::FluxioSkillStatusUpdate update = {
- armarx::DateTime::Now(), this->id, this->skillId.skillName, status.value().status};
+ armarx::DateTime::Now(), this->id, this->fluxioUUID, status.value().status};
// newest statusupdate is aways at the end of the vector
if (this->statusUpdates.empty() || update.status != this->statusUpdates.front().status)
@@ -85,8 +95,8 @@ namespace armarx::plugins
FluxioNativeExecutor::setStatus(skills::SkillStatus status)
{
const skills::FluxioSkillStatusUpdate update = {
- armarx::DateTime::Now(), this->id, this->skillId.skillName, status};
+ armarx::DateTime::Now(), this->id, this->fluxioUUID, status};
this->status = update;
this->statusUpdates.push_front(update);
}
-} // namespace armarx::plugins
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.h b/source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b8288cac2887a7fdb446a7152f4e32912dc4b3f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <optional>
+#include <string>
+
+#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"
+#include "RobotAPI/libraries/skills/core/SkillID.h"
+#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
+
+#include "../FluxioSkillStatusUpdate.h"
+#include "../SkillExecutionID.h"
+#include "FluxioExecutor.h"
+
+namespace armarx::skills
+{
+ class FluxioNativeExecutor : public FluxioExecutor
+ {
+ public:
+ FluxioNativeExecutor(
+ std::string& id,
+ skills::SkillID& skillId,
+ std::string& fluxioUUID,
+ const std::function<bool(const skills::SkillExecutionID&)>&& abortSkillFunc,
+ const std::function<skills::SkillExecutionID(const skills::SkillExecutionRequest&)>&&
+ executeSkillAsyncFunc,
+ const std::function<std::optional<skills::SkillStatusUpdate>(
+ const skills::SkillExecutionID&)>&& getSkillExecutionStatusFunc);
+
+ void run(const std::string executorName, armarx::aron::data::DictPtr parameters) override;
+ void abort() override;
+ std::optional<std::vector<skills::FluxioSkillStatusUpdate>> getStatusUpdate() override;
+
+ private:
+ std::optional<skills::SkillExecutionID> executionId = std::nullopt;
+ skills::SkillID skillId;
+ std::string fluxioUUID;
+ void setStatus(skills::SkillStatus status) override;
+ const std::function<bool(const skills::SkillExecutionID&)> abortSkill;
+ const std::function<skills::SkillExecutionID(const skills::SkillExecutionRequest&)>
+ executeSkillAsync;
+ const std::function<std::optional<skills::SkillStatusUpdate>(
+ const skills::SkillExecutionID&)>
+ getSkillExecutionStatus;
+ };
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/manager/CMakeLists.txt b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
index 14400219e0d7a06468d9873eeeb387e47ef2b3e9..34494a91f8cc27d289aa8d28773c10250a4bbb4a 100644
--- a/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
@@ -18,15 +18,9 @@ armarx_add_library(
SOURCES
SkillManagerComponentPlugin.cpp
SkillManagerComponentPluginUser.cpp
- FluxioExecutor.cpp
- FluxioNativeExecutor.cpp
- FluxioCompositeExecutor.cpp
HEADERS
SkillManagerComponentPlugin.h
SkillManagerComponentPluginUser.h
- FluxioExecutor.h
- FluxioNativeExecutor.h
- FluxioCompositeExecutor.h
)
add_library(RobotAPI::skills::manager ALIAS RobotAPISkillsManager)
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.cpp b/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.cpp
deleted file mode 100644
index da6c89ae5b5180df10d3ffbb44b7ac6a779fe178..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.cpp
+++ /dev/null
@@ -1,470 +0,0 @@
-#include "FluxioCompositeExecutor.h"
-
-#include <algorithm>
-#include <thread>
-#include <vector>
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include "RobotAPI/libraries/skills/core/FluxioNode.h"
-#include "RobotAPI/libraries/skills/core/FluxioParameter.h"
-#include "RobotAPI/libraries/skills/core/FluxioParameterNode.h"
-#include "RobotAPI/libraries/skills/core/FluxioSkill.h"
-#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
-#include "RobotAPI/libraries/skills/core/FluxioSubSkillNode.h"
-#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
-#include "RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h"
-
-namespace armarx::plugins
-{
- FluxioCompositeExecutor::FluxioCompositeExecutor(std::string& id,
- SkillManagerComponentPlugin* plugin,
- skills::FluxioSkill* skill) :
- FluxioExecutor(id, plugin, false), skill(skill)
- {
- }
-
- void
- FluxioCompositeExecutor::run(std::string executorName, armarx::aron::data::DictPtr parameters)
- {
- // if (!validateSkill())
- // {
-
- // ARMARX_WARNING << "Skill execution cancelled.";
- // return;
- // }
-
- // std::string NewExecutorName = skill->name;
- subExecutionsMap.clear();
- setStatus(skills::SkillStatus::Constructing);
-
- // poll the status updates in a separate thread
- std::atomic_bool threadRunning = true;
- auto statusUpdateThread = std::thread(
- [this, &threadRunning]
- {
- while (threadRunning)
- {
- this->pollSubStatuses();
- std::this_thread::sleep_for(std::chrono::milliseconds(250));
- }
- });
- statusUpdateThread.detach();
-
- if (skill == nullptr)
- {
- ARMARX_WARNING << "Fluxioskill is not set";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- const auto& nodes = skill->nodes;
- if (nodes.empty())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no nodes";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& edges = skill->edges;
- if (edges.empty())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no edges";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& skillParams = skill->parameters;
- if (skillParams.empty())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no parameters";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& startParam =
- std::find_if(skillParams.begin(),
- skillParams.end(),
- [](const std::pair<std::string, skills::FluxioParameter>& param)
- { return (param.second.type == "Event" && param.second.isInput); });
- if (startParam == skillParams.end())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no start parameter";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& startNode =
- std::find_if(nodes.begin(),
- nodes.end(),
- [&startParam](const std::pair<std::string, skills::FluxioNode*>& node)
- {
- // check for parameter node
- if (node.second->nodeType != "PARAMETER")
- {
- return false;
- }
- // cast to parameter node
- const auto* paramNode =
- dynamic_cast<const skills::FluxioParameterNode*>(node.second);
- return (paramNode->parameterPtr->id == startParam->second.id);
- });
- if (startNode == nodes.end())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no start node";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& edgeWithStartNode =
- std::find_if(edges.begin(),
- edges.end(),
- [&startNode, &startParam](const skills::FluxioEdge& edge)
- {
- return (edge.fromNodePtr->nodeId == startNode->second->nodeId &&
- edge.fromParameterPtr->id == startParam->second.id);
- });
- if (edgeWithStartNode == edges.end())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no edge with start node";
- setStatus(skills::SkillStatus::Failed);
- return;
- }
-
- skills::FluxioNode* nextNode = edgeWithStartNode->toNodePtr;
- while (nextNode->nodeType == "SUBSKILL")
- {
- if (status->status == skills::SkillStatus::Aborted ||
- status->status == skills::SkillStatus::Failed ||
- status->status == skills::SkillStatus::Succeeded)
- {
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- // cast to subskill node
- const auto* subSkillNode = dynamic_cast<const skills::FluxioSubSkillNode*>(nextNode);
- if (subSkillNode == nullptr)
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no subskill node. Pointer null";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- auto* executorPtr =
- plugin->executeFluxioSkill(subSkillNode->skillPtr->id, executorName);
- if (executorPtr == nullptr)
- {
- ARMARX_WARNING << "Failed to execute subskill " << subSkillNode->skillPtr->id;
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread t
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- subExecutionsMap.insert({nextNode->nodeId, executorPtr});
- std::string eventParamName;
- setStatus(skills::SkillStatus::Running);
- bool running = true;
- while (running)
- {
- // sleep for a while
- std::this_thread::sleep_for(std::chrono::milliseconds(250));
- const auto& executionStatus = executorPtr->getStatus();
-
- // check if this composite skill has terminated or aborted
- if (status->status == skills::SkillStatus::Aborted ||
- status->status == skills::SkillStatus::Failed ||
- status->status == skills::SkillStatus::Succeeded)
- {
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- if (!executionStatus.has_value())
- {
- ARMARX_INFO << "No status update from skill " << subSkillNode->skillPtr->name
- << " yet. Waiting...";
- continue;
- }
-
- if (executionStatus->status == skills::SkillStatus::Succeeded)
- {
- eventParamName = "Succeeded";
- running = false;
- }
- else if (executionStatus->status == skills::SkillStatus::Failed)
- {
- eventParamName = "Failed";
- running = false;
- }
- else if (executionStatus->status == skills::SkillStatus::Aborted)
- {
- eventParamName = "Aborted";
- running = false;
- }
- else
- {
- ARMARX_INFO << "Skill " << subSkillNode->skillPtr->name
- << " is still running. Waiting...";
- }
- }
-
- const auto& eventParam = std::find_if(
- subSkillNode->skillPtr->parameters.begin(),
- subSkillNode->skillPtr->parameters.end(),
- [&eventParamName](const std::pair<std::string, skills::FluxioParameter>& param)
- { return (param.second.type == "Event" && param.second.name == eventParamName); });
- if (eventParam == skillParams.end())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no " << eventParamName
- << " parameter";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- const auto& eventNode = subSkillNode;
- const auto& edgeWithEventNode =
- std::find_if(edges.begin(),
- edges.end(),
- [&eventNode, &eventParam](const skills::FluxioEdge& edge)
- {
- return (edge.fromNodePtr->nodeId == eventNode->nodeId &&
- edge.fromParameterPtr->id == eventParam->second.id);
- });
- if (edgeWithEventNode == edges.end())
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no edge with " << eventParamName
- << " node";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- nextNode = edgeWithEventNode->toNodePtr;
- }
-
- if (nextNode->nodeType != "PARAMETER")
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no parameter node";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- // cast to parameter node
- const auto* paramNode = dynamic_cast<const skills::FluxioParameterNode*>(nextNode);
- if (paramNode == nullptr)
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no parameter node. Pointer null";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
- if (paramNode->parameterPtr->type != "Event")
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no valid event parameter";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
-
-
- if (paramNode->parameterPtr->name == "Succeeded")
- {
- setStatus(skills::SkillStatus::Succeeded);
- }
- else if (paramNode->parameterPtr->name == "Failed")
- {
- setStatus(skills::SkillStatus::Failed);
- }
- else if (paramNode->parameterPtr->name == "Aborted")
- {
- setStatus(skills::SkillStatus::Aborted);
- }
- else
- {
- ARMARX_WARNING << "Skill " << skill->name << " has no valid event parameter";
- setStatus(skills::SkillStatus::Failed);
-
- // kill thread
- threadRunning = false;
- statusUpdateThread.join();
- return;
- }
- }
-
- void
- FluxioCompositeExecutor::abort()
- {
- ARMARX_INFO << "Aborting skill " << skill->name;
- setStatus(skills::SkillStatus::Aborted);
-
- if (plugin == nullptr)
- {
- ARMARX_WARNING << "Plugin is not set, cannot abort subskills.";
- return;
- }
-
- // abort all subskills
- for (const auto& [nodeId, executorPtr] : subExecutionsMap)
- {
- auto s = executorPtr->getStatus();
- if (!s.has_value() || s->status == skills::SkillStatus::Succeeded ||
- s->status == skills::SkillStatus::Failed ||
- s->status == skills::SkillStatus::Aborted)
- {
- continue;
- }
-
- plugin->abortFluxioSkill(executorPtr->id);
- statusUpdates.push_front(
- {armarx::DateTime::Now(), executorPtr->id, nodeId, skills::SkillStatus::Aborted});
- }
- }
-
- std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
- FluxioCompositeExecutor::getStatusUpdate()
- {
- ARMARX_INFO << "Getting status updates for skill " << skill->name;
- // convert statusupdates list to vector
- auto ret = std::vector<skills::FluxioSkillStatusUpdate>(statusUpdates.begin(),
- statusUpdates.end());
-
- return ret;
- }
-
- void
- FluxioCompositeExecutor::pollSubStatuses()
- {
- for (const auto& [nodeId, executorPtr] : subExecutionsMap)
- {
- executorPtr->getStatusUpdate();
- auto s = executorPtr->getStatus();
- if (!s.has_value())
- {
- continue;
- }
-
- const auto& lastStatus =
- find_if(statusUpdates.begin(),
- statusUpdates.end(),
- [&](const skills::FluxioSkillStatusUpdate& statusUpdate)
- { return statusUpdate.subSkillNodeId == nodeId; })
- ->status;
- if (lastStatus != s->status)
- {
- statusUpdates.push_front(
- {armarx::DateTime::Now(), executorPtr->id, nodeId, s->status});
- }
- }
- }
-
- bool
- FluxioCompositeExecutor::validateSkill()
- {
- // check if skill is not null
- if (skill == nullptr)
- {
- ARMARX_WARNING << "Skill is not set";
- return false;
- }
-
- // get start parameter
- const auto& startParam =
- std::find_if(skill->parameters.begin(),
- skill->parameters.end(),
- [](const std::pair<std::string, skills::FluxioParameter>& param) {
- return (param.second.type == "Event" && param.second.isInput &&
- param.second.name == "Start");
- });
-
- if (startParam == skill->parameters.end())
- {
- ARMARX_WARNING << "Skill has no start parameter";
- return false;
- }
-
- // get all parameter nodes for the start parameter
- const auto& startNodes =
- std::find_if(skill->nodes.begin(),
- skill->nodes.end(),
- [startParam](const std::pair<std::string, skills::FluxioNode*>& nodeEntry)
- {
- if (nodeEntry.second->nodeType != "PARAMETER")
- {
- return false;
- }
-
- const auto* paramNode =
- dynamic_cast<const skills::FluxioParameterNode*>(nodeEntry.second);
- return (paramNode->parameterPtr->id == startParam->second.id);
- });
-
- // there can only be one
- if (startNodes == skill->nodes.end() || std::distance(skill->nodes.begin(), startNodes) > 1)
- {
- ARMARX_WARNING << "Skill has no or multiple start nodes";
- return false;
- }
-
- // TODO: the rest
-
- ARMARX_WARNING << "Skill validation is not implemented yet. Returning true.";
- return true;
- }
-
- void
- FluxioCompositeExecutor::setStatus(skills::SkillStatus status)
- {
- const skills::FluxioSkillStatusUpdate update = {
- armarx::DateTime::Now(), id, skill->id, status};
- this->status = update;
- this->statusUpdates.push_front(update);
- }
-} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.h b/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.h
deleted file mode 100644
index 77db7bd23d956c3249108fcef23e8f6da184cb35..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include <optional>
-#include <string>
-
-#include "RobotAPI/libraries/skills/core/FluxioSkill.h"
-#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
-#include "RobotAPI/libraries/skills/core/SkillExecutionID.h"
-#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
-
-#include "FluxioExecutor.h"
-#include "SkillManagerComponentPlugin.h"
-
-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();
-
- 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
- };
-} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.h b/source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.h
deleted file mode 100644
index 10668222cc4181d495d61ed24651857b2cbe0d9f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/manager/FluxioNativeExecutor.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <optional>
-#include <string>
-#include <functional>
-
-#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
-#include "RobotAPI/libraries/skills/core/SkillExecutionID.h"
-
-#include "FluxioExecutor.h"
-#include "SkillManagerComponentPlugin.h"
-
-namespace armarx::plugins
-{
- class SkillManagerComponentPlugin; // forward declaration
-
- class FluxioNativeExecutor : public FluxioExecutor
- {
- public:
- FluxioNativeExecutor(std::string& id,
- SkillManagerComponentPlugin* plugin,
- skills::SkillID& skillId);
-
- void run(std::string executorName, armarx::aron::data::DictPtr parameters) override;
- void abort() override;
- std::optional<std::vector<skills::FluxioSkillStatusUpdate>> getStatusUpdate() override;
-
- private:
- std::optional<skills::SkillExecutionID> executionId = std::nullopt;
- skills::SkillID skillId;
- void setStatus(skills::SkillStatus status) override;
-
- //std::function<void(skills::SkillID&)> foo;
- };
-} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 818ba16068494a8e889cfe4aa0b7626587f72de4..aeb8fca7e17b42aae45b67f8f50c98b1921a4d63 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,12 +1,20 @@
#include "SkillManagerComponentPlugin.h"
-#include <cstddef>
#include <list>
#include <optional>
+#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
+#include <boost/locale.hpp>
+#include <boost/uuid/name_generator.hpp>
+#include <boost/uuid/nil_generator.hpp>
+#include <boost/uuid/string_generator.hpp>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
#include <IceUtil/UUID.h>
#include <ArmarXCore/core/Component.h>
@@ -21,15 +29,14 @@
#include "RobotAPI/libraries/skills/core/FluxioSkill.h"
#include "RobotAPI/libraries/skills/core/FluxioValue.h"
#include "RobotAPI/libraries/skills/core/error/FluxioException.h"
-#include "RobotAPI/libraries/skills/manager/FluxioCompositeExecutor.h"
-#include "RobotAPI/libraries/skills/manager/FluxioExecutor.h"
+#include "RobotAPI/libraries/skills/core/executor/FluxioCompositeExecutor.h"
+#include "RobotAPI/libraries/skills/core/executor/FluxioExecutor.h"
+#include "RobotAPI/libraries/skills/core/executor/FluxioNativeExecutor.h"
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
-#include "FluxioNativeExecutor.h"
-
namespace armarx::plugins
{
void
@@ -543,7 +550,7 @@ namespace armarx::plugins
auto statusUpdate = skills::SkillStatusUpdate::FromIce(
provider_statusUpdate_ice.value(), providerId);
- ARMARX_WARNING << "Status update: " << statusUpdate.result;
+ // ARMARX_WARNING << "Status update: " << statusUpdate.result;
return statusUpdate;
}
catch (...)
@@ -612,8 +619,9 @@ namespace armarx::plugins
//** Fluxio related methods **//
//****************************//
- FluxioExecutor*
- SkillManagerComponentPlugin::executeFluxioSkill(std::string skillId, std::string executorName)
+ skills::FluxioExecutor*
+ SkillManagerComponentPlugin::executeFluxioSkill(const std::string& skillId,
+ const std::string& executorName)
{
const auto& result = getSkill(skillId);
if (!result.isSuccess())
@@ -630,8 +638,18 @@ namespace armarx::plugins
// currently empty parameters
armarx::aron::data::DictPtr emptyParameters = {};
+ const auto& executeFluxioSkillFunc =
+ [this](const std::string& skillId, const std::string& executorName)
+ { return this->executeFluxioSkill(skillId, executorName); };
+
+ const auto& abortFluxioSkill = [this](const std::string& executionId)
+ { this->abortFluxioSkill(executionId); };
+
fluxioDC.fluxioExecutors[executionId] =
- new FluxioCompositeExecutor(executionId, this, &fluxioDC.skills[skillId]);
+ new skills::FluxioCompositeExecutor(executionId,
+ &fluxioDC.skills[skillId],
+ std::move(abortFluxioSkill),
+ std::move(executeFluxioSkillFunc));
std::thread(
[this, executionId, executorName, emptyParameters]()
@@ -641,8 +659,8 @@ namespace armarx::plugins
else
{
skills::SkillID sID = skills::SkillID{
- .providerId = skills::ProviderID{.providerName = skill.skillProviderPtr->id},
- .skillName = skill.id};
+ .providerId = skills::ProviderID{.providerName = skill->skillProviderPtr->name},
+ .skillName = skill->name};
auto skillDescr = getSkillDescription(sID);
if (!skillDescr.has_value())
@@ -652,9 +670,26 @@ namespace armarx::plugins
return nullptr;
}
+ const auto& abortSkillFunc = [this](const skills::SkillExecutionID& executionId)
+ { return this->abortSkill(executionId); };
+
+ const auto& executeSkillAsyncFunc = [this](const skills::SkillExecutionRequest& req)
+ { return this->executeSkillAsync(req); };
+
+ const auto& getSkillExecutionStatusFunc =
+ [this](const skills::SkillExecutionID& executionId)
+ { return this->getSkillExecutionStatus(executionId); };
+
+
// FIXME: do not use new, instead use smart pointer
+ std::string fluxioSkillUUID = skill->id;
fluxioDC.fluxioExecutors[executionId] =
- new FluxioNativeExecutor(executionId, this, sID);
+ new skills::FluxioNativeExecutor(executionId,
+ sID,
+ fluxioSkillUUID,
+ std::move(abortSkillFunc),
+ std::move(executeSkillAsyncFunc),
+ std::move(getSkillExecutionStatusFunc));
fluxioDC.fluxioExecutors[executionId]->run(executorName,
skillDescr->rootProfileDefaults);
}
@@ -672,13 +707,13 @@ namespace armarx::plugins
return;
}
- FluxioExecutor* executorPtr = executorIt->second;
+ skills::FluxioExecutor* executorPtr = executorIt->second;
if (executorPtr->native)
{
- FluxioNativeExecutor* castedExecutor = nullptr;
+ skills::FluxioNativeExecutor* castedExecutor = nullptr;
try
{
- castedExecutor = dynamic_cast<FluxioNativeExecutor*>(executorPtr);
+ castedExecutor = dynamic_cast<skills::FluxioNativeExecutor*>(executorPtr);
if (castedExecutor == nullptr)
{
ARMARX_WARNING << "Executor with id '" << executionId
@@ -697,10 +732,10 @@ namespace armarx::plugins
}
else
{
- FluxioCompositeExecutor* castedExecutor = nullptr;
+ skills::FluxioCompositeExecutor* castedExecutor = nullptr;
try
{
- castedExecutor = dynamic_cast<FluxioCompositeExecutor*>(executorPtr);
+ castedExecutor = dynamic_cast<skills::FluxioCompositeExecutor*>(executorPtr);
if (castedExecutor == nullptr)
{
ARMARX_WARNING << "Executor with id '" << executionId
@@ -726,51 +761,58 @@ namespace armarx::plugins
if (executorIt == fluxioDC.fluxioExecutors.end())
{
ARMARX_WARNING << "Execution with id '" << executionId << "' not found.";
- auto error = skills::error::FluxioException("Execution with id" + executionId + " not found.");
+ auto error =
+ skills::error::FluxioException("Execution with id" + executionId + " not found.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
- return error;
+ return error;
}
- FluxioExecutor* executorPtr = executorIt->second;
+ skills::FluxioExecutor* executorPtr = executorIt->second;
if (executorPtr->native)
{
- FluxioNativeExecutor* castedExecutor = nullptr;
+ skills::FluxioNativeExecutor* castedExecutor = nullptr;
try
{
- castedExecutor = dynamic_cast<FluxioNativeExecutor*>(executorPtr);
+ castedExecutor = dynamic_cast<skills::FluxioNativeExecutor*>(executorPtr);
if (castedExecutor == nullptr)
{
ARMARX_WARNING << "Executor with id '" << executionId
<< "' is not a FluxioNativeExecutor.";
- auto error = skills::error::FluxioException("Executor with id" + executionId + "is not a FluxioNativeExecutor");
- error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
- return error;
+ auto error = skills::error::FluxioException("Executor with id" + executionId +
+ "is not a FluxioNativeExecutor");
+ error.addToContext(
+ std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
+ return error;
}
}
catch (const std::bad_cast& e)
{
ARMARX_WARNING << "Error while getting execution runner for fluxio skill with id "
<< executionId << ": " << e.what();
- auto error = skills::error::FluxioException("Error while getting execution runner for fluxio skill with id " + executionId + ": " + e.what());
- error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
+ auto error = skills::error::FluxioException(
+ "Error while getting execution runner for fluxio skill with id " + executionId +
+ ": " + e.what());
+ error.addToContext(
+ std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
}
-
+
return {(castedExecutor->getStatusUpdate()).value()};
}
- FluxioCompositeExecutor* castedExecutor = nullptr;
+ skills::FluxioCompositeExecutor* castedExecutor = nullptr;
try
{
- castedExecutor = dynamic_cast<FluxioCompositeExecutor*>(executorPtr);
+ castedExecutor = dynamic_cast<skills::FluxioCompositeExecutor*>(executorPtr);
if (castedExecutor == nullptr)
{
ARMARX_WARNING << "Executor with id '" << executionId
<< "' is not a FluxioCompositeExecutor.";
- auto error = skills::error::FluxioException("Executor with ID: " + executionId + " is not a FluxioCompositeExecutor.");
- error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
+ auto error = skills::error::FluxioException("Executor with ID: " + executionId +
+ " is not a FluxioCompositeExecutor.");
+ error.addToContext(
+ std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
-
}
}
catch (const std::bad_cast& e)
@@ -778,10 +820,11 @@ namespace armarx::plugins
ARMARX_WARNING << "Error while getting execution runner for fluxio skill with id "
<< executionId << ": " << e.what();
- auto error = skills::error::FluxioException("Error while getting execution runner for fluxio skill with executionID: " + executionId + ":" + e.what());
+ auto error = skills::error::FluxioException(
+ "Error while getting execution runner for fluxio skill with executionID: " +
+ executionId + ":" + e.what());
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
-
}
@@ -792,7 +835,7 @@ namespace armarx::plugins
* @brief converts parametersType or resultType from skilldescription to fluxio parameters
* @param ret the map to insert the fluxio parameters into
* @param ptr the aron object ptr to convert (this is either skillDescription.parametersType or skillDescription.resultType)
- * @param isInput whether the parameters are input parameters or output parameters
+ * @param isInput value for the isInput field of the fluxio parameter(s)
*/
void
SkillManagerComponentPlugin::convertAronObjectPtrToFluxioParameters(
@@ -816,6 +859,29 @@ namespace armarx::plugins
}
}
+ boost::uuids::uuid
+ SkillManagerComponentPlugin::createUuidWithString(
+ const std::string& str,
+ std::optional<const boost::uuids::uuid> namespaceUUID)
+ {
+ boost::uuids::uuid nameS;
+ if (namespaceUUID.has_value())
+ {
+ nameS = namespaceUUID.value();
+ }
+ else
+ {
+ // use default null uuid
+ nameS = boost::uuids::nil_uuid();
+ }
+
+ // initialise the name generator with the starting UUID
+ boost::uuids::name_generator generator(nameS);
+
+ // generate a new UUID, seeded with the given string
+ return generator(str);
+ }
+
skills::Result<std::vector<skills::FluxioSkill>, skills::error::FluxioException>
SkillManagerComponentPlugin::getSkillList()
{
@@ -824,8 +890,11 @@ namespace armarx::plugins
for (const auto& [skillId, skillDescription] : skillDescriptions)
{
- auto skillsEntry = fluxioDC.skills.find(skillId.skillName);
+ // generate skill id (idempotent)
+ const auto& providerId = createUuidWithString(skillId.providerId->providerName);
+ const auto& id = createUuidWithString(skillId.skillName, providerId);
+ auto skillsEntry = fluxioDC.skills.find(boost::uuids::to_string(id));
if (skillsEntry != fluxioDC.skills.end())
{
continue;
@@ -833,67 +902,73 @@ namespace armarx::plugins
skills::FluxioSkill s;
- s.id = skillId.skillName;
- s.name = s.id;
+ s.id = boost::uuids::to_string(id);
+ s.name = skillId.skillName;
s.description = skillDescription.description;
s.lastChanged = "";
s.executable = false;
s.native = true;
- auto providersEntry = fluxioDC.providers.find(skillId.providerId->providerName);
-
+ const auto& providersEntry =
+ fluxioDC.providers.find(boost::uuids::to_string(providerId));
if (providersEntry != fluxioDC.providers.end())
{
s.skillProviderPtr = &(providersEntry->second);
}
else
{
- skills::FluxioProvider p;
- p.id = skillId.providerId->providerName;
- p.name = p.id;
-
- fluxioDC.providers[p.id] = p;
- s.skillProviderPtr = &fluxioDC.providers[p.id];
+ const auto& p = addFluxioProvider(skillId.providerId->providerName);
+ if (p == nullptr)
+ {
+ ARMARX_WARNING << "Could not add provider '" << skillId.providerId->providerName
+ << "'.";
+ continue;
+ }
+ s.skillProviderPtr = &fluxioDC.providers[p->id];
}
s.parameters = {};
// add default control flow parameters
skills::FluxioParameter start;
- start.id = IceUtil::generateUUID();
start.name = "Start";
- start.description = "Start the skill";
start.type = "Event";
+ const auto& startIdStr = start.name + start.type + "isInput";
+ start.id = boost::uuids::to_string(createUuidWithString(startIdStr, id));
+ start.description = "Start the skill";
start.required = true;
start.isInput = true;
- skills::FluxioParameter success;
- success.id = IceUtil::generateUUID();
- success.name = "Succeeded";
- success.description = "Skill has been executed successfully";
- success.type = "Event";
- success.required = false;
- success.isInput = false;
-
- skills::FluxioParameter failure;
- failure.id = IceUtil::generateUUID();
- failure.name = "Failed";
- failure.description = "Skill has failed";
- failure.type = "Event";
- failure.required = false;
- failure.isInput = false;
+ skills::FluxioParameter succeeded;
+ succeeded.name = "Succeeded";
+ succeeded.type = "Event";
+ const auto& successIdStr = succeeded.name + succeeded.type + "isOutput";
+ succeeded.id = boost::uuids::to_string(createUuidWithString(successIdStr, id));
+ succeeded.description = "Skill has been executed successfully";
+ succeeded.required = false;
+ succeeded.isInput = false;
+
+ skills::FluxioParameter failed;
+ failed.name = "Failed";
+ failed.type = "Event";
+ const auto& failedIdStr = failed.name + failed.type + "isOutput";
+ failed.id = boost::uuids::to_string(createUuidWithString(failedIdStr, id));
+ failed.description = "Skill has failed";
+ failed.required = false;
+ failed.isInput = false;
skills::FluxioParameter aborted;
- aborted.id = IceUtil::generateUUID();
aborted.name = "Aborted";
- aborted.description = "Skill has been aborted";
aborted.type = "Event";
+ const auto& abortedIdStr = aborted.name + aborted.type + "isOutput";
+ aborted.id = boost::uuids::to_string(createUuidWithString(abortedIdStr, id));
+ aborted.description = "Skill has been aborted";
aborted.required = false;
aborted.isInput = false;
s.parameters[start.id] = start;
- s.parameters[success.id] = success;
- s.parameters[failure.id] = failure;
+ s.parameters[succeeded.id] = succeeded;
+ s.parameters[failed.id] = failed;
s.parameters[aborted.id] = aborted;
if (skillDescription.parametersType != nullptr)
@@ -938,7 +1013,6 @@ namespace armarx::plugins
return {convertMapValuesToVector(fluxioDC.skills)};
}
-
skills::Result<skills::FluxioSkill, skills::error::FluxioException>
SkillManagerComponentPlugin::getSkill(const std::string& id)
{
@@ -947,10 +1021,10 @@ namespace armarx::plugins
{
return {skillsEntry->second};
}
- auto error = skills::error::FluxioException("Skill with ID: " + id + " could not be found.");
+ auto error =
+ skills::error::FluxioException("Skill with ID: " + id + " could not be found.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
-
}
void
@@ -997,7 +1071,8 @@ namespace armarx::plugins
return {profilesEntry->second};
}
- auto error = skills::error::FluxioException("Profile with ID: " + id + " could not be found.");
+ auto error =
+ skills::error::FluxioException("Profile with ID: " + id + " could not be found.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
}
@@ -1022,26 +1097,46 @@ namespace armarx::plugins
fluxioDC.profiles[profile.id].name = profile.name;
fluxioDC.profiles[profile.id].description = profile.description;
fluxioDC.profiles[profile.id].parentPtr = profile.parentPtr;
- } else {
- //TODO: Do we throw the error here?
+ }
+ else
+ {
+ //TODO: Do we throw the error here?
}
}
+ skills::FluxioProvider*
+ SkillManagerComponentPlugin::addFluxioProvider(const std::string& name)
+ {
+ const std::string& providerId = boost::uuids::to_string(createUuidWithString(name));
+
+ if (fluxioDC.providers.find(providerId) != fluxioDC.providers.end())
+ {
+ ARMARX_WARNING << "Provider with name '" << name << "' already exists.";
+ return nullptr;
+ }
+
+ skills::FluxioProvider p;
+ p.id = providerId;
+ p.name = name;
+
+ fluxioDC.providers[p.id] = p;
+ return &fluxioDC.providers[p.id];
+ }
+
skills::Result<std::vector<skills::FluxioProvider>, skills::error::FluxioException>
SkillManagerComponentPlugin::getProviderList()
{
- for (const auto& [providerName, providerPrx] : skillProviderMap)
+ for (const auto& [providerID, providerPrx] : skillProviderMap)
{
- const auto& providersEntry = fluxioDC.providers.find(providerName.providerName);
+ const auto& id = boost::uuids::to_string(createUuidWithString(providerID.providerName));
+ const auto& providersEntry = fluxioDC.providers.find(id);
if (providersEntry != fluxioDC.providers.end())
{
continue; // provider already exists
}
- //TODO: use real uuids for the provider id
- fluxioDC.providers[providerName.providerName] = {providerName.providerName,
- providerName.providerName};
+ addFluxioProvider(providerID.providerName);
}
return {convertMapValuesToVector(fluxioDC.providers)};
@@ -1058,7 +1153,8 @@ namespace armarx::plugins
return {providersEntry->second};
}
- auto error = skills::error::FluxioException("Provider with ID: " + id + " could not be found.");
+ auto error =
+ skills::error::FluxioException("Provider with ID: " + id + " could not be found.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
}
@@ -1071,7 +1167,8 @@ namespace armarx::plugins
if (providersEntry == fluxioDC.providers.end())
{
- auto e = skills::error::FluxioException("Skillprovider with id: " + id + " could not be found.");
+ auto e = skills::error::FluxioException("Skillprovider with id: " + id +
+ " could not be found.");
e.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return e;
}
@@ -1090,71 +1187,91 @@ namespace armarx::plugins
return {ret};
}
-
skills::Result<skills::FluxioSkill, skills::error::FluxioException>
SkillManagerComponentPlugin::addSkillToProvider(const std::string& userId,
const std::string& providerId,
skills::FluxioSkill& skill)
{
+ // check if the provider exists
+ const auto& providerIt = fluxioDC.providers.find(providerId);
+ if (providerIt == fluxioDC.providers.end())
+ {
+ auto error = skills::error::FluxioException("FluxioException");
+ error.addToContext("Provider with ID: " + providerId + " could not be found.",
+ "SkillManagerComponentPlugin",
+ __FUNCTION__,
+ __LINE__);
+ return error;
+ }
+
+ if (skill.id == "")
+ {
+ // its a new skill, its a new world, ...
+ skill.id = IceUtil::generateUUID();
+ }
+
+ boost::uuids::uuid uuid;
+ try
+ {
+ boost::uuids::string_generator gen;
+ uuid = gen(skill.id);
+ }
+ catch (std::runtime_error& e)
+ {
+ ARMARX_WARNING << "Could not convert skill id to uuid: " << e.what();
+ skill.id = IceUtil::generateUUID(); // fallback
+ }
+
// add default control flow parameters
skills::FluxioParameter start;
- start.id = IceUtil::generateUUID();
start.name = "Start";
+ start.id = boost::uuids::to_string(createUuidWithString(start.name, uuid));
start.description = "Start the skill";
start.type = "Event";
start.required = true;
start.isInput = true;
- skills::FluxioParameter success;
- success.id = IceUtil::generateUUID();
- success.name = "Succeeded";
- success.description = "Skill has been executed successfully";
- success.type = "Event";
- success.required = false;
- success.isInput = false;
-
- skills::FluxioParameter failure;
- failure.id = IceUtil::generateUUID();
- failure.name = "Failed";
- failure.description = "Skill has failed";
- failure.type = "Event";
- failure.required = false;
- failure.isInput = false;
+ skills::FluxioParameter succeeded;
+ succeeded.name = "Succeeded";
+ succeeded.id = boost::uuids::to_string(createUuidWithString(succeeded.name, uuid));
+ succeeded.description = "Skill has been executed successfully";
+ succeeded.type = "Event";
+ succeeded.required = false;
+ succeeded.isInput = false;
+
+ skills::FluxioParameter failed;
+ failed.name = "Failed";
+ failed.id = boost::uuids::to_string(createUuidWithString(failed.name, uuid));
+ failed.description = "Skill has failed";
+ failed.type = "Event";
+ failed.required = false;
+ failed.isInput = false;
skills::FluxioParameter aborted;
- aborted.id = IceUtil::generateUUID();
aborted.name = "Aborted";
+ aborted.id = boost::uuids::to_string(createUuidWithString(aborted.name, uuid));
aborted.description = "Skill has been aborted";
aborted.type = "Event";
aborted.required = false;
aborted.isInput = false;
skill.parameters[start.id] = start;
- skill.parameters[success.id] = success;
- skill.parameters[failure.id] = failure;
+ skill.parameters[succeeded.id] = succeeded;
+ skill.parameters[failed.id] = failed;
skill.parameters[aborted.id] = aborted;
- // check if the provider exists
- const auto& providerIt = fluxioDC.providers.find(providerId);
- if (providerIt == fluxioDC.providers.end())
- {
- auto error = skills::error::FluxioException("FluxioException");
- error.addToContext("Provider with ID: " + providerId + " could not be found.", "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
- return error;
- }
- // set id, lastChanged and store in data collector
- std::string id = IceUtil::generateUUID();
- fluxioDC.skills[id] = skill;
- fluxioDC.skills[id].id = id;
- fluxioDC.skills[id].lastChanged = armarx::DateTime::Now().toDateTimeString();
+ skill.lastChanged = armarx::DateTime::Now().toDateTimeString();
+
+ // store in data collector
+ fluxioDC.skills[skill.id] = skill;
// set mutex
- setEditFluxioSkillMutex(true, userId, id);
+ setEditFluxioSkillMutex(true, userId, skill.id);
return {fluxioDC.skills[id]};
}
- skills::Result<bool,skills::error::FluxioException>
+ skills::Result<bool, skills::error::FluxioException>
SkillManagerComponentPlugin::setEditFluxioSkillMutex(bool aquireMutex,
const std::string& userId,
const std::string& skillId)
@@ -1190,7 +1307,8 @@ namespace armarx::plugins
}
// mutex could not be aquired
- auto error = skills::error::FluxioException("Mutex for skill with ID: " + skillId + " could not be aquired.");
+ auto error = skills::error::FluxioException("Mutex for skill with ID: " + skillId +
+ " could not be aquired.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
}
@@ -1207,11 +1325,13 @@ namespace armarx::plugins
if (std::get<0>(fluxioDC.skillMutexMap[skillId]) == userId)
{
fluxioDC.skillMutexMap.erase(skillId);
- return {true};
+ return {true};
}
// mutex could not be removed
- auto error = skills::error::FluxioException("Mutex for skill with ID: " + skillId + " could not be removed because the user does not hold the Mutex.");
+ auto error = skills::error::FluxioException(
+ "Mutex for skill with ID: " + skillId +
+ " could not be removed because the user does not hold the Mutex.");
error.addToContext(std::nullopt, "SkillManagerComponentPlugin", __FUNCTION__, __LINE__);
return error;
}
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index d27b6099ae44c214852c3030ff365e35dbda9a4d..c4151d91153e819ecde36206e9946d06f324d9a3 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -1,8 +1,11 @@
#pragma once
#include <mutex>
+#include <optional>
#include <string>
+#include <boost/uuid/uuid.hpp>
+
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/time/DateTime.h>
@@ -16,15 +19,15 @@
#include <RobotAPI/libraries/skills/core/FluxioParameter.h>
#include <RobotAPI/libraries/skills/core/FluxioProfile.h>
#include <RobotAPI/libraries/skills/core/FluxioProvider.h>
+#include <RobotAPI/libraries/skills/core/FluxioResult.h>
#include <RobotAPI/libraries/skills/core/FluxioSkill.h>
#include <RobotAPI/libraries/skills/core/FluxioValue.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
-#include <RobotAPI/libraries/skills/core/FluxioResult.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/core/executor/FluxioExecutor.h>
-#include "FluxioExecutor.h"
#include "SkillManagerComponentPluginUser.h"
namespace armarx
@@ -34,8 +37,6 @@ namespace armarx
namespace armarx::plugins
{
- class FluxioExecutor; // forward declaration
-
class SkillManagerComponentPlugin : public ComponentPlugin
{
public:
@@ -77,21 +78,24 @@ namespace armarx::plugins
//** Fluxio related methods **//
//****************************//
- FluxioExecutor* executeFluxioSkill(std::string skillId,
- std::string executorName = "Fluxio");
+ skills::FluxioExecutor* executeFluxioSkill(const std::string& skillId,
+ const std::string& executorName);
void abortFluxioSkill(const std::string& executionId);
skills::Result<std::vector<skills::FluxioSkillStatusUpdate>, skills::error::FluxioException>
getFluxioSkillExecutionStatus(const std::string& executionId);
- skills::Result<std::vector<skills::FluxioSkill>, skills::error::FluxioException> getSkillList();
+ skills::Result<std::vector<skills::FluxioSkill>, skills::error::FluxioException>
+ getSkillList();
- skills::Result<skills::FluxioSkill, skills::error::FluxioException>getSkill(const std::string& id);
+ skills::Result<skills::FluxioSkill, skills::error::FluxioException>
+ getSkill(const std::string& id);
void removeSkill(const std::string& id);
- skills::Result<bool, skills::error::FluxioException> getSkillMutex(const std::string& skillId, const std::string& userId);
+ skills::Result<bool, skills::error::FluxioException>
+ getSkillMutex(const std::string& skillId, const std::string& userId);
void deleteSkillMutex(const std::string& skillId, const std::string& userId);
@@ -99,33 +103,42 @@ namespace armarx::plugins
const std::string& skillId,
const std::string& parameterId);
- skills::Result<std::vector<skills::FluxioProfile>, skills::error::FluxioException> getProfileList();
+ skills::Result<std::vector<skills::FluxioProfile>, skills::error::FluxioException>
+ getProfileList();
- skills::Result<skills::FluxioProfile, skills::error::FluxioException> getProfile(const std::string& id);
+ skills::Result<skills::FluxioProfile, skills::error::FluxioException>
+ getProfile(const std::string& id);
- skills::Result<skills::FluxioProfile, skills::error::FluxioException> createProfile(const skills::FluxioProfile& profile);
+ skills::Result<skills::FluxioProfile, skills::error::FluxioException>
+ createProfile(const skills::FluxioProfile& profile);
void updateProfile(const skills::FluxioProfile& profile);
- skills::Result<std::vector<skills::FluxioProvider>, skills::error::FluxioException> getProviderList();
+ skills::Result<std::vector<skills::FluxioProvider>, skills::error::FluxioException>
+ getProviderList();
+
+ skills::Result<std::optional<skills::FluxioProvider>, skills::error::FluxioException>
+ getProvider(const std::string& id);
- skills::Result<std::optional<skills::FluxioProvider>, skills::error::FluxioException> getProvider(const std::string& id);
+ skills::FluxioProvider* addFluxioProvider(const std::string& name);
- skills::Result<std::vector<skills::FluxioSkill>, skills::error::FluxioException> getSkillsOfProvider(const std::string& id);
+ skills::Result<std::vector<skills::FluxioSkill>, skills::error::FluxioException>
+ getSkillsOfProvider(const std::string& id);
- skills::Result<skills::FluxioSkill, skills::error::FluxioException> addSkillToProvider(const std::string& userId,
- const std::string& providerId,
- skills::FluxioSkill& skill);
+ skills::Result<skills::FluxioSkill, skills::error::FluxioException>
+ addSkillToProvider(const std::string& userId,
+ const std::string& providerId,
+ skills::FluxioSkill& skill);
private:
struct FluxioDataCollector
{
- std::map<std::string, skills::FluxioSkill> skills = {};
- std::map<std::string, skills::FluxioProfile> profiles = {};
- std::map<std::string, skills::FluxioProvider> providers = {};
- std::map<std::string, std::tuple<std::string, armarx::DateTime>> skillMutexMap = {};
+ std::map<std::string, skills::FluxioSkill> skills;
+ std::map<std::string, skills::FluxioProfile> profiles;
+ std::map<std::string, skills::FluxioProvider> providers;
+ std::map<std::string, std::tuple<std::string, armarx::DateTime>> skillMutexMap;
const std::int64_t mutexTimeout = 30; // minutes
- std::map<std::string, FluxioExecutor*> fluxioExecutors = {};
+ std::map<std::string, skills::FluxioExecutor*> fluxioExecutors;
};
FluxioDataCollector fluxioDC = {};
@@ -139,6 +152,10 @@ namespace armarx::plugins
template <typename S, typename T>
std::vector<T> convertMapValuesToVector(std::map<S, T>& map);
+ static boost::uuids::uuid
+ createUuidWithString(const std::string& str,
+ std::optional<const boost::uuids::uuid> namespaceUUID = std::nullopt);
+
static void
convertAronObjectPtrToFluxioParameters(std::map<std::string, skills::FluxioParameter>& ret,
const aron::type::ObjectPtr& ptr,
@@ -148,9 +165,10 @@ namespace armarx::plugins
void replaceVectorContent(std::vector<T>& originalVector,
const std::vector<T>& replacementVector);
- skills::Result<bool, skills::error::FluxioException> setEditFluxioSkillMutex(bool aquireMutex,
- const std::string& userId,
- const std::string& skillId);
+ skills::Result<bool, skills::error::FluxioException>
+ setEditFluxioSkillMutex(bool aquireMutex,
+ const std::string& userId,
+ const std::string& skillId);
friend class armarx::SkillManagerComponentPluginUser;
};
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.cpp
index 2f291e8ff07f699f8ae9a8df0f749de04f6e6278..51118f707180c8c37e518aab1b6c658d8d816022 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPluginUser.cpp
@@ -169,7 +169,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;
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
index 8eaa3633f7248dcb7c78b91c16874ccc7811a407..45925621cbb9871c9a49786e3b9b8ce124742251 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.cpp
@@ -101,13 +101,16 @@ namespace armarx::skills::gui
this->skillDetailsTreeWidget = new SkillDetailsTreeWidget(this->memory, this);
this->skillDescriptionWidget = new SkillDescriptionWidget(this);
this->layout = new QVBoxLayout();
+ this->splitter = new QSplitter();
+ splitter->setOrientation(Qt::Orientation::Vertical);
// Layouting
this->setLayout(layout);
layout->addWidget(this->profileMenuWidget);
- layout->addWidget(this->skillDescriptionWidget);
- layout->addWidget(this->skillDetailsTreeWidget);
+ layout->addWidget(splitter);
+ splitter->addWidget(this->skillDescriptionWidget);
+ splitter->addWidget(this->skillDetailsTreeWidget);
layout->addWidget(this->executeSkillButton);
// Text
diff --git a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
index 1723870bc6fb5a0c32c24efad41d17c9baf0fb58..3d2a56b14c7c6cc224e7daa18d5df5d3375e2b62 100644
--- a/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
+++ b/source/RobotAPI/libraries/skills_gui/skill_details/SkillDetailsGroupBox.h
@@ -3,6 +3,7 @@
#include <QGroupBox>
#include <QPushButton>
+#include <QSplitter>
#include <QVBoxLayout>
#include "RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetController.h"
@@ -50,6 +51,7 @@ namespace armarx::skills::gui
void connectSignals();
QVBoxLayout* layout = nullptr;
+ QSplitter* splitter = nullptr;
QPushButton* executeSkillButton = nullptr;
SkillDetailsTreeWidget* skillDetailsTreeWidget = nullptr;
SkillDescriptionWidget* skillDescriptionWidget = nullptr;
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
index 30b26364853dd4d96fb159847edb575b372ae833..d57753f5ce0ca37536296237ca5d29e1d733f8de 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillGroupBox.cpp
@@ -78,6 +78,7 @@ namespace armarx::skills::gui
connect(
this->acceptSearchButton, &QPushButton::clicked, this, &SkillGroupBox::filterAndFetch);
connect(this->searchBar, &QLineEdit::editingFinished, this, &SkillGroupBox::handleSearch);
+ connect(this->searchBar, &QLineEdit::textChanged, this, &SkillGroupBox::handleSearch);
connect(this,
&SkillGroupBox::searchRequest,
this->memory.get(),